Fixed new servo subsystem and command.  Needs to be tested