Fixed the drive code.

* Correctly convert between unsigned and signed char
* Fixed wheel and controller inversions
* Moved teleop dispatch code into Main.c