Add servoWrite