Commit after bag day

some systems tested. still need to do more testing at SD and with
practice bot.
Also have smartdashboard configuration for debug/tuning/and diagnostics
TODO before SD:
-Clean up teleop
-Make controls more intuitive
-prep code for testing
TODO In SD:
-Tune arm PID for new shorter arm
-Tune arm manual control
-Test arm motor
-Tune shooter control loops
-Test auton driving functions
-Write and test simple auton
-Test controls