Refactor ContinuousServo driver