fix servo_write