Cleaned up the robot state machine