Updated code for real robot.