# Algorithm to route a robot based into a Compass

I need help into a algorithm to turn a robot left (CCW and stop if compass<= target) or right (CW and stop if compass>= target). The problem is on the zero cross (when the countin pass from 359 to 0 or the opposite), example, I need to turn a robot left 30CCW, and I am into 10. So I need to turn left untill compass<= target, so 340, but see: the position I’m already is compass<= target. THIS IS THE GOAL, fix this.

Below is my function:

``````void routingPathC(int targetC, int compass)
{
if (routePathCompassStart == false)
{
routePathCompassStart = false;
routingPathCompass = true;
cw = false;
ccw = false;
degreesC = 0;
calc1 = 0;
calc2 = 0;
doneC = false;

if (targetC < compass)
{
calc1 = compass - targetC;
calc2 = 360 - compass + targetC;
if (calc1 <= calc2)
{
degreesC = calc1;
ccw = true;
}
else
{
degreesC = calc2;
cw = true;
}
}
else if (targetC > compass)
{
calc1 = targetC - compass;
calc2 = 360 - targetC + compass;
if (calc1 <= calc2)
{
degreesC = calc1;
cw = true;
}
else
{
degreesC = calc2;
ccw = true;
}
}
else if (targetC == compass)
{
degreesC = 0;
cw = false;
ccw = false;
}
Serial.print("- Turn: ");
Serial.print(degreesC);
if (cw == true)Serial.println(" CW");
if (ccw == true)Serial.println(" CCW");
}
if (routingPathCompass == true)
{
// HERE I PUT THE MOVEMENT FUNCTION FOR CW OR CCW
}
}
``````

When navigating by compass, you need to correct for “compass wrap” twice, once when calculating the bearing and once when calculating the heading error.

Here are the routines I use:

``````// routine to calculate heading error in degrees, taking into account compass wrap

{
int error = current_heading - bearing;
if (error >  180) error -= 360;
if (error < -180) error += 360;
return error;
}

// routine to correct for compass wrap

int wrap360(int direction) {
while (direction > 359) direction -= 360;
while (direction <   0) direction += 360;
return direction;
}

// part of Main program, for differential steering with two motors

...

// continue to run this leg, steering with IMU
// (may not have recent GPS info in the following)

I2C_Read3Vectors(BNO055_A0,BNO055_FUSED_EULER, (int16_t *)v);  //get Euler angles

for(i=0; i<3; i++) v[i] >>= 4;

// update LCD display
clear();
print_long(point_bearing);
print(" ");
lcd_goto_xy(1,1);
print_long(num_sats);
print("s ");
print_long(dist_to_go);

// heading error and PID steering

//error is positive if current_heading > bearing (compass direction)
//positive bias acts to reduce left motor speed, so bear left

bias = (kp*error)/10;  //Kp in tenths (Ki, Kd not necessary in this case)

//dump to log file

printf("I,%lu,%d,%d,%d,%d,%d,%d,%u,\n",millis(),