Diff: main.cpp

Revision:

8:8131269dc46e

Parent:

7:22392ed60534

Child:

9:a01f90f88920

--- a/main.cpp	Fri Apr 27 13:07:13 2018 +0000
+++ b/main.cpp	Fri Apr 27 16:28:34 2018 +0000
@@ -108,7 +108,7 @@
 //Init
     
     int route[200] = {0};
-    int r = 0;
+    volatile int r = 0;
     
     int junction[20] = {0};
     int j = 0;
@@ -152,9 +152,13 @@
         //Junction Check
         if ((lWall + cWall + rWall) < 2) {
             
-            if (junction[j] > 0) {j++;}
-            junction[j] = r;
-            printf("Kreuzung Schritt: %d\n", r);
+            
+            if (junction[j-1] != r) {
+                junction[j] = r;
+                j++;
+            }
+            
+            printf("Kreuzung: %d Schritt: %d\n", j, r);
         }
         //No wall left
         if (lWall == 0) {
@@ -184,13 +188,34 @@
                
             }else if (route[r] == RIGHT) {
                 // Kreuzung führt zu Sackgassen -> löschen
-                junction[j] = 0;
+                junction[j-1] = 0;
                 //if (j > 0) {j--;}
                 printf("Kreuzung %d Schritt %d geloscht\n", j, r);
                 j--;
                 //reverseToJunction(junction[j], r, route);
                 
-                while (junction[r] < r ) {
+                /*for (int i = r; i > junction[j]; i--) {
+                    r = i;
+                    //invert rotation
+                    if (route[r] == LEFT) {
+                        
+                        route[r] = RIGHT;
+                        
+                    }else if (route[r] == RIGHT) {
+                        
+                        route[r] = LEFT;
+                    }
+                    
+                    motion.runTask(route[r]);
+                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+                    route[r] = 0;          
+                }
+                printf("Loop fertig\n");
+                r--; 
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+                */
+                
+                while (junction[j-1] < r ) {
                     
                     //invert rotation
                     if (route[r] == LEFT) {
@@ -203,9 +228,10 @@
                     }
                     
                     motion.runTask(route[r]);
+                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                     route[r] = 0;
                     r--;
-                }  
+                } 
             }else{
                 
                 route[r] = LEFT;
@@ -237,13 +263,34 @@
                 
             }else if (route[r] == MOVE) {
                 
-                junction[j] = 0;
+                junction[j-1] = 0;
                 //if (j > 0) {j--;}
                 printf("Kreuzung %d Schritt %d geloscht\n", j, r);
                 j--;
                 //reverseToJunction(junction[j], r, route);
                 
-                while (junction[r] < r ) {
+                /*for (int i = r; i > junction[j]; i--) {
+                    r = i;
+                    //invert rotation
+                    if (route[r] == LEFT) {
+                        
+                        route[r] = RIGHT;
+                        
+                    }else if (route[r] == RIGHT) {
+                        
+                        route[r] = LEFT;
+                    }
+                    
+                    motion.runTask(route[r]);
+                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+                    route[r] = 0;          
+                }
+                printf("Loop fertig\n");
+                r--; 
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+                */
+                
+                while (junction[j-1] < r ) {
                     
                     //invert rotation
                     if (route[r] == LEFT) {
@@ -256,6 +303,7 @@
                     }
                     
                     motion.runTask(route[r]);
+                    printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                     route[r] = 0;
                     r--;
                 }  
@@ -289,15 +337,17 @@
             printf("Sackgasse Schritt: %d\n", r);
             r--;
             //Return to last junction
-            while (junction[j] <= r ) {
-                
-                if (junction[j] == r) {
+            while (junction[j-1] <= r ) {
+            //for (int i = r; i >= junction[j]; i--
+            
+                if (junction[j-1] == r) {
                     
                     switch (route[r]) {
                         case MOVE: 
                             motion.runTask(route[r]);
                             printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                             r--;
+                            break;
                         case LEFT:
                             r--;
                             break;