@Error404:Robot Not Found, you are correct, you get a 
for catching a couple of sloppy mistakes in my post. Lets just pretend there is a magik “foo” sensor that reports w = 100 RPM when flywheel runs at its nominal speed and has no more than +/- 5 RPM of noise.
Seriously, though, past weekend our launcher speed control performance went from “quite good” to “borderline erratic” when technik jr changed delay interval from 50 to 10 msec while, at the same time, refusing to acknowledge the importance of handling roundoff errors.
Apparently, “not-your-TBH” code, that technik jr is proud of writing, becomes unstable over shorter intervals, when noise in the measurements exceeds some threshold. That was the reason, I run a number of experiments demonstrating the noise in the measurements to any denier.
Calculating velocity over 1 msec interval must be killing you with the roundoff errors. Here is a couple of graphs from the two experiments, where velocity was computed based on the difference between quad encoder readings over 5 msec and 20 msec intervals:
You can clearly see the blocky nature and significantly larger noise level in the experiment with 5 msec interval.
Since you asked not to post the entire code I will just give you partial solution, and let you figure out what else needs to be done:
float w, wSmooth = 0;
int w1, w2=0, w3=0, w4=0, w5=0;
while(1)
{
w1 = SensorValue[foo];
w = (w1-w5)/20.0;
wSmooth = (3*wSmooth + w)/4;
// do something with w and wSmooth
// do something to shift old sensor values around
delay(5);
}