I have a PID controller that controls the yaw direction. With a compass, the yaw heading is given form -180 degrees to 180 degrees. The issue with this is how the PID controller calculates error and correction direction.
error = input - setpoint;
P = (error * kp);
If the Arduino is at 45 degrees and wants to go to 50 degrees, the PID controller workes perfectly. The issue comes into play when the setpoint approaches -180 or 180 degrees.
If the Arduino is at 175 degrees, and the setpoint is -175 degrees. The PID controller will correct 350 degrees in the wrong direction, instead of going 10 degrees in the correct direction. Is there a way to calculate the shortest distance between Pi and -Pi and input that value into the equation above?
Hey mschindl,
Are you using a PID librairy or an handmade P regulator? Sorry but this is hard for me to see with only 2 lines
F
rom a rough approach, maybe just placing a condition with a "if test". If the error is above 180 degrees (in absolut value?) you can send the 360-|x| value of the one you were supposed to send to your arduino.
This is just a concept idea without testing but it might be a good thing to test from my opinion.
It is standard, and much easier to use 0-360 instead of -180 to 180 for compass directions (0 = true North, 90 = East). Add 360 to the compass output, subtract 360 if necessary by:
heading = (360+compass reading)%360;
then (for PID steering) the yaw error and shortest direction to correct it is automatically chosen by:
Another way is to make the Setpoint zero and make the Input the difference between desired heading and current heading. Are the headings in integers? Floats? Degrees? Radians? You talk about both +/-180 and +/-Pi.
int headingError = desiredHeading - currentHeading;
if (headingError > 180)
headingError -= 360;
if (headingError < -180)
headingError += 360;
Input = headingError;
With your example:
desiredHeading = -175; // SSW
currentHeading = 175; // SSE
headingError = -350;
(-350 < -180) so:
Input = headingError + 360 = 10;