Changed a lot of code to allow for PID control. We are now using PIDMotor for all PID code.