Diff: main.cpp

Revision:

4:932eb2d29206

Parent:

3:076dd7ec7eb4

Child:

5:e2c0a4388d85

--- a/main.cpp	Fri Apr 20 13:37:21 2018 +0000
+++ b/main.cpp	Mon Apr 23 12:14:54 2018 +0000
@@ -1,4 +1,4 @@
-/** 
+ /** 
  *      Micromouse PES2
  *
  *      Suchfahrtalg. + Schnellfahrtalg.  
@@ -8,14 +8,14 @@
  /* todo:
  
     - Regler gerade Fahrt
-        > Mitte Werte L/R
-    - Suchfahrt testen
-        > Tresholds bestimmen
+        > Rotationen
     - gespeicherten Weg fahren
     - Problem: Links beschleunigt schneller
-    - Ziellinie
+    
+    Optimieren:
     - Beschleunigung
     - über längere Strecke schneller fahren
+    - Abbiegen
     
 */
 #include <mbed.h>
@@ -68,7 +68,6 @@
     const int LEFT = 2;
     const int RIGHT = 3;
     const int SPEED = 4;
-    const int STOP = 5;
     
     //Sensor tresholds [mm]
     const float thresholdL = 80;
@@ -112,7 +111,7 @@
     int route[50] = {0};
     int r = 0;
     
-    int junction[5] = {0};
+    int junction[10] = {0};
     int j = 0;
     
     short lWall;
@@ -156,6 +155,7 @@
             
             if (junction[j] > 0) {j++;}
             junction[j] = r;
+            printf("Kreuzung Schritt: %d\n", r);
         }
         //No wall left
         if (lWall == 0) {
@@ -163,6 +163,7 @@
             if (route[r] == LEFT) {
                 
                 route[r] = MOVE;
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 r++;
                 
                 motion.rotateL();
@@ -172,8 +173,10 @@
             }else if (route[r] == MOVE) {
                 
                 route[r] = RIGHT;
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 r++;
                 route[r] = MOVE;
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 r++;
                 
                 motion.rotateL();
@@ -183,16 +186,21 @@
             }else if (route[r] == RIGHT) {
                 // Kreuzung führt zu Sackgassen -> löschen
                 junction[j] = 0;
-                if (j > 0) {j--;}
+                //if (j > 0) {j--;}
+                printf("Kreuzung %d Schritt %d geloscht\n", j, r);
+                j--;
                 reverseToJunction(junction[j], r, route);
                  
             }else{
                 
                 route[r] = LEFT;
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 r++;
                 route[r] = MOVE;
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 r++;
                 
+                
                 motion.rotateL();
                 motion.scanMove();
                 motion.stop();
@@ -203,8 +211,10 @@
             if (route[r] == LEFT) {
                 
                 route[r] = RIGHT;
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 r++;
                 route[r] = MOVE;
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 r++;
             
                 motion.scanMove();
@@ -213,12 +223,15 @@
             }else if (route[r] == MOVE) {
                 
                 junction[j] = 0;
-                if (j > 0) {j--;}
+                //if (j > 0) {j--;}
+                printf("Kreuzung %d Schritt %d geloscht\n", j, r);
+                j--;
                 reverseToJunction(junction[j], r, route);
                 
             }else{
                 
                 route[r] = MOVE;
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 r++;
             
                 motion.scanMove();
@@ -228,8 +241,10 @@
         }else if (rWall == 0) {
             
             route[r] = RIGHT;
+            printf("Schritt: %d, Befehl: %d\n", r, route[r]);
             r++;
             route[r] = MOVE;
+            printf("Schritt: %d, Befehl: %d\n", r, route[r]);
             r++;
             
             motion.rotateR();
@@ -240,24 +255,34 @@
         }else if ((lWall + cWall + rWall) == 3) {
             
             motion.rotate180();
+            printf("Sackgasse Schritt: %d\n", r);
             r--;
             //Return to last junction
-            while (junction[j] < r ) {
+            while (junction[j] <= r ) {
                 
+                if (junction[j] == r && (route[r] == LEFT || route[r] == RIGHT)) {
+                        
+                    //route[r] = 0;
+                    r--;
+                
+                }else{
                 //invert rotation
-                if (route[r] == LEFT) {
-                    
-                    route[r] = RIGHT;
-                    
-                }else if (route[r] == RIGHT) {
-                    
-                    route[r] = LEFT;
+                    if (route[r] == LEFT) {
+                        
+                        route[r] = RIGHT;
+                        
+                    }else if (route[r] == RIGHT) {
+                        
+                        route[r] = LEFT;
                 }
                 //Run tasks in declining order
                 motion.runTask(route[r]);
+                printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 route[r] = 0;
                 r--;
+                }
             }
+            r++;
         }
 
         if (motion.finish() == 1) {
@@ -305,6 +330,7 @@
         
         if (route[r] == 0) {
             //Weg fertig
+            motion.stop();
             start = 0;    
         }    
     }