Fully Autonomous Golf Caddy

Forum 2005-2010 (read only) General Exhibition
25

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]