Servo library major rework