adding drive straight function and extending testDrive() to allow for tuning and testing of driving straight
Merge branch 'develop' of github.com:erhs-robotics/frc2014 into develop