Quote:
Originally Posted by DanielHa
at certain absolute angles, if you simply add or subtract 180 (or in your case the slightly altered values), then you might get a value which is outside the range of the navX.
|
Let S be your sensor angle reading (in degrees, can be any range, even outside the range +/-360)
Let T be your desired angle position (in degrees, can be any range, even outside the range +/-360)
the angle between them (let's call it ERR) is found by the equation:
ERR = (T-S) - 360*floor(0.5+(T-S)/360);
the above returns a value for ERR between -180 and +180 degrees,
the shortest angle path to the target. Don't believe me? Try it.
So it tells you which direction to rotate as well as how much to rotate.
To use this with a PID, use processVariable = setpoint-ERR