Added untested code for milestone 5 since I dont have the robot...