Servo motor control