it seems very jumpy, I just incorporated an exponentially weighted moving average for the compass heading and hopefully that will stabilize it a bit. Playing with it in house it seems much more stable, although it does lag behind about 200ms instead of 50ms now, which is less than desirable but it might be necessary for fluid motion.
With the jumpiness we can't get very accurate readings or smooth motion. I have lots of videos of the movement as is but it's kind of crappy.
it's 30 degrees out now so i'm going to continue testing some other time.
#include <String.h>
#include <ctype.h>
#include <Wire.h>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[9];
char CurrentYascii[10];
float CurrentX;
float CurrentY;
float CurrentHeading;
float CurrentHeading1;
float CurrentHeading2;
float CurrentHeading3;
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 LeftSpeed=0;
int RightSpeed=0;
double ParkingLotX[3] = {4031.24554, 4031.24116, 4031.23294};
double ParkingLotY[3] = {-7427.58914,-7427.57996,-7427.5853};
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);
}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);
}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* = 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]));*
- CurrentHeading3 = heading/10;*
- CurrentHeading2=(CurrentHeading3+CurrentHeading2)/2;*
- CurrentHeading1=(CurrentHeading2+CurrentHeading1)/2;*
- CurrentHeading=(CurrentHeading+CurrentHeading1)/2;*
*} *
void readGPS()
{
- GPStimer=0;*
- do{*
_ byteGPS=Serial2.read();_- if (byteGPS == -1)*
- {*
- delay(100);*
- } *
- else {*
- linea[conta]=byteGPS;*
- conta++;*
- if (byteGPS==13){*
- digitalWrite(ledPin, LOW);*
- cont=0;*
- bien=0;*
- for (int i=1;i<7;i++){*
_ if (linea*==comandoGPR[i-1]){_
_ bien++;_
_ }_
_ }_
_ if(bien==6){_
_ for (int i=0;i<300;i++){_
_ if (linea==','){
indices[cont]=i;
cont++;
}
if (linea==''){
* indices[12]=i;
cont++;
}
}
Serial.println("");
Serial.println("");
Serial.println("---------------");
for (int i=0;i<12;i++){
for (int j=indices,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;
}
}
}
CurrentX=atof(CurrentXascii);
CurrentY=-atof(CurrentYascii);
}
conta=0;
for (int i=0;i<300;i++){
linea=' ';
}
}
}
GPStimer++;
}
while(GPStimer<300);
}
[/quote]*_
void setup()
{
delay(500);
Wire.begin();
Wire.send(0x82);
delay(500);
Serial.begin(9600);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
Serial2.begin(4800);
for (int i=0;i<300;i++){
linea*=' ';*
- } *
_ Serial3.begin(9600);_- delay(2000);*
- setEngineSpeed1(0);*
- setEngineSpeed2(0);*
- Instance=0;*
*} *
void loop()
{- buttonState = digitalRead(buttonPin);*
- if (buttonState != lastButtonState) {*
- if (buttonState == HIGH) {*
- buttonPushCounter++;*
- }*
- lastButtonState = buttonState;*
- }*
- Instance=buttonPushCounter;*
- readSensor();*
_ Serial.print("Heading: ");_
_ Serial.print(CurrentHeading);_
_ Serial.print("\t Tilt: ");_
_ Serial.print(tilt / 10, DEC);_
_ Serial.print("\t Roll: ");_
_ Serial.println(roll / 10, DEC);_- delay(50);*
- readGPS();*
_ Serial.print("Current X Value: ");_
_ Serial.print(CurrentX, 10);_
_ Serial.print("\t"); _
_ Serial.print("Current Y Value: ");_
_ Serial.println(CurrentY, 10);_
_ Serial.println("---------------");_- do{*
- readGPS();*
- DeltaX=ParkingLotX[Instance]-CurrentX;*
- DeltaY=ParkingLotY[Instance]-CurrentY;*
- if (DeltaY<0){*
- if (DeltaX<0){*
_ DesiredHeading=270-atan(DeltaY/DeltaX)*180/3.14159265;_- }else if (DeltaX>0){*
_ DesiredHeading=180-atan(DeltaY/DeltaX)*180/3.14159265;_- }*
- }else if (DeltaY>0){*
- if (DeltaX<0){*
_ DesiredHeading=360-atan(DeltaY/DeltaX)*180/3.14159265;_- }else if (DeltaX>0){*
_ DesiredHeading=90-atan(DeltaY/DeltaX)*180/3.14159265;_- }*
- }*
- readSensor();*
- if (((DesiredHeading-CurrentHeading) < 15)&&((DesiredHeading-CurrentHeading)>6)){*
- LeftSpeed=40;*
- RightSpeed=30;*
- }else if (((DesiredHeading-CurrentHeading) <= 30)&&((DesiredHeading-CurrentHeading)>15)){*
- LeftSpeed=40;*
- RightSpeed=20;*
- }else if (((DesiredHeading-CurrentHeading) <= 60)&&((DesiredHeading-CurrentHeading)>30)){*
- LeftSpeed=40;*
- RightSpeed=10;*
- }else if (((DesiredHeading-CurrentHeading) <= 360)&&((DesiredHeading-CurrentHeading)>60)){*
- LeftSpeed=40;*
- RightSpeed=-40;*
- }else if (((DesiredHeading-CurrentHeading) <= -6)&&((DesiredHeading-CurrentHeading)>-15)){*
- LeftSpeed=30;*
- RightSpeed=40;*
- }else if (((DesiredHeading-CurrentHeading) <= -15)&&((DesiredHeading-CurrentHeading)>-30)){*
- LeftSpeed=20;*
- RightSpeed=40;*
- }else if (((DesiredHeading-CurrentHeading) <= -30)&&((DesiredHeading-CurrentHeading)>-60)){*
- LeftSpeed=10;*
- RightSpeed=40;*
- }else if (((DesiredHeading-CurrentHeading) <= -60)&&((DesiredHeading-CurrentHeading) >- 360)){*
- LeftSpeed=-40;*
- RightSpeed=40;*
- }else if (((DesiredHeading-CurrentHeading) <= 6)&&((DesiredHeading-CurrentHeading) >= -6)){*
- LeftSpeed=40;*
- RightSpeed=40;*
- }*
- setEngineSpeed1(LeftSpeed);*
- setEngineSpeed2(RightSpeed);*
_ Serial.print("LeftSpeed: ");_
_ Serial.print(LeftSpeed);_
_ Serial.print("\t");_
_ Serial.print("RightSpeed:");_
_ Serial.print(RightSpeed);_
_ Serial.print("\t");_
_ Serial.print("DesiredHeading:");_
_ Serial.print(DesiredHeading);_
_ Serial.print("\t");_
_ Serial.print("CurrentHeading:");_
_ Serial.print(CurrentHeading);_
_ Serial.print("\t");_
_ Serial.print("CurrentX:");_
_ Serial.print(CurrentX, 4);_
_ Serial.print("\t");_
_ Serial.print("CurrentY:");_
_ Serial.print(CurrentY, 4);_
_ Serial.print("\t");_
_ Serial.print("Y:");_
_ Serial.print(ParkingLotY[Instance], 4);_
_ Serial.print("\t");_
_ Serial.print("X:");_
_ Serial.print(ParkingLotX[Instance], 4);_
_ Serial.print("\t");_
_ Serial.print("Instance:");_
_ Serial.println(Instance);_- delay(50);*
- }while((abs(DeltaX)>0.0001)&&(abs(DeltaY)>0.0001));*
- setEngineSpeed1(0);*
- setEngineSpeed2(0);*
}
[/quote]
in case anyone's wondering about the strange coordinates, this is how to convert it:
GPS outputs in XXYY.ZZZZ where XX is degrees YY is minutes and ZZZZ is fractional minutes
google maps works XX.FFFFFFFFFF where F is simply fractional degrees
so this coordinate: 40.520576, -74.45974
take the decimals, .520576, .45974 and multiply by 60
31.23456, 27.5844
append to the degrees
4031.23456, -7427.5844
and that's how the GPS speaks. It's kind of annoying that the gps doesn't just output as fractional degrees but whatever, we've gotten used to the converting back and forth now and it's not so bad.
added in a exponential moving average smoothing for the motor control too, smoothen out those jumps a little, I lessened the smoothing for the compass because the lag was too much.
With my 8 weeks experience of programming, can I ask - are you really holding the golf bag up with one frictional gripping pivot at the bottom ?? 
For the time being, we're not doing any actual testing with it as i'm sure you've realized that shit will not hold up if I breathe on it hard enough.
We're working on implementing a magnet system to click grip it and a strap that goes around the stem to the golf bag's grip, we may incorporate a small "basket" for it on the bottom, we've already designed and built the basket but it looks ugly so we're looking at alternative methods.
Good eye!
Just kidding, I'm so impressed with the scale of the project, well done
I though about doing this a long time ago but you have a very good start. You know there is a lot easier way of doing this. No gps needed. You just want the bag to stay within a certain distance of the golfer while walking, say 4 ft behind you. You just need a small signal transmitter that could be on a key chain, Then something attached to the arduino that looks at the signal strength to determine distance. Then you just need 2 buttons go and stop so when you actually go to the tee it is not trying to follow you there. That way it works anywhere not just on 1 golf course and if you shank a ball your bag isn't 30ft away from you. Just need a way to identify you from your others in your party. Could even use infrared leds to follow.
Maybe this helps
That actually already exists and it's undesirable because it still requires attention. Senior Design is about pushing the industry, not trying to simply make something cheaper.
Shadow Caddy, Bat Caddy, and Follow-Me Caddy were our literature examples.
this will ideally drive by itself completely. So far it's getting close but it still needs a bit of tuning and optimizing.
what you're talking about was designed with active RFIDs, it works pretty well.
Essentially you have a "stop" and "follow" button on a belt clip and it does exactly that.
Not very well priced though, most of the solutions out there cost quite a bit (although somewhat insignificant when compared to the cost of golfing)
Are you planning on doing a full write up when the project is done? A video with working caddy while under GPS Nav would be nice too. Keep up the good work!!! 
yeah, there's going to be a full report and presentation. Not sure if the presentation is going to be video taped but I can put the full report up here when it's done and there are already lots of videos and there will be many more.
check my youtube channel (bleedscarlet) for latest videos, when we come up with good solid tests the videos will get posted here but you can see our progress there.
We are GPS navigating! just not very well nor very fast...
with some assistance in the troubleshooting section of the forum we figured out a way to get the accuracy we need. We drop the degrees and just use the minutes and fractional minutes instead 
since a degree is >42 miles and the golf course is in the middle of a degree we don't need to bother with that complication for now.
In a full scale deployment we would have to put an if statement around the whole thing to add some kind of directional correction if say you needed to move from 74.0001 to 73.9989 or something, but we're not concerning ourselves with that mostly because i'm burning out from staying in the lab until 2AM every night.
Some asshole decided texting and driving is cool.
We almost got run over.
Needless to say the physical damage isn't too bad, but every component is broken. Our demonstration was supposed to be at 1PM on friday.....this happened at 4PM on thursday...
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';
}
// CurrentX1=atol(CurrentXascii);
// CurrentY1=-atol(CurrentYascii);
// CurrentX=floor(CurrentX1/100000)+(CurrentX1%10000)/10000;
// CurrentY=floor(CurrentY1/100000)+(CurrentY1%10000)/10000;
// CurrentXoff=(atof(CurrentXascii));
// CurrentYoff=(atof(CurrentYascii));
CurrentX=(atof(CurrentXascii))-0.00324; //+OffsetX;
CurrentY=(atof(CurrentYascii))+0.01864; //+OffsetY;
}
conta=0;
for (int i=0;i<300;i++)
{
linea[i]=' ';
}
}
}
}
}
/*******************************************************************
Void Setup
*******************************************************************/
void setup()
{
delay(500);
Wire.begin();
Wire.send(0x82);
delay(500);
//Telemetry:
Serial.begin(9600);
pinMode(13, OUTPUT);
pinMode(MoveSwitchPin, INPUT);
digitalWrite(13, HIGH);
//GPS Module:
Serial2.begin(4800);
for (int i=0;i<300;i++)
{
linea[i]=' ';
}
//Motor Controller:
Serial3.begin(9600);
delay(2000);
setEngineSpeed1(0);
setEngineSpeed2(0);
Instance=0;
MoveSwitch=digitalRead(MoveSwitchPin);
//Cell Phone:
Serial1.begin(19200); // the GPRS baud rate
// switchModule(); // swith the module ON
for (int i=0;i<2;i++)
{
delay(5000);
}
Serial1.println("AT+CMGF=1"); // set the SMS mode to text
delay(100);
}
/*******************************************************************
Void Loop
*******************************************************************/
void loop()
{
// buttonState = digitalRead(buttonPin);
// if (buttonState != lastButtonState) {
// if (buttonState == HIGH) {
// buttonPushCounter++;
// }
// lastButtonState = buttonState;
// }
// Instance=buttonPushCounter;
MoveSwitch=digitalRead(MoveSwitchPin);
if (Calibration==0)
{
readGPS();
if (CalibrationCounter<10)
{
delay(10);
CalibrationCounter++;
Serial.println("Calibrating...");
}
else if (CalibrationCounter>=10)
{
// OffsetX=31.2523-CurrentXoff;
// OffsetY=27.6102-CurrentYoff;
Calibration=1;
Serial.println("Calibration Complete.");
}
}
else if (Calibration==1)
{
if (MoveSwitch == LOW)
{
if (Instance<3)
{
readSensor();
// Serial.print("Heading: ");
// Serial.print(CurrentHeading);
// Serial.print("\t Tilt: ");
// Serial.print(tilt / 10, DEC);
// Serial.print("\t Roll: ");
// Serial.println(roll / 10, DEC);
delay(5);
readGPS();
// Serial.print("Current X Value: ");
// Serial.print(CurrentX, 10);
// Serial.print("\t");
// Serial.print("Current Y Value: ");
// Serial.println(CurrentY, 10);
// Serial.println("---------------");
/*******************************************************************
Figure Out Delta X & Delta Y
*******************************************************************/
DeltaX=ParkingLotX[Instance]-CurrentX;
DeltaY=ParkingLotY[Instance]-CurrentY;
if((abs(DeltaX)>0.002)||(abs(DeltaY)>0.002))
{
readGPS();
/*******************************************************************
Calculate Desired Heading
*******************************************************************/
if (DeltaX<0) //Must Go South
{
if (DeltaY>0) // Must Go South East (Reversed from normal quadrant because of hemisphere flip)
{
DesiredHeading=270+atan(DeltaX/DeltaY)*180/3.14159265;
}
else if (DeltaY<0) // Must Go South West (Reversed from normal quadrant because of hemisphere flip)
{
DesiredHeading=90+atan(DeltaX/DeltaY)*180/3.14159265;
}
}
else if (DeltaX>0) //Must Go North
{
if (DeltaY>0) // Must Go North East (Reversed from normal quadrant because of hemisphere flip)
{
DesiredHeading=270+atan(DeltaX/DeltaY)*180/3.14159265;
}
else if (DeltaY<0) // Must Go North West (Reversed from normal quadrant because of hemisphere flip)
{
DesiredHeading=90+atan(DeltaX/DeltaY)*180/3.14159265;
}
}
// if (DeltaY<0)
// {
// if (DeltaX>0)
// {
// DesiredHeading=270-atan(DeltaY/DeltaX)*180/3.14159265;
// }
// else if (DeltaX<0)
// {
// DesiredHeading=90+atan(DeltaY/DeltaX)*180/3.14159265;
// }
// }
// else if (DeltaY>0)
// {
// if (DeltaX>0)
// {
// DesiredHeading=270+atan(DeltaY/DeltaX)*180/3.14159265;
// }
// else if (DeltaX<0)
// {
// DesiredHeading=90-atan(DeltaY/DeltaX)*180/3.14159265;
// }
// }
/*******************************************************************
Pull Current Heading & Auto Correct
*******************************************************************/
readSensor();
if (((DesiredHeading-CurrentHeading) < 15)&&((DesiredHeading-CurrentHeading)>6))
{
LeftSpeed=40;
RightSpeed=30;
}
else if (((DesiredHeading-CurrentHeading) <= 30)&&((DesiredHeading-CurrentHeading)>15))
{
LeftSpeed=40;
RightSpeed=20;
}
else if (((DesiredHeading-CurrentHeading) <= 60)&&((DesiredHeading-CurrentHeading)>30))
{
LeftSpeed=40;
RightSpeed=10;
}
else if (((DesiredHeading-CurrentHeading) <= 180)&&((DesiredHeading-CurrentHeading)>60))
{
LeftSpeed=40;
RightSpeed=1;
}
else if (((DesiredHeading-CurrentHeading) <= 360)&&((DesiredHeading-CurrentHeading)>180))
{
LeftSpeed=-1;
RightSpeed=39;
}
else if (((DesiredHeading-CurrentHeading) <= -6)&&((DesiredHeading-CurrentHeading)>-15))
{
LeftSpeed=30;
RightSpeed=39;
}
else if (((DesiredHeading-CurrentHeading) <= -15)&&((DesiredHeading-CurrentHeading)>-30))
{
LeftSpeed=20;
RightSpeed=39;
}
else if (((DesiredHeading-CurrentHeading) <= -30)&&((DesiredHeading-CurrentHeading)>-60))
{
LeftSpeed=10;
RightSpeed=39;
}
else if (((DesiredHeading-CurrentHeading) <= -60)&&((DesiredHeading-CurrentHeading) > -180))
{
LeftSpeed=-1;
RightSpeed=39;
}
else if (((DesiredHeading-CurrentHeading) <= -180)&&((DesiredHeading-CurrentHeading) >= -360))
{
LeftSpeed=39;
RightSpeed=-1;
}
else if (((DesiredHeading-CurrentHeading) <= 6)&&((DesiredHeading-CurrentHeading) >= -6))
{
LeftSpeed=40;
RightSpeed=39;
}
/*******************************************************************
Exponential Moving Average Motor Control & Send Motor Speeds
*******************************************************************/
LeftSpeed0=.9*LeftSpeed+.1*LeftSpeed0;
RightSpeed0=.9*RightSpeed+.1*RightSpeed0;
setEngineSpeed1(LeftSpeed0);
setEngineSpeed2(RightSpeed0);
/*******************************************************************
Serial Prints for data verification (to be removed with final implementation)
*******************************************************************/
Serial.print("LeftSpeed: ");
Serial.print(LeftSpeed0);
Serial.print("\t");
Serial.print("RightSpeed: ");
Serial.print(RightSpeed0);
Serial.print("\t");
Serial.print("DesiredHeading: ");
Serial.print(DesiredHeading);
Serial.print("\t");
Serial.print("CurrentHeading: ");
Serial.print(CurrentHeading);
Serial.print("\t");
Serial.print("Calibrated? ");
Serial.print(Calibration);
Serial.print("\t");
Serial.print("Offset X: ");
Serial.print(OffsetX,6);
Serial.print("\t");
Serial.print("Offset Y: ");
Serial.println(OffsetY,6);
Serial.print("CurrentXascii: ");
Serial.print(CurrentXascii);
Serial.print("\t");
Serial.print("CurrentYascii: ");
Serial.print(CurrentYascii);
Serial.print("\t");
Serial.print("CurrentX: ");
Serial.print(CurrentX, 4);
Serial.print("\t");
Serial.print("CurrentY: ");
Serial.print(CurrentY, 4);
Serial.print("\t");
Serial.print("X: ");
Serial.print(ParkingLotX[Instance],6);
Serial.print("\t");
Serial.print("Y: ");
Serial.print(ParkingLotY[Instance],6);
Serial.print("\t");
Serial.print("Instance: ");
Serial.println(Instance);
/*******************************************************************
End Telemetry
*******************************************************************/
}
else if((abs(DeltaX)<=0.002)&&(abs(DeltaY)<=0.002))
{
Instance++;
setEngineSpeed1(0);
setEngineSpeed2(0);
delay(5000);
}
}
else
{
/*******************************************************************
Send Text Message Indicating Arrival
*******************************************************************/
while (count < timesToSend)
{
delay(1500);
Serial1.print("AT+CMGS="); // send the SMS the number
Serial1.print(0x22,BYTE); // send the " char
Serial1.print("17322337949"); // send the number (change ********* by the actual number)
Serial1.println(0x22,BYTE); // send the " char
delay(1500);
Serial1.println("Hey, I'm here!"); // the SMS body
delay(500);
Serial1.println(0x1A,BYTE); // end of message command 1A (hex)
delay(5000);
count++;
}
}
/*******************************************************************
Issue full stop for 5 seconds and start next instance
*******************************************************************/
MoveSwitch=digitalRead(MoveSwitchPin);
}
/*******************************************************************
If initial assistance switch is depressed, ignore sensors & go 100%.
*******************************************************************/
else if(MoveSwitch == HIGH)
{
setEngineSpeed1(100);
setEngineSpeed2(97);
Serial.print("Moving...");
delay(200);
MoveSwitch=digitalRead(MoveSwitchPin);
}
}