Diff: Motion.cpp

Revision:

21:41997651337a

Parent:

20:20573f55a5fd

Child:

22:91526c8d15ba

--- a/Motion.cpp	Fri May 11 13:12:28 2018 +0000
+++ b/Motion.cpp	Sat May 12 16:30:18 2018 +0000
@@ -372,9 +372,9 @@
         
         if (abs(errorP) < 80.0f) {
             totalError = KP*errorP + KD*errorD;
-        }else{
+        }/*else{
             totalError = 0;
-        }
+        }*/
         
         controller.setDesiredSpeedLeft(actSpeed - totalError);
         controller.setDesiredSpeedRight(-actSpeed - totalError);
@@ -387,9 +387,11 @@
             case 1:
                 if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) {
                     acceleration = true;
+                    longMove = true;
                     deceleration = false;
                 }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0)) {
                     acceleration = true;
+                    longMove = true;
                     deceleration = false;
                 }else{
                     acceleration = false; 
@@ -401,10 +403,12 @@
                     deceleration = true;
                     acceleration = false;
                     lastMove = true;
+                    longMove = false;
                 }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
                     deceleration = true;
                     acceleration = false;
                     lastMove = true;
+                    longMove = false;
                 }else{
                     deceleration = false;
                 }
@@ -443,6 +447,10 @@
                 
             actSpeed = ACCEL_CONST * t.read()*60.0f; 
             
+        }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) {
+            
+            actSpeed =  55.0f;
+            
         }else if (avgSpeed < targetSpeed*2.6f && acceleration == true) {
             
             actSpeed = 2.5f * t.read()*60.0f;