Hi ChuckCrunch,
Zero and 360 deg crossing in movement make it difficult to use a compass for course /heading stabilization. This is why you’ll not see many (if any) references on google. However, it’s not impossible as the video illistrates.
Keep in mind, this is still a work in progress, that’s why I’ve not posted complete code. I would also recommend setting this up on a test Arduino will a compass (double sided taped) so you can debug with the Serial Monitor. If you use this in a project, please acknowledge this as my code and if you modify it, please share it with me.
My approach was to sample heading whenever I change state to take a heading (setpoint), then as the robot moves repeatedly read it’s updated heading and take the absolute difference to setpoint. This error is multipled by some factor to create a correction signal. I then figure out if the difference in heading is above or below the setpoint and apply this to the motor left and right speed respectively.
So …basically… this should work…
the first routine is just to sample heading.
long getheading()
{
// sample heading angle
hmc6352.wake();
sensVal = hmc6352.getHeading();
hmc6352.sleep();
return sensVal;
}
Next while running (polling);
void setheading()
/-----------------------------------------------------
SetHeading Routine
by Jim Winburn
Appiphania.com
9/2/2011
/------------------------------------------------------
{
Input = getheading(); // I sample heading again
c = 180 - abs(180- abs(Setpoint -Input)); //take the absolute difference
d = c*10; // apply multiplier to achive desired correction response (you can replace this with a proper PID algorithm if needed)
e = (maxspeed-d); // apply correction
if (e<= 0) //if correction < 0 (negative), make it zero
{
e=0;
}
if (Setpoint - Input > 180) //test if the difference is above or below the setopint, compensating for zero and 360 crossing
{
Input +=360;
}
else if (Setpoint - Input < -180)
{
Input -=360;
}
b = Setpoint - Input;
if (b < 0) // = negative … moving to the right … correct respective motor speed based on state
{
if (anfwd == 1) //forward
{
leftspeed = e;
rightspeed = maxspeed;
}
else if (anrev == 1) //reverse
{
leftspeed = maxspeed;
rightspeed = e;
}
}
else if (b > 0) //= positive … moving to the left … correct respective motor speed based on state
{
if (anfwd == 1)
{
leftspeed = maxspeed;
rightspeed = e;
}
else if (anrev == 1)
{
leftspeed = e;
rightspeed = maxspeed;
}
}
}
