Fixed SERVO write bug