Diff: Motion.cpp

Revision:

26:f964408401fa

Parent:

25:8854037f6336

Child:

27:f111ba194412

--- a/Motion.cpp	Tue May 15 15:29:02 2018 +0000
+++ b/Motion.cpp	Tue May 15 17:57:59 2018 +0000
@@ -6,7 +6,7 @@
 
 const float Motion::LEFT_MID_VAL = 40.73f; //44.73
 const float Motion::RIGHT_MID_VAL = 43.03f; //47.03
-const float Motion::KP = 2.5;
+const float Motion::KP = 2.0;
 const float Motion::KD = 0.0f;
 const int Motion::MOVE_DIST = 1605;
 const float Motion::MOVE_SPEED = 50.0f;
@@ -376,17 +376,17 @@
 void Motion::control() {
         
         float wallLeft = 47.0f; //48.73
-        float wallRight = 44.0f; //51.03
+        float wallRight = 44.0f; //51.03f;
         
         distanceL = irSensorL.readL();
         distanceR = irSensorR.readR();
         
         
-        if (distanceL < wallLeft && distanceR < wallRight) {
+        /*if (distanceL < wallLeft-1.0f && distanceR < wallRight-1.0f) {
             
             errorP = distanceL - distanceR - 3.0f;
     
-        }/* if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
+        }else if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
                 
             errorP = distanceL - LEFT_MID_VAL;
             
@@ -394,11 +394,7 @@
                 
             errorP = RIGHT_MID_VAL - distanceR;
             
-        }*/else if (distanceL < wallLeft && distanceR > wallRight) {
-                
-            errorP = distanceL - wallLeft;
-            
-        }else if (distanceL < wallLeft+15.0f && distanceL > wallLeft && distanceR > wallRight) {
+        }*/ if (distanceL < wallLeft && distanceR > wallRight) {
                 
             errorP = distanceL - wallLeft;
             
@@ -406,7 +402,11 @@
                 
             errorP = wallRight - distanceR;
             
-        }else if (distanceR < wallRight+15.0f && distanceL > wallLeft && distanceR > wallRight) {
+        }else if (distanceL < wallLeft+10.0f && distanceL > wallLeft && distanceR > wallRight) {
+                
+            errorP = distanceL - wallLeft;
+            
+        }else if (distanceR < wallRight+10.0f && distanceL > wallLeft && distanceR > wallRight) {
                 
             errorP = wallRight - distanceR;
             
@@ -420,7 +420,7 @@
         
         oldErrorP = errorP;
         
-        if (abs(errorP) > 1.0f && abs(errorP) < 80.0f) {
+        if (abs(errorP) < 80.0f) {
             totalError = KP*errorP + KD*errorD;
         }/*else{
             totalError = 0;
@@ -547,7 +547,7 @@
             
         }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) {
             
-            actSpeed =  51.0f;
+            actSpeed =  55.0f;
             
         }else if (avgSpeed < targetSpeed*2.8f && acceleration == true) {