Added drive train

Reorganized a lot of code, added motors to run the drive train, and
added the ability to take input from a controller.