Servo class update

Fixed bugs