servoControl modifications