Modified after test on the robot