Added Test() function code to run robot. If you turn the robot on, it NEEDS to be in test mode.

Select "TEST" in the driver station program.
Added comments to teleop.
Added watchdog functionality to test() method.
Updated control code.