Update robot.ino

Todo, and tidy