system
40
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);
}
}