Updated command set and stubbed out servo control.