Diff: Motion.cpp

Revision:

18:3309329d5f42

Parent:

17:8a8758bfe3c5

Child:

19:6cd6cc5c8b4c

--- a/Motion.cpp	Mon May 07 18:28:34 2018 +0000
+++ b/Motion.cpp	Mon May 07 19:52:54 2018 +0000
@@ -416,15 +416,15 @@
         controller.setDesiredSpeedRight(-actSpeed - totalError);
 }
 
-void Motion::runTask(int path[],int task, bool reverse) {
+void Motion::runTask(int path[],int task, bool reverse, int junction) {
       
         switch(path[task]) {
             
             case 1:
-                if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task]) {
+                if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) {
                     acceleration = true;
                     deceleration = false;
-                }else if (reverse == false && path[task+1] == path[task] && path[task-1] != path[task]) {
+                }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0)) {
                     acceleration = true;
                     deceleration = false;
                 }else{
@@ -433,7 +433,7 @@
                     avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
                 }
             
-                if (reverse == true && path[task-1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
+                if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) {
                     deceleration = true;
                     acceleration = false;
                 }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {