Add drivetrain code from RobotCode-2014 (with my modified DriveStraight)