Turning With PID: How to find the shortest turn

Hey there o/

I was wondering is some of you could help me with a problem I have been having with my PID implementation. The robot will properly turn if it is in the range of 2-358 degrees. My issue arises when the robot rotates to a position that would be a negative degree or a degree outside of 360. An example I can give is that, if I tell the robot to turn to the heading of 270, when the robot is at 0, the robot will rotate the longest route to get to 270(it turns right). However, it would be much more efficient to take the shorter route and turn left. This same issue occurs if there is idiosyncrasies in my inertial sensor. Sometimes, there is drift in the sensor and rather than starting at 0, the values will drift to 359. This will cause the same issue where the robot will take the longest route to get to 90 degrees. If you need more clarification please feel free to ask. As always, any advice or constructive criticism is welcomed and appreciated!

Have a fantastic day!

–Josh

Generally speaking the best way to handle this is to use a processed “rotation” value rather than the heading itself, where the rotation value tracks the amount of total turning done. For example if we started at heading 30 and made a 720 degree turn, our heading would still be 30 but this rotation value should be 750. If you’re using vexcode, use rotation() instead of heading(). Similar for PROS (with slightly different syntax). Then in the PID, turn to a certain rotation instead of a certain heading. This means that you can always turn the short way if you want (unless your robot gets completely messed up rotation-wise, which shouldn’t happen anyways), but you can also turn the long way if you need to for whatever reason.

When you compute the error between the current angle and goal angle you just need to tweak it to be between -180 and 180 degrees. The following pseudo code for example checks when the error is large (going to turn the suboptimal direction) and shift by 360 degrees so it takes the shorter path.

if abs(error) > 180
    error -= sgn(error) * 360

So every loop of your controller just do the above.

5 Likes

Hi, so if I am reading this correctly, you want to robot to turn at the shortest angle. Luckily for you, I tried to solve this months ago and developed an algorithm that I don’t mind sharing.

Find shortest turn code
int DesiredAng(int angle) {
 double CheckL;
 
 if(angle > OdomHeading) {
   CheckL = abs(angle - OdomHeading);
  } else{
   CheckL = (360-OdomHeading)+angle;
  }
 
 double CheckR;

  if(angle > OdomHeading) {
   CheckR = (360-angle)+OdomHeading;
  } else{
   CheckR = (OdomHeading - angle);
  }

  double Heading;
  if(CheckL > abs(CheckR)) {
   Heading = CheckR;
    } else {
   Heading = -CheckL;
  }

This here should be what you are looking for. However, what’s the point of just giving you code without an explanation? So I will provide you with a link that will allow you to play with the function. You can use this to see how it works and how to implement it into your code.

shortest angle link

Shortest turn link

Directions
  1. copy and paste all characters in “shortest angle link” into a different tab and press enter
  2. the code is pretty straight forward, there are comments that will tell you where to enter values.
  3. And run the code, the terminal will display your current, desired, left, right, and quickest rotation.

If you need any more help or further explanation, feel free to ask.

Comparison if anyone is curious

my version on c++ shell

Cpp code if you didn’t like the psuedo code from before

  if(abs(error)>180){
      error -= copysign(1.0,error) * 360;
  }
1 Like

One other benefit to @tabor473 's implementation - change the desired heading from 170 to 170.2 (or any heading with a decimal value):

     Left Turn is 340     Right Turn is 20     Desired Angle is 170.2     Current angle is -170     Required rotation 20
     before 340.2
     after -19.8

Assumptions about data types when programming can affect the accuracy of your robot.

2 Likes

Here is what I use:

double constrainAngle(double x){
    x = fmod(x + 180,360);
    if (x < 0)
        x += 360;
    return x - 180;
}

This function will take any angle and convert it to the equivalent angle between -180 and 180.
Example:

Error = constrainAngle(TargetAngle - CurrentAngle); 
//Attempting to reach 270, current angle is 0.
//rather than turning right for 270 degrees, this will convert 270 to -90.
//... instead turning left 90 degrees

credit to User Mysticial - Stack Overflow for the function.