Here was our last program framework.
/*******************************************************************
Avinash Arora
Carl Barreau
Stephen Dudek
Asa Roper
Senior Design - Industrial and Systems Engineering - 2010
Motor Controller uses Serial 3
GPS uses Serial 2
Serial 1 cell phone
Compass uses SCL & SLA, SCL=green, SLA=blue
*******************************************************************/
/*******************************************************************
Libraries
*******************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <String.h>
#include <ctype.h>
#include <math.h>
#include <Wire.h>
/*******************************************************************
Initialize Variables
*******************************************************************/
int onModulePin = 2; // the pin to switch on the module (without press on button)
int timesToSend = 1; // Numbers of SMS to send
int count = 0;
int compassAddress = 0x32 >> 1;
int heading = 0;
int tilt = 0;
int roll = 0;
int ledPin = 13;
int rxPin = 0;
int txPin = 1;
int byteGPS=-1;
char linea[300] = "";
char comandoGPR[7] = "$GPRMC";
int cont=0;
int bien=0;
int conta=0;
int indices[13];
char CurrentXascii[14];
char CurrentYascii[14];
int CurrentXsingle;
int CurrentYsingle;
float CurrentX;
float CurrentY;
long CurrentX1;
long CurrentY1;
int CurrentX2;
int CurrentY2;
double CurrentXi;
double CurrentYi;
long CurrentXoff;
long CurrentYoff;
double CurrentHeading;
double CurrentHeading0;
byte responseBytes[6];
int GPStimer=0;
double DeltaX=0.000;
double DeltaY=0.000;
int Instance=0;
int InstanceSize=0;
int buttonState=0;
int buttonPin = 22;
int lastButtonState=0;
int buttonPushCounter=0;
float DesiredHeading=0.0;
int CompassComplete=0;
int LeftSpeed=0;
int RightSpeed=0;
int LeftSpeed0=0;
int RightSpeed0=0;
int MoveSwitchPin=34;
int MoveSwitch=0;
int Calibration=0;
int CalibrationCounter=0;
double ParkingLotX[3] = {31.2462,31.2475,31.2523};
double ParkingLotY[3] = {27.6146,27.5992,27.6102};
long OffsetX=0.0000;
long OffsetY=0.0000;
/*******************************************************************
Set Engine Speed 1
*******************************************************************/
void setEngineSpeed1(signed char cNewMotorSpeedH)
{
unsigned char cSpeedVal_Motor1 = 0;
if(cNewMotorSpeedH==0)
{
Serial3.print(0,BYTE);
return;
}
cSpeedVal_Motor1 = map(cNewMotorSpeedH,-100,100,1,127);
Serial3.print(cSpeedVal_Motor1, BYTE);
}
/*******************************************************************
Set Engine Speed 2
*******************************************************************/
void setEngineSpeed2(signed char cNewMotorSpeed2)
{
unsigned char cSpeedVal_Motor2 = 0;
if(cNewMotorSpeed2 == 0)
{
Serial3.print(0,BYTE);
return;
}
cSpeedVal_Motor2 = map(cNewMotorSpeed2,-100,100,128,255);
Serial3.print(cSpeedVal_Motor2, BYTE);
}
/*******************************************************************
Switch Cell Phone Module On
*******************************************************************/
void switchModule()
{
digitalWrite(onModulePin,HIGH);
delay(2000);
digitalWrite(onModulePin,LOW);
delay(10000);
}
/*******************************************************************
Read Sensor (Compass)
*******************************************************************/
void readSensor()
{
Wire.beginTransmission(compassAddress);
Wire.send(0x50);
Wire.endTransmission();
delay(2);
Wire.requestFrom(compassAddress, 6);
if(6 <= Wire.available())
{
for(int i = 0; i<6; i++)
{
responseBytes[i] = Wire.receive();
}
}
heading = ((int)responseBytes[0]<<8) | ((int)responseBytes[1]);
tilt = (((int)responseBytes[2]<<8) | ((int)responseBytes[3]));
roll = (((int)responseBytes[4]<<8) | ((int)responseBytes[5]));
CurrentHeading = heading/10;
}
/*******************************************************************
Read Sensor (GPS)
*******************************************************************/
void readGPS()
{
for(GPStimer=0;GPStimer<250;GPStimer++)
{
byteGPS=Serial2.read();
if (byteGPS == -1)
{
delay(10);
}
else
{
linea[conta]=byteGPS;
conta++;
if (byteGPS==13)
{
digitalWrite(ledPin, LOW);
cont=0;
bien=0;
for (int i=1;i<7;i++)
{
if (linea[i]==comandoGPR[i-1])
{
bien++;
}
}
if(bien==6)
{
for (int i=0;i<300;i++)
{
if (linea[i]==',')
{
indices[cont]=i;
cont++;
}
if (linea[i]=='*')
{
indices[12]=i;
cont++;
}
}
// Serial.println("");
// Serial.println("");
Serial.println("---------------");
for (int i=0;i<12;i++)
{
for (int j=indices[i],k=0;j<(indices[i+1]-1);j++,k++)
{
switch(i)
{
case 2 :
CurrentXascii[k]=linea[j+1];
break;
case 4 :
CurrentYascii[k]=linea[j+1];
break;
}
}
}
// int xlength=sizeof(CurrentXascii);
// int ylength=sizeof(CurrentYascii);
// for (int i=0;i<xlength;i++){
// CurrentX1[i]=(int)CurrentXascii[i];
// }
// for (int i=0;i<ylength;i++){
// CurrentY[i]=(int)CurrentXascii[i];
// }
// CurrentX=atof(CurrentXascii)/10000;
// CurrentY=-atof(CurrentYascii)/10000;
/* for (int i=0,k=5;i<5;i++,k--){
CurrentX1=CurrentX1+(10^k)*((int)CurrentXascii[i]);
Serial.println(CurrentX1);
}
for (int i=6,k=8;i<13;i++,k--){
CurrentX2=CurrentX2+(10^k)*((int)CurrentXascii[i]);
Serial.println(CurrentX2);
}
for (int i=1,k=5;i<6;i++,k--){
CurrentY1=CurrentY1+(10^k)*((int)CurrentYascii[i]);
Serial.println(CurrentY1);
}
for (int i=7,k=7;i<13;i++,k--){
CurrentY2=CurrentY2+(10^k)*((int)CurrentYascii[i]);
Serial.println(CurrentY2);
}
CurrentXi=CurrentX1+(CurrentX2/10000);
CurrentYi=-(CurrentY1+(CurrentY2/10000));
CurrentX=CurrentXi;
CurrentY=CurrentYi;
CurrentX1,CurrentX2,CurrentY1,CurrentY2,CurrentXi,CurrentYi=0;
String CurrentXc=CurrentXascii.replace(".",""); */
//remove degrees to allow for greater dimensional accuracy
for (int i=0,j=9;i<2;i++)
{
CurrentXascii[i]='0';
}
for (int i=0;i<3;i++)
{
CurrentYascii[i]='0';
}
//clear extraneous digits
for (int i=9;i<13;i++)
{
CurrentXascii[i]='0';
}
for (int i=10;i<13;i++)
{
CurrentYascii[i]='0';
}