Fully Autonomous Golf Caddy

Forum 2005-2010 (read only) General Exhibition
24

#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]*_