Servo code working