This would be a good place to start...

PID-filter routine (2nd try). (HenrikOlsson)
http://www.picbasic.co.uk/forum/showthread.php?t=5874

It takes a bit of playing to get the right sample rate and P-I-D constants, but it works pretty good when you do.
<br>