[FEATURE] Instead of derivating position to get velocity and PID on it, integrate target velocity

[Moddingear]

Is your feature request related to a problem? Please describe.
The current velocity mode is sensitive to the resolution of the encoder for high update rate function, and gets very noisy. Furthermore, on motors with cogging, it struggles to keep up.

Describe the solution you'd like
Instead of derivating the angle of the sensor, integrating the target velocity to get a position would reduce the noise. Of course, that delta position would need to be divided by delta time to stay a velocity.

[dekutree64]

Do you have a code example? I fail to see how integrating the target would accomplish anything.

A solution I've considered is dynamic filter constant and/or sensor.min_elapsed_time, so low speed averages many frames worth of readings and high speed is nearly unfiltered. It's basically an aliasing problem. If the encoder ticking rate and loop rate are nearly equal, you'll sometimes see two ticks per update which appears as a sudden doubling of speed, or zero ticks per update which appears as a sudden stop. As the speed increases or loop rate decreases, that variation of one tick per update becomes a smaller percentage of the measured value so it's not a problem.

It would probably be best to apply sensor.min_elapsed_time to the motion PID update rate too. It solves the aliasing problem by decreasing the loop rate for velocity reading, which causes a delay between physical velocity change and measurement change, so if the PID is fighting to correct it during that time it will probably overcompensate before the next reading comes in.