Code Cleanup

-Shrunk down some code
-Reorganized OI.h to go joystick by joystick.
-Added a Debug function for each subsystem to output to console
relevenat information.
-Removed unused PID Controllers, variables, and functions.
-Added commenting throughout.

To Do:
-Vision.
-Autonomous.
-Auto-climb.
-Auto-shoot.