Servo changes

Cleaned it up