Diff: Motion.cpp

Revision:

31:2c54f8304ef5

Parent:

30:bdb8c92434a0

Child:

32:e984b7959cb0

--- a/Motion.cpp	Tue May 22 16:40:36 2018 +0000
+++ b/Motion.cpp	Tue May 22 23:35:53 2018 +0000
@@ -85,20 +85,26 @@
             
             //Correct distance from wall
             }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) {
-                countsLOld += 500;
-                countsROld -= 500; 
+                countsLOld += 1000;
+                countsROld -= 1000; 
                   
             //Stop after certain distance if side wall and front wall missing
-            }else if ( deceleration == true && speedRun == false && ( distanceL > 60.0f || distanceR > 60.0f ) && distanceC > 200.0f ) {
+            }else if ( deceleration == true && speedRun == false && ( distanceL > 80.0f || distanceR > 80.0f ) && distanceC > 200.0f ) {
+
+                controller.counterReset();
                 countsLOld = counterLeft.read();
                 countsROld = counterRight.read();
+                countsL = counterLeft.read();
+                countsR = counterRight.read();
                 
-                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f || (countsR - countsROld) > -0.5f*MOVE_DIST) {
+                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 400.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 400.0f) {
                     countsL = counterLeft.read();
                     countsR = counterRight.read(); 
                     
-                    if (speedRun == true) accel(RUN_SPEED);
-                    else accel(MOVE_SPEED);
+                    deceleration = true;
+                    acceleration = false;
+                    
+                    accel(MOVE_SPEED);
                     control();
                 }
                 stop();
@@ -110,6 +116,8 @@
         t.stop();
         t.reset();
         lastMove = false;
+        acceleration = false;
+        deceleration = false;
 }
 
 /**
@@ -146,9 +154,13 @@
                 break;    
             
             //Stop after certain distance if side wall missing
-            }else if ( (distanceL > 60.0f || distanceR > 60.0f) && lastMove == false && deceleration == true && distanceC < 190.0f) {
+            }else if ( (distanceL > 80.0f || distanceR > 80.0f) && lastMove == false && deceleration == true && distanceC < 190.0f) {
+
+                controller.counterReset();
                 countsLOld = counterLeft.read();
                 countsROld = counterRight.read();
+                countsL = counterLeft.read();
+                countsR = counterRight.read();
                 
                 while ((countsL - countsLOld)  <  600.0f || (countsR - countsROld) > -600.0f) {
                     countsL = counterLeft.read();
@@ -170,6 +182,8 @@
         t.stop();
         t.reset();
         lastMove = false;
+        acceleration = false;
+        deceleration = false;
 }
 
 /**
@@ -186,6 +200,8 @@
         countsL = counterLeft.read();
         countsR = counterRight.read();
         
+        bool sideWalls = false;
+        
         t.start();
         
         while ((countsL - countsLOld)  <  MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
@@ -196,6 +212,9 @@
             distanceL = irSensorL.readL();
             distanceR = irSensorR.readR();
             
+            //for distance correcture with side wall
+            if (distanceL < 80.0f && distanceR < 80.0f) sideWalls = true;
+            
             if (enableMotorDriver == 0) {
                 enableMotorDriver = 1;
             }
@@ -226,6 +245,21 @@
             }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
                 countsLOld += 500;
                 countsROld += 500;   
+            
+            //Stop after certain distance if side wall and front wall missing
+            }else if (sideWalls == true && speedRun == false && ( distanceL > 80.0f || distanceR > 80.0f ) && distanceC > 200.0f ) {
+                countsLOld = counterLeft.read();
+                countsROld = counterRight.read();
+                
+                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 320.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 320.0f) {
+                    countsL = counterLeft.read();
+                    countsR = counterRight.read(); 
+                    
+                    if (speedRun == true) accel(RUN_SPEED);
+                    else accel(MOVE_SPEED);
+                    control();
+                }
+                break;
             }
         } 
          
@@ -484,7 +518,7 @@
             case 1:
             
                 //Acceleration
-                if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) {     //if next move() and previous no move() and step no junction
+                if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction && task-1 != junction) {     //if next move() and previous no move() and step no junction
                     
                     acceleration = true;
                     longMove = true;