Problem with Servo and Encoder