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
Bookmarks