Velocity Calculation rework

[Copper280z]

trafficstars

Seeing the relatively frequent questions and problems associated with noisy velocity measurements on the discord server, what do you think about a change like this to the velocity calculation?

The idea being that currently when we make a call to getVelocity(), if the angle change is zero then we get a zero velocity, if we have a non-zero angle change it appears as a very high velocity because all of that change is accounted for in one cycle. Because our loop can often run many cycles between angle changes. This shows up (unfiltered) as a series of zero velocity measurements with large single sample spikes, this is obviously nonphysical. The current fix to this is to heavily filter the velocity signal, which introduces a large phase loss in the control system.

This change only updates the velocity if an angle change is observed and otherwise reports the previous velocity. This results in a closer approximation to a continuous signal.

One degenerate case that is not handled is if the motor comes to a complete and total stop, the velocity will not be reported as zero. Practically, this doesn't seem like a problem because the reported velocity will be very small, very few motors will come to an absolute stop for any long period of time, and a floating point zero value (vs very small non-zero) isn't actually very useful.

This change is currently untested on hardware.