You r doing hell of a work machan!!!
Do you have any books on robotics machan?
specially this one "J. L. Jones, A. M. Flynn and B. A. Seiger, Mobile Robots:
Inspiration to Implementation, A.K. Peters, 2nd Edition, 1999. "
Pls upload if you have. Thanks bro.