First attempt at refactoring of original servo code. Only adding servo support as a start to keep things simple.
