Finished up basic driver functonality
Hacked together velocity control on a position controller. Needs testing.