Trouble coding Servo motor to move 90 degrees