Uncommented everything/cleaned unneeded ".h"s

Uncommented all code, removed WPILib.h when unnecessary.
Fixed library issue. Code runs with issues.

Drive didn't work, arms didn't work, comp turned off and on.