Servo level up

Forgot to mention, adding some tests....