Updates to drive system, numerous bugfixes..

Tested Stable. No pneumatics or autonomous yet, but everything else is
tested stable.