Compass math assistance required

Hello All

Mission Statement

When the 'bot detects an obstical it stops and scans left and right for a clear path, preserving the angle on the servo for the best path.
This angle is then added to (subtracted from) the current 'bot heading to get the desired heading.

Purpose is then to turn towards the desired heading until the robot heading is ‘equivalent’ (say within 5 degrees) to the desired heading.

We know which direction to turn based on the servo angle, but detecting the angle to turn based on current and desired headings would be better as it would allow for a more modular approach.


I am strugling to get my head round the math when the desired heading is close to 360 degrees…

Can anyone give me pointers to deal with the 360/0 rollover.


If the desiredAngle < 360 then desiredAngle = desiredAngle+360
Then desiredAngle = desiredAngle MODULO 360


Since there is a delay between each compass reading, and the fact that the compass reading fluctuates by 3-4 degrees when standing still I cannot simply turn towards the heading until I reach it as I may overshoot…

Any thoughts or pointers to compass math would be appreciated.

The servo angle won't be anywhere near 360 could easily get the servo angle relative to "forward", so that you are scanning 90 degrees left of right of straight ahead. That tells you how many degrees to add to/subtract from your current heading. If you add, and get a value greater than 360, subtract 360. If you subtract, and get a negative value, add 360.

This way, the angle will always be between 0 and 360.

Hi Paul


Foreward is 90 degrees on the servo so :-
rawDesiredAngle = Heading + (90 - angle)
If rawDesiredAngle < 0 then desiredAngle = 360 + rawDesiredAngle
ELSE desiredAngle = rawDesiredAngle MOD 360.

So determining the desired heading is not a problem, but it gets tricky when the desiredAngle is very close to 360/0 degrees.

Currently my (psuedo) code runs something like this…

turnUsingCompass(servoAngle, currentHeading) {
  int turnDirection; //0 = left, 1 = right;
  int rawDesiredHeading = currentHeading + 90 - servoAngle;
  int desiredHeading;
  //*****************determine desiredHeading
  if (rawDesiredHeading < 0 ) {
    desiredHeading = 360 + rawDesiredHeading;
  else if ( rawDesiredHeading > 360 ) {
    desiredHeading = rawDesiredHeading MOD 360;
  //**************** Determine turn direction - can be done using angle but using heading & desiredHeading to make modular
  if ( desiredHeading - currentHeading  < 0 ) {
    turnDirection = 0; //turn left
  else {
    turnDirection = 1; //turn right
  //**** correct turn based on the size of the difference
  if ( ABSOLUTE( desiredHeading - heading )) > 180 ) {
    turnDirection = (turnDirection + 1) MOD 2; // turnDirection = !turnDirection == turn the opposite way
  if ( turnDirection = 1 ) {
    while (ABSOLUTE(desiredHeading - currentHeading) > 5) {
      currentHeading = compass.heading();
  else { //turnDirection = 0
    while (ABSOLUTE(desiredHeading - currentHeading) > 5) {
      currentHeading = compass.heading();
do some stuff to verify

Now the problem with this is if the desired heading is close to 360/0 then it has a small window in which to stop the turn loop. It is this small window that I am having trouble figuring out a solution…

Actually, FWIW this (psuedo) code seems to work fine in my excel simulations. Although I cannot simulate the vagarities of the compass readings, when I do simulate a jump of say 5 degrees or so, a tolerance of 5 is generally good enough to be caught even when the desiredHeading is 0 degrees (worst case with a window of 6 degrees), giving my ABS(desiredHeading - currentHeading) > 5 switch at least one chance to fall out of the loop.

I guess I need some manner of determining when we are moving away from our desired heading having headed towards it. Perhaps by limiting the largest turn distance to 90 degrees or a way of measuring convergence followed by divergence flag...


Work out the difference between your desired heading and your current heading. This will be a signed number between +/- 360 degrees. Use a simple range check to decide which direction to turn towards the desired heading. This produces a turn angle in the range +/- 180 degrees. Choose the turning speed proportional to the turn angle. This means the bot will turn rapidly at first, and then slow down as it reaches the desired heading. If you're doing the direction sensing by dead reckoning, you might also want to control the acceleration so the speed ramps up/down progressively to avoid wheelslip.