removed tuning code and cout statements from master code

Author: jschlind411

src/behaviours/src/DriveController.cpp

@@ -220,7 +220,7 @@ void DriveController::ProcessData()
 
 void DriveController::fastPID(float errorVel, float errorYaw , float setPointVel, float setPointYaw) {
 
-  float velOut = 0;//fastVelPID.PIDOut(errorVel, setPointVel);
+  float velOut = fastVelPID.PIDOut(errorVel, setPointVel);
   float yawOut = fastYawPID.PIDOut(errorYaw, setPointYaw);
 
   int left = velOut - yawOut;
@@ -238,7 +238,7 @@ void DriveController::fastPID(float errorVel, float errorYaw , float setPointVel
 
 void DriveController::slowPID(float errorVel,float errorYaw, float setPointVel, float setPointYaw) {
 
-  float velOut = 0;//slowVelPID.PIDOut(errorVel, setPointVel);
+  float velOut = slowVelPID.PIDOut(errorVel, setPointVel);
   float yawOut = slowYawPID.PIDOut(errorYaw, setPointYaw);
 
   int left = velOut - yawOut;

src/behaviours/src/LogicController.cpp

@@ -154,14 +154,10 @@ Result LogicController::DoWork() {
     //used for precision driving pass through
   case LOGIC_STATE_PRECISION_COMMAND: {
 
-//  cout << "I MADE IT HERE!" << endl;
-
     //unlike waypoints precision commands change every update tick so we ask the
     //controller for new commands on every update tick.
     result = control_queue.top().controller->DoWork();
 
- //  cout << result.pd.cmdAngularError;
-
     //pass the driving commands to the drive controller so it can interpret them
     driveController.SetResultData(result);
 

src/behaviours/src/PID.cpp

@@ -123,7 +123,6 @@ float PID::PIDOut(float calculatedError, float setPoint) {
 
   float PIDOut = P + I + D + FF;
 
-  cout << "PID: " << "P:" << P << "  I:" << I << "  D:" << D << endl;
   if (PIDOut > config.satUpper) //cap vel command
   {
     PIDOut = config.satUpper;

src/behaviours/src/SearchController.cpp

@@ -25,14 +25,7 @@ void SearchController::Reset() {
  */
 Result SearchController::DoWork() {
 
-    result.type = precisionDriving;
-    result.pd.cmdAngularError = angles::shortest_angular_distance(currentLocation.theta, 0);
-    result.pd.cmdVel = 0;
-    result.PIDMode = SLOW_PID;
-
-    cout << "PID:  " << result.pd.cmdAngularError << endl;
-
-/*  if (!result.wpts.waypoints.empty()) {
+  if (!result.wpts.waypoints.empty()) {
     if (hypot(result.wpts.waypoints[0].x-currentLocation.x, result.wpts.waypoints[0].y-currentLocation.y) < 0.10) {
       attemptCount = 0;
     }
@@ -71,7 +64,7 @@ Result SearchController::DoWork() {
 
     result.wpts.waypoints.clear();
     result.wpts.waypoints.insert(result.wpts.waypoints.begin(), searchLocation);
-    */
+    
     return result;
   //}