Servo control now working