Programming Puzzle

[Chris Hibner]

I wrote a vi called HeadingWrap.vi that we've used for autonomous. You can use it to convert an angle outside of +/-180 degrees to be within the +/-180 degree range. You can use it to do what you're trying to do.

This is how it's used: Calculate your angle error into your PID (i.e. desired - actual) and feed the result into HeadingWrap.vi. The output will come out within the +/-180 range and will give you the shortest turn to your desired angle.

You can find it here: http://www.chiefdelphi.com/media/papers/2553 in the sample robot code zip file.

Here's more or less how it works:

Let's say you command your canon to 355 degrees and it overshoots so your sensor reads 3 degrees. For your next PID calculation, your error is 355 - 3 = 352 degrees. This is the problem you've had: it wants to command your canon to do another loop. To solve it, check your error to be within +/- 180. If not, subtract (or add, if the error is negative) 360 degrees. For this case, we subtract 360 so: 352 - 360 = -8 degrees. This will get your canon to head in the right direction by the correct number of degrees.

Here's the basic algorithm:

Error = desired - actual;
while (abs(Error) > 180)
{
   if (Error > 180)
      Error -= 360;
   else
      Error += 360;
}

You now just have to trick your PID: feed the above "Error" calculation into your setpoint and feed 0 into the process variable.

EDIT: For your application, the while loop is unnecessary. We use it in autonomous since we allow the heading integrator to grow past 360 in both directions (i.e. we don't wrap the heading integral - we just take care of the wrap on the error term as stated above).