Grafico el path!
Encontre el error... Solucionado!
Arreglando el alg... Pero sin saber porque no modifica el curso actual el robot :(
Ya anda! Martin, el mundo no es puro :(. Luchando contra las animaciones ahora
Solo se grafica en draw, en el resto solo se calcula la posicion y se va avanzando el robot