Finished the straightenUp method. Changed to a fixed rotation servo.