No need to go far away from your PBP lanquage.

Have a look at Henrik's routines here http://www.picbasic.co.uk/forum/showthread.php?p=54846

It is not easy as A-B-C but also not too hard to follow. May the difficult part would be to tune the PID parameters.

Ioannis