Clean up the unessecary code. Added a timer to turn right so that the robot has to be in the correct orientation for a second before it is finished.