Maravilloso!
El enfoque es muy parecido al de RedPic (en CCS).
Me parece barbaro que lo compartas, yo estoy haciendo lo mismo con el hard de mi robot (
www.ucontrol.com.ar). Cuando haya adaptado tu codigo te comento como me fue, me va a llevar algunos dias por que estoy por ser papa en cualquier momento.
Muchas gracias!