# compass rotate

Hi,
I'm building a robot with arduino mega and a cmps03 with 12c.
I would like to put the robot in different place, and make sure he rotates for spesific amount of degrees (90, 45 and so on). in the program I will rotate the robot CW or CCW (not based by the compass value, I will select the direction each time in the program).
I have no problem to read the compass value, but I just don't know how the handle the logic behind it.
My compass is between 0-360, so there's no negative values.
So, for example, I want to rotate the robot 90 degress.
If the compass starting value is 0, I will just wait until the compass read will be grater than 90.

But however, If the compass no is 280 deg, the final reading should be 10 deg (280+360-90=10) and that's why I can't just substract the current and starting values, otherwise I'll get
The robot would be in different angels so i want to know if you have any suggestion of programing it.
I currently only thought about those situation, but I'm sure that's theres many more others I didn't even get to my mind (like when I want the robot to rotate CCW instead of CW).

Again, I don't have any code currently, I just want to understand the logic behind it, I'm pretty sure the soultion is multiple if cases like (currentvalue-startvalue >=180) but I cant figgured that out.

Thanks for the helpers

My compass is between 0-360, so there’s no negative values.

easy Subtract 180 from your value. You will want to do this.
always have your setpoint as Zero
make your adjustment to the value you are getting from your compass and use it as an offset of zero
Working example: Balancing bot truing 90° every 5 seconds it could have turned to any angle here is my code pertaining to turning:

``````//PID Configuration in the beginning of my code uses PID_V1 library
double DegSetpoint = 0; // Degree Setpoint should always be Zero!
double YawInput; //(YawInput = YawMPU + YawOffset) with rollover at +-180°
double Turn; // Outout to drive motors
double YconsKp = 30, YconsKi = 0, YconsKd = .1; // PID values for turning these should be good!
PID myTurnPID(&YawInput, &Turn, &DegSetpoint, YconsKp, YconsKi, YconsKd, REVERSE); // Make it so

The Math routine
void MPUMath() {
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
YawMPU = (ypr * 180 / M_PI) ;
YawInput = YawMPU + YawOffset;
if (YawInput > 180) YawInput -= 360; // rollover to negative
if (YawInput < -180) YawInput += 360; // rollover to positive
YawInput = YawInput;
myTurnPID.Compute();// Calculates the amount of "Turn" to place to the motors. for my robot it was a value of +-80 to one motor opposite the other while balancing causing it to rotate the PID adjusts the power to the motors so it lands on the new setpoint
DirectDrive(Output, Turn);
}
``````

I use a PID routine to calculate how my output directs my robot to point to zero
The following code is what I used in to control my robot’s turning

``````static long QTimer = millis();
if ( (long)( millis() - QTimer ) >= 5000 ) {
QTimer = millis()
YawOffset += 90;
if (YawOffset > 180) YawOffset -= 360; // rollover to negative
if (YawOffset < -180) YawOffset += 360; // rollover to positive
}
``````

(current+offset+360)%360

So you have a current heading of current_heading

both these values are between 0 and 360.
You want to know whether to turn left or right.

Now at this point, we want to do a modulus. The problem is that the % operator does odd things with negative numbers. so:

Take the modulus % 360.

The result will be a value between 0 and 360. You can then safely subtract 180 from that.

course_correction = (desired_heading-current_heading+180+720) % 360 - 180;

positive means you turn one way, negative means you turn the other. Zero means you are on course.

Well you say you know if you want to turn say 45 deg CW or CCW

To get to target heading

If CW you want to subtract 45
If CCW you want to add 45

Both those operations can lead to a number greater than 360 or lower than zero which you don't want

If CW you test is the result of the subtraction current-45 is negative, if yes, add 360 to it if not keep as it is.
Example: current = 30, 30-45 is negative 15, so you add 360 and you get 345 as target heading

If CCW you test if the result is greater than 360 and if so you substract 360
Example: current = 320, 320+45 is 365, so greater than 360 , so you substract 360 and you get 5 as target heading

Then do turn in the CW or CCW sense you decided upfront until the heading value is equal to the target heading.

Example CW: current = 30, 30-45 is negative 15, so you add 360 and you get 345 as target heading
So you turn CW, your compass will tell you 30,29,28,27,....0,359,358,357,...345 - bingo! you stop

Example CCW: current = 320, 320+45 is 365, so greater than 360 , so you subtract 360 and you get 5 as target heading
So you turn CCW, your compass will tell you 320, 321, 322,...359,0,1,2,3,4,5 - bingo! you stop.

This is similar to using modulo operator but as modulo of negative number needs to be handled manually, it means you need a test anyway, so just do simple math - addition or subtraction are much faster than a modulo.

Being fast is important so that you don't miss one heading reading because you have been too slow in checking where you currently head esp. if you only check for == value (you might end up doing a full extra turn or more before the values match).

You probably need to implement a better logic of slowing down rotation when you approach target heading - which requires again a bit of smart math near zero angle. Easy way to handle that is if target angle is between 355 and 5 degrees, then check the cosinus of the differences of current_heading and target_heading. the cosinus does not care if its parameter is negative or positive and as cos(0)=1, basically when you do the difference, if you are close to 1 for the cosinus between the 2 angles, it means you are close and you slow down the motor.

You can do this in general case, but as cosinus is slow - when your target angle is not near zero, then just compute the mathematical distance between the angle as absolute value of (target_heading - current_heading). When this gets close to zero then slow. abs is much faster than cos so your general case will be better.