Diff: main.cpp

Revision:

11:2960fc540616

Parent:

9:a01f90f88920

Child:

12:75d0291a9785

--- a/main.cpp	Sat Apr 28 12:29:20 2018 +0000
+++ b/main.cpp	Mon Apr 30 18:27:23 2018 +0000
@@ -107,11 +107,11 @@
 int main() {
 //Init
     
-    int route[200] = {0};
+    volatile int route[200] = {0};
     volatile int r = 0;
     
-    int junction[20] = {0};
-    int j = 0;
+    volatile int junction[20] = {0};
+    volatile int j = 0;
     
     short lWall;
     short cWall;
@@ -151,11 +151,14 @@
         
             //Junction Check
             if ((lWall + cWall + rWall) < 2) {
-            
-            
-                if (junction[j-1] != r) {
-                    junction[j] = r;
-                    j++;
+     
+                if (junction[j] != r) {
+                    if (junction[j] > 0) {
+                        j += 1;
+                        junction[j] = r;   
+                    }else{
+                        junction[j] = r;   
+                    }
                 }
             
                 printf("Kreuzung: %d Schritt: %d\n", j, r);
@@ -191,7 +194,7 @@
                     junction[j] = 0;                                            /*Änderung alex*/
                     //if (j > 0) {j--;}
                     printf("Kreuzung %d Schritt %d geloscht\n", j, r);
-                    j--;
+                    j -= 1;
                     //reverseToJunction(junction, j, r, route);
                 
                     /*for (int i = r; i > junction[j]; i--) {
@@ -208,14 +211,23 @@
                     
                         motion.runTask(route[r]);
                         printf("Schritt: %d, Befehl: %d\n", r, route[r]);
-                        route[r] = 0;          
+                        route[r] = 0;
+                        
+                        if ( r == junction[j] + 1) {
+                            while (controller.getSpeedLeft() > 0.0f) {
+                                motion.stop();   
+                            }   
+                        }         
+                        
                     }
+                    motion.stop();
+                    
                     printf("Loop fertig\n");
                     r--; 
                     printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                     */
                 
-                    while (junction[j-1] < r ) {
+                    do {
                     
                         //invert rotation
                         if (route[r] == LEFT) {
@@ -231,10 +243,12 @@
                         printf("Schritt: %d, Befehl: %d\n", r, route[r]);                   //At work!!!!!
                         route[r] = 0;
                         r--;
-                    }
-                    printf("loop end");
+                        
+                    } while (junction[j] < r ) ;
+                    motion.stop();
+
                 }else{
-                    printf("else");
+                    
                     route[r] = LEFT;
                     printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                     r++;
@@ -267,7 +281,7 @@
                     junction[j] = 0;                                            /*Änderung alex*/
                     //if (j > 0) {j--;}
                     printf("Kreuzung %d Schritt %d geloscht\n", j, r);
-                    j--;
+                    j -= 1;
                     //reverseToJunction(junction[j], r, route);
                 
                     /*for (int i = r; i > junction[j]; i--) {
@@ -284,14 +298,22 @@
                     
                         motion.runTask(route[r]);
                         printf("Schritt: %d, Befehl: %d\n", r, route[r]);
-                        route[r] = 0;          
+                        route[r] = 0; 
+                        
+                        if ( r == junction[j] + 1) {
+                            while (controller.getSpeedLeft() > 0.0f) {
+                                motion.stop();   
+                            }   
+                        }     
+                            
                     }
+                    motion.stop();
                     printf("Loop fertig\n");
                     r--; 
                     printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                     */
                 
-                    while (junction[j-1] < r ) {
+                    do {
                     
                         //invert rotation
                         if (route[r] == LEFT) {
@@ -303,11 +325,13 @@
                             route[r] = LEFT;
                         }
                     
-                            motion.runTask(route[r]);
-                            printf("Schritt: %d, Befehl: %d\n", r, route[r]);
-                            route[r] = 0;
-                            r--;
-                    }  
+                        motion.runTask(route[r]);
+                        printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+                        route[r] = 0;
+                        r--;
+                        
+                    } while (junction[j] < r );
+                    motion.stop();
                 
                 }else{
                 
@@ -338,10 +362,10 @@
                 printf("Sackgasse Schritt: %d\n", r);
                 r--;
                 //Return to last junction
-                while (junction[j-1] <= r ) {
+                while (junction[j] <= r ) {
                 //for (int i = r; i >= junction[j]; i--
             
-                    if (junction[j-1] == r) {
+                    if (junction[j] == r) {
                     
                         switch (route[r]) {
                             case MOVE: