system
39
// 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;
}
}