Diff: Motion.cpp

Revision:

24:11c5fb5280eb

Parent:

23:accd07ca2da7

Child:

25:8854037f6336

--- a/Motion.cpp	Tue May 15 13:35:50 2018 +0000
+++ b/Motion.cpp	Tue May 15 15:11:37 2018 +0000
@@ -128,7 +128,7 @@
             if (distanceC < 101.0f && lastMove == false) {
                 stop();
                 break;    
-            }else if (distanceC < 40.0 && lastMove == true) {
+            }else if (distanceC < 40.0f && lastMove == true) {
                 stop();
                 break;
             }
@@ -375,8 +375,8 @@
 
 void Motion::control() {
         
-        float wallLeft = 48.73f;
-        float wallRight = 51.03f;
+        float wallLeft = 47.0f; //48.73
+        float wallRight = 44.0f; //51.03
         
         distanceL = irSensorL.readL();
         distanceR = irSensorR.readR();
@@ -384,16 +384,32 @@
         
         if (distanceL < wallLeft && distanceR < wallRight) {
             
-            errorP = distanceL - distanceR + 2.30f;
+            errorP = distanceL - distanceR - 3.0f;
     
-        }else if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
+        }/* 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 if (distanceL < wallLeft && distanceR > wallRight) {
+                
+            errorP = distanceL - wallLeft;
+            
+        }else if (distanceL < wallLeft+15.0f && distanceL > wallLeft && distanceR > wallRight) {
+                
+            errorP = distanceL - wallLeft;
+            
+        }else if (distanceL > wallLeft && distanceR < wallRight) {
+                
+            errorP = wallRight - distanceR;
+            
+        }else if (distanceR < wallRight+15.0f && distanceL > wallLeft && distanceR > wallRight) {
+                
+            errorP = wallRight - distanceR;
+            
         }else{
             
             errorP = 0; 
@@ -531,7 +547,7 @@
             
         }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) {
             
-            actSpeed =  55.0f;
+            actSpeed =  50.0f;
             
         }else if (avgSpeed < targetSpeed*2.8f && acceleration == true) {
             
@@ -540,6 +556,11 @@
         }else if (avgSpeed > targetSpeed && deceleration == true) {
         
             actSpeed = targetSpeed*2.8f - ACCEL_CONST * t.read()*60.0f;
-        }        
+            
+        }else if (avgSpeed < targetSpeed && deceleration == true) {
+            
+            actSpeed = 50.0f;
+            
+        }    
 }