Move servo code to servo.go