I am currently working on a project for school. The idea was to create a robot that could perform pathfinding functions using A*. The path is found using a virtual grid of nodes programmed into the Arduino where "X"s are non-passable objects, “0” as the start, and “1” as the destination.
Example:
char createPlane[][planeSizeX] = {{'.', '.', '.', '.', '.', 'X', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
{'.', 'X', '.', 'X', '.', 'X', '.', 'X', 'X', 'X', 'X', 'X', '.', '.', 'X', '.'},
{'.', 'X', '.', '.', '.', '.', '.', 'X', '.', '.', '.', '.', '.', '.', 'X', '.'},
{'.', '.', 'X', '.', 'X', 'X', 'X', 'X', '.', 'X', '.', '.', 'X', '.', '0', '.'},
{'.', 'X', '.', '.', 'X', '.', '.', '.', '.', '.', '.', '.', '1', 'X', 'X', '.'},
{'.', '.', '.', '.', '.', '.', 'X', '.', 'X', '.', 'X', '.', '.', '.', 'X', '.'},
{'.', 'X', 'X', '.', '.', 'X', 'X', '.', 'X', '.', 'X', 'X', 'X', '.', 'X', '.'},
{'.', '.', '.', '.', '.', '.', '.', '.', 'X', '.', '.', '.', '.', '.', '.', '.'}};
The instructions for the path above would be: left, up, left, left, down, down, right.
Current parts:
Dagu Rover 5 tracked robot (no encoders, 2 motors)
Arduino Mega 2560
Arduino Motor Shield
HMC5883L magnetometer compass
My pathfinding application works great and I know I am building the correct instructions for the robot to follow. At this point, I have already spent weeks just trying to get this thing to turn 90 degrees consistently. It is vital for my robot to turn as accurately as possible in order to arrive at the correct destination.
I bought the HMC5883L compass hoping that it would allow me to turn 90 degrees. I have gotten to the point where it will turn until the correct heading is reached (according to the compass), but it is not consistent at all. Sometimes too much, sometimes not enough.
Currently, I am taking an average of the X and Y readings along with the adjustment values for X and Y that I calculated using this blog. I believe I have correct declination for my area as well.
float getAvg(float array[])
{
float average = 0;
for (int i = 0; i < 5; i++)
average += array[i];
average /= 5;
return average;
}
float getHeading(HMC5883L &compass)
{
float declinationAngle = 12.183;
float xScaled[5];
float yScaled[5];
MagnetometerScaled scaled;
for (int i = 0; i < 5; i++)
{
scaled = compass.ReadScaledAxis();
xScaled[i] = (scaled.XAxis - xAdj);
yScaled[i] = (scaled.YAxis - yAdj);
}
float xFinal = getAvg(xScaled);
float yFinal = getAvg(yScaled);
float heading = atan2(yFinal, xFinal);
heading += declinationAngle / 1000;
if(heading < 0)
heading += 2*PI;
if(heading > 2*PI)
heading -= 2*PI;
heading *= 180/M_PI;
return heading;
}
Here is the messy code I am currently using to turn:
void drive(Stack <short> thePath)
{
short instruction;
short temp;
analogWrite(pwmA, 255);
analogWrite(pwmB, 255);
digitalWrite(brakeA, 0);
digitalWrite(brakeB, 0);
int err = 0;
HMC5883L compass;
Wire.begin();
compass = HMC5883L();
err = compass.SetScale(1.3);
if(err != 0) //check for error
Serial.println(compass.GetErrorText(err));
err = compass.SetMeasurementMode(Measurement_Continuous);
if(err != 0) //check for error
Serial.println(compass.GetErrorText(err));
instruction = thePath.pop();
while (1)
{
//checks if the robot needs to turn
if (instruction != thePath.peek())
{
float heading = -1;
float currentHeading = -1;
heading = getHeading(compass);
currentHeading = heading;
temp = instruction - thePath.peek();
if (temp == -1 || temp == 3)
{
int currentHeadCorrect = 0;
heading += 90;
if(heading > 360)
heading -= 360;
if (currentHeading >= 270)
currentHeadCorrect = 360;
while ((currentHeading - currentHeadCorrect) < heading)
{
currentHeading = getHeading(compass);
if ((currentHeading - currentHeadCorrect) <= -300)
currentHeadCorrect = 0;
digitalWrite(directionA, bwd); //turn right
digitalWrite(directionB, fwd);
}
}
else if (temp == 1 || temp == -3)
{
int currentHeadCorrect = 0;
heading -= 90;
if(heading <= 0)
heading += 360;
if (currentHeading < 90)
currentHeadCorrect = 360;
while ((currentHeading + currentHeadCorrect) > heading) //if (temp == -1 || temp == 3)
{
currentHeading = getHeading(compass);
if ((currentHeading + currentHeadCorrect) > 700)
currentHeadCorrect = 0;
digitalWrite(directionA, fwd); //turn left
digitalWrite(directionB, bwd);
}
}
else
error();
}
//go straight
analogWrite(pwmA, 255);
analogWrite(pwmB, 255);
digitalWrite(directionA, bwd);
digitalWrite(directionB, bwd);
delay(930);
instruction = thePath.pop();
}
}
Even with all of this, I am still not getting the precision I need for turning. Please let me know of any ideas any of you have that would allow me to have the precision in turning that I am looking for.
