Cambios
- RobotState contiene ahora el control aplicado en velocidades lineales y angulares en vez de velocidad de las ruedas
- Debug ahora es un atributo de OptimalControl
- Se movieron tres los metodos printBand y stop para el final del archivo
- Se imprime al final de la ejecucion el tiempo que le llevaria al robot recorrer todo el camino.