Working code currently on the robot (debugged)