Diff: Motion.cpp

Revision:

5:e2c0a4388d85

Parent:

4:932eb2d29206

Child:

6:4868f789c223

--- a/Motion.cpp	Mon Apr 23 12:14:54 2018 +0000
+++ b/Motion.cpp	Mon Apr 23 17:59:13 2018 +0000
@@ -7,7 +7,7 @@
 const float Motion::LEFT_MID_VAL = 44.73f;
 const float Motion::RIGHT_MID_VAL = 47.03f;
 const float Motion::KP = 2.0f;
-const float Motion::KD = 0.00f;
+const float Motion::KD = 0.0f;
 
 
 Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
@@ -26,7 +26,6 @@
  */
 void Motion::move() {
         
-        controller.counterReset();
         countsLOld = counterLeft.read();
         countsROld = counterRight.read();
         countsL = counterLeft.read();
@@ -38,14 +37,29 @@
         controller.setDesiredSpeedLeft(speedLeft);
         controller.setDesiredSpeedRight(speedRight);
         
-        while ((countsL - countsLOld)  < 1565 || (countsR - countsROld) > -1565) {
+        while ((countsL - countsLOld)  < 1605 || (countsR - countsROld) > -1605) {
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
+            distanceC = irSensorC.readC();
             
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
             
-            control();
+                control();
+            
+            if ((distanceC) < 40.0f) {
+                countsLOld = countsL;
+                countsROld = countsR;
+                while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
+                    countsL = counterLeft.read();
+                    countsR = counterRight.read(); 
+                }
+                stop();
+                break;    
+            }else if ( ((countsL - countsLOld)  >= 1647 || (countsR - countsROld) >= -1647) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
+                countsLOld += 500;
+                countsROld += 500;   
+            }
             
         }    
 }
@@ -54,7 +68,6 @@
  */
 void Motion::moveFast() {
         
-        controller.counterReset();
         countsLOld = counterLeft.read();
         countsROld = counterRight.read();
         countsL = counterLeft.read();
@@ -70,11 +83,16 @@
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
+            distanceC = irSensorC.readC();
             
             if (enableMotorDriver == 0) {enableMotorDriver = 1;}
             
             control();
             
+            if (distanceC < 40.0f) {
+                stop();
+                break;    
+            }
         }    
 }
 /**
@@ -82,7 +100,6 @@
  */
 void Motion::scanMove() {
         
-        controller.counterReset();
         countsLOld = counterLeft.read();
         countsROld = counterRight.read();
         countsL = counterLeft.read();
@@ -94,7 +111,9 @@
         controller.setDesiredSpeedLeft(speedLeft);
         controller.setDesiredSpeedRight(speedRight);
         
-        while ((countsL - countsLOld)  <  1565 || (countsR - countsROld) > -1565) {
+        distanceC = irSensorC.readC();
+        
+        while ((countsL - countsLOld)  <  1605 || (countsR - countsROld) > -1605) {
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
@@ -108,15 +127,22 @@
                 line = 1;  
             }
             
-            control();
+                control();
             
-            if (distanceC < 40.0f) {
+            if ((distanceC) < 40.0f) {
+                countsLOld = countsL;
+                countsROld = countsR;
+                while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
+                    countsL = counterLeft.read();
+                    countsR = counterRight.read(); 
+                }
                 stop();
                 break;    
+            }else if ( ((countsL - countsLOld)  >= 1647 || (countsR - countsROld) >= -1647) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
+                countsLOld += 500;
+                countsROld += 500;   
             }
-            
-        }    
-        
+        }  
 }
 /**
  * 90° Rotation nach Links
@@ -264,8 +290,9 @@
     distanceL = irSensorL.readL();
     distanceR = irSensorR.readR();
     
+    
     if (distanceL < wallLeft && distanceR < wallRight) {
-        /*if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
+       /* if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
             
             errorP = LEFT_MID_VAL - distanceL;
                
@@ -275,19 +302,25 @@
         }*/
         
         errorP = distanceL - distanceR + 2.30f;
+        //errorD = errorP - oldErrorP; 
     }else{
         
-        errorP = 0;   
+        errorP = 0; 
+        errorD = 0; 
     }
     
-    errorD = errorP - oldErrorP;  
+    //errorD = errorP - oldErrorP;  
     
     oldErrorP = errorP;
     
-    totalError = KP*errorP + KD*errorD;
+    if (abs(errorP) < 80.0f) {
+        totalError = KP*errorP + KD*errorD;
+    }else{
+        totalError = 0;
+    }
     
-    controller.setDesiredSpeedLeft(speedLeft + totalError);
-    controller.setDesiredSpeedRight(speedRight + totalError);
+    controller.setDesiredSpeedLeft(speedLeft - totalError);
+    controller.setDesiredSpeedRight(speedRight - totalError);
 }
 
 void Motion::runTask(int task) {