Fix robot main loop