Hey, one error: MAximizing the robot speed makes him useless...he moves between two points and does nothing anymore
I haven't noticed this problem until now, thanks for the help