Controlling a servo motor