pic: Small CIM in wheel swerve

[Ether]

Originally Posted by Ether

If your sample time is fast and stable enough that the encoder could never turn 180 degrees from one sample to the next, then "all ya gotta do" at each sample is assume the encoder turned through the shortest angle, and keep track of zero crossings.

Here's the code (I think). Haven't tested it:

Let A be the encoder angle reading in degrees (0 to 360 clockwise) at the previous sample, and B be the encoder angle reading in degrees at the current sample. Then:

D = B-A;
D -= 360*floor(0.5+D/360);
if((D>0)&&(A>B)) turns++;
else if((D<0)&&(A<B)) turns--;