Diff: Motion.cpp

Revision:

6:4868f789c223

Parent:

5:e2c0a4388d85

Child:

7:22392ed60534

--- a/Motion.cpp	Mon Apr 23 17:59:13 2018 +0000
+++ b/Motion.cpp	Tue Apr 24 15:35:24 2018 +0000
@@ -4,10 +4,12 @@
 
 using namespace std;
 
-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::LEFT_MID_VAL = 41.73f; //44.73
+const float Motion::RIGHT_MID_VAL = 44.03f; //47.03
+const float Motion::KP = 2.5;
 const float Motion::KD = 0.0f;
+const int Motion::MOVE_DIST = 1605;
+const float Motion::MOVE_SPEED = 50.0f;
 
 
 Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
@@ -34,10 +36,10 @@
         speedLeft = 50.0f;
         speedRight = -50.0f;
         
-        controller.setDesiredSpeedLeft(speedLeft);
-        controller.setDesiredSpeedRight(speedRight);
+        controller.setDesiredSpeedLeft(MOVE_SPEED);
+        controller.setDesiredSpeedRight(-MOVE_SPEED);
         
-        while ((countsL - countsLOld)  < 1605 || (countsR - countsROld) > -1605) {
+        while ((countsL - countsLOld)  < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
@@ -56,7 +58,7 @@
                 }
                 stop();
                 break;    
-            }else if ( ((countsL - countsLOld)  >= 1647 || (countsR - countsROld) >= -1647) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
+            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
                 countsLOld += 500;
                 countsROld += 500;   
             }
@@ -108,12 +110,12 @@
         speedLeft = 50.0f;
         speedRight = -50.0f;
         
-        controller.setDesiredSpeedLeft(speedLeft);
-        controller.setDesiredSpeedRight(speedRight);
+        controller.setDesiredSpeedLeft(MOVE_SPEED);
+        controller.setDesiredSpeedRight(-MOVE_SPEED);
         
         distanceC = irSensorC.readC();
         
-        while ((countsL - countsLOld)  <  1605 || (countsR - countsROld) > -1605) {
+        while ((countsL - countsLOld)  <  MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
             
             countsL = counterLeft.read();
             countsR = counterRight.read();
@@ -138,7 +140,7 @@
                 }
                 stop();
                 break;    
-            }else if ( ((countsL - countsLOld)  >= 1647 || (countsR - countsROld) >= -1647) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
+            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
                 countsLOld += 500;
                 countsROld += 500;   
             }
@@ -284,32 +286,32 @@
 
 void Motion::control() {
         
-    float wallLeft = 80.0f;
-    float wallRight = 80.0f;
+    float wallLeft = 48.73f;
+    float wallRight = 51.03f;
     
     distanceL = irSensorL.readL();
     distanceR = irSensorR.readR();
     
     
     if (distanceL < wallLeft && distanceR < wallRight) {
-       /* if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
-            
-            errorP = LEFT_MID_VAL - distanceL;
-               
-        }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
-            
-            errorP = distanceR - RIGHT_MID_VAL;
-        }*/
         
         errorP = distanceL - distanceR + 2.30f;
-        //errorD = errorP - oldErrorP; 
+
+    }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;
+        
     }else{
         
         errorP = 0; 
         errorD = 0; 
     }
     
-    //errorD = errorP - oldErrorP;  
+    errorD = errorP - oldErrorP;  
     
     oldErrorP = errorP;