Now it controls the servo OK