Attempted Merge between Auto and Teleop code

Functionality of this code is not guaranteed. Test subsystems.
Auto Fwd, Setting Speeds
Code from After mini-regional

several tweaks/function changes
Product of sync from team laptop
cleaned up code merge between nick and ashwin
fixed merge conflict