Excessive jitter spindle position tracking #164

Looking at axis velocity / acceleration during G33 motions, there is a noticeable jitter in velocity, and large acceleration swings. The image below (zoomed in significantly) shows this occuring.

This is strange to see in a simulation config, because the encoder signal is very high resolution, and the the simulated spindle speed signal is clean, constant, and smooth. This obviously isn't representative of real machines with real encoders, but I believe this ideal case shows a flaw in the position error calculation.

Here are the steps I follow to reproduce the issue:

1. Load an axis sim config with a simulated spindle encoder (I used AXIS as well, but it doesn't matter)
2. Configure HALScope with two channels, Zvel and Zacc
3. Run this G code program

G90 G20 G64 P0.005
G0 Z0
S240 M3
G33 Z-.5 K0.1
G0 Z0.1
M5
M2

Here's a branch with this change implemented. With a perfect spindle encoder, there is no jitter in the motion tracking:

With encoder resolution of 256 PPR, there is still jitter, but not as much:

Compare this to stock 2.7 performance with the same simulated spindle encoder:

For reference, here's a test branch with stock 2.7 spindle tracking and the quantized spindle encoder.