Fixed bugs, added the correct rotation to the robot.