Several improvements.  Code used for the Torneo Mexicano de Robotica demostration