Check out this post, code in post 16.
http://www.picbasic.co.uk/forum/showthread.php?t=4972