Refactor in functions.
Refactor servo methods: move common functionality to _set_servo.