Arduino Servo Motor Control Code