I suppose what you did produces a small oscillation at the output.

I had a similar implementation with that result.

So I thought that PID routine made by Henrik could help me but had never managed to properly adjust the P-I-D settings:

http://www.picbasic.co.uk/forum/showthread.php?t=5874

Maybe you are in luck making it work for you.

Ioannis