EasyC PID Control

What i want to do is to be able to maintain my flywheel at a constant speed no matter what battery i have. Also, i want it to have a very low cool-up on the flywheel and i know the PID can help with that. This is what i have so far and it sort of works. I am using IME to detect the speed of my motors. What i have noticed is that the speed it is detecting its not completely constant which means that my encoder will never be in the target value because it is constantly going up and down the speed to reach the value but it cant because it is not that precise for some reason. Is there a way i can fix this or make it better? Has anyone made this work in Easy C?
Thanks in advanced

ok first of what you have there is not PID control. here is a document explaining PID control, the documents and video with in are written in general C I don’t know how to use easyC but maybe you can convert.

it seems to me like the problem you are complaining about is that you have unstable velocity measurements, the best Idea I can give you is to take some kind of average, perhaps you could take a simple average of the last 10 velocity readings. What I use is something called an exponentially weighted moving average. essentialy what this does is average a group of past values but gives more importance to the newer ones. this has helped me make a more stable measurment