Diff: Motion.cpp

Revision:

34:0587c0943263

Parent:

33:836ab2089565

Child:

35:5a4e1a87b3da

--- a/Motion.cpp	Wed May 23 12:30:39 2018 +0000
+++ b/Motion.cpp	Thu May 24 14:11:21 2018 +0000
@@ -412,9 +412,6 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
-        //controller.setDesiredSpeedLeft(-ROTATE_SPEED);
-        //controller.setDesiredSpeedRight(-ROTATE_SPEED);
-        
         t.start();
         
         while ((countsL - countsLOld)  > -900 || (countsR - countsROld) > -900) {
@@ -460,21 +457,6 @@
         distanceL = irSensorL.readL();
         distanceR = irSensorR.readR();
         
-        
-        /*if (distanceL < wallLeft-1.0f && distanceR < wallRight-1.0f) {
-            
-            errorP = distanceL - distanceR - 3.0f;
-    
-        }else if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
-                
-            errorP = distanceL - LEFT_MID_VAL;
-            
-        }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
-                
-            errorP = RIGHT_MID_VAL - distanceR;
-            
-        }*/ 
-        
         if (distanceL < wallLeft && distanceR > wallRight) {
                 
             errorP = distanceL - wallLeft;
@@ -503,9 +485,7 @@
         
         if (abs(errorP) < 80.0f) {
             totalError = KP*errorP + KD*errorD;
-        }/*else{
-            totalError = 0;
-        }*/
+        }
         
         controller.setDesiredSpeedLeft(actSpeed - totalError);
         controller.setDesiredSpeedRight(-actSpeed - totalError);