cleaned up closed loop control, added code for PID control to test.
Merge origin/CompBot into CompBot