Hi,
I'm building this project where I have to move a car in a straight path for a certain distance at regular intervals. The issue I'm facing is that that car is not moving in a straight path. This is the code I have:
void Compass_Forward()
{
while (blueToothVal == 9) // Go forward until Bluetooth 'Stop' command is sent
{
getCompass(); // Update Compass Heading
bluetooth(); // Check to see if any Bluetooth commands have been sent
if (blueToothVal == 5) {break;} // If a Stop Bluetooth command ('5') is received then break from the Loop
if ( abs(desired_heading - compass_heading) <= compass_dev ) // If the Desired Heading and the Compass Heading are within the compass deviation, X degrees of each other then Go Forward
// otherwise find the shortest turn radius and turn left or right
{
Forward();
} else
{
int x = (desired_heading - 359); // x = the GPS desired heading - 360
int y = (compass_heading - (x)); // y = the Compass heading - x
int z = (y - 360); // z = y - 360
if ((z <= 180) && (z >= 0)) // if z is less than 180 and not a negative value then turn left
{ // otherwise turn right
SlowLeftTurn();
}
else
{
SlowRightTurn();
}
}
} // End While Loop
} // End Compass_Forward
and these are the functions that I'm calling
void getCompass() // get latest compass value
{
Vector norm = compass.readNormalize();
// Calculate heading
float heading = atan2(norm.YAxis, norm.XAxis);
if(heading < 0)
heading += 2 * M_PI;
compass_heading = (int)(heading * 180/M_PI); // assign compass calculation to variable (compass_heading) and convert to integer to remove decimal places
}
void bluetooth()
{
while (Serial1.available()) // Read bluetooth commands over Serial1 // Warning: If an error with Serial1 occurs, make sure Arduino Mega 2560 is Selected
{
{
str = Serial1.readStringUntil('\n'); // str is the temporary variable for storing the last sring sent over bluetooth from Android device
//Serial.print(str); // for testing purposes
}
blueToothVal = (str.toInt()); // convert the string 'str' into an integer and assign it to blueToothVal
Serial.print("BlueTooth Value ");
Serial.println(blueToothVal);
// **************************************************************************************************************************************************
switch (blueToothVal)
{
case 9:
Serial1.println("Compass Forward");
setHeading();
Compass_Forward();
break;
}
}
}
// end of switch case
void SlowLeftTurn()
{
motor1.setSpeed(175);
motor2.setSpeed(175);
motor3.setSpeed(175);
motor4.setSpeed(175);
motor1.run(BACKWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(BACKWARD);
}
void SlowRightTurn()
{
motor1.setSpeed(175);
motor2.setSpeed(175);
motor3.setSpeed(175);
motor4.setSpeed(175);
motor1.run(FORWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor4.run(FORWARD);
and my compass deviation is 5
