[Richard100]

Originally Posted by Ether

modulo is not what you want here.

what you want is the shortest angle from the previous reading to the present reading, with the correct sign. The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

C# and Java both support IEEERemainder.


If your language does not support IEEERemainder, you can use this one-line function instead:

shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360);

Some practical considerations regarding this approach ...

Note that if one is streaming readings from an angular position sensor (such as a gyro or IMU), one would need to add an accumulator to this shortest_angle function to continually maintain angular position.

Also be aware that the function is sensitive to sampling rate. This is because a late sample might allow a current position (the 'present' variable) to get more than 180 degrees away the previous position (the 'previous' variable). If this happens then the shortest angle will switch to the other side of the cycle. If you've ever noticed movies where wagon or car wheels appear to be moving backwards then you've seen this aliasing effect (this can happen to your sensor, too).

To avoid this, ensure the iteration period is at least shorter than (1 / 2*RPS), where RPS is the maximum revolutions per second expected from the sensor reading stream. For example, if your robot maximum yaw rotation rate is 1000 deg/sec, you must iterate the function no slower than every 180 ms ... (someone check my math).

This is likely not a problem for most FRC robots, as yaw rates don't normally get that high (ours don't get much higher than 500 deg/sec), and computational iteration rates are probably much faster - but one should be aware of the constraint. You might try the function on a robot mechanism that rotates much faster and wonder why your code is suddenly unreliable.

If the sensor readings are noisy, this will further erode the sampling margin. If the peak noise level in degrees is 'n', then the sampling constraint becomes:

Loop Period < (180 - 2*n)/(360 * RPS)

Of course, excessive noise should be remedied (fixing the root cause). One should just keep in mind that it's good practice to maintain healthy margin for practical issues like noise, and in fact whether your loop can always be trusted to occur on schedule.

The exploit of the shortest directional path property of the IEEERemainder function is quite clever for dealing with these orientation discontinuities. I wasn't aware of it until Ether pointed it out - (thank you Ether). It doesn't appear that LabVIEW has this function (although easy enough to build from primitives) ... unless I'm just not finding it - anyone know for sure?