Diff: main.cpp

Revision:

7:22392ed60534

Parent:

6:4868f789c223

Child:

8:8131269dc46e

--- a/main.cpp	Tue Apr 24 15:35:24 2018 +0000
+++ b/main.cpp	Fri Apr 27 13:07:13 2018 +0000
@@ -8,12 +8,11 @@
  /* todo:
  
     - Regler gerade Fahrt
-        > Rotationen
     - gespeicherten Weg fahren
-    - Problem: Links beschleunigt schneller
     
     Optimieren:
     - Beschleunigung
+    - Zentrieren
     - über längere Strecke schneller fahren
     - Abbiegen
     
@@ -82,7 +81,7 @@
 }
 
 //Return to last junction
-void reverseToJunction(int& junc, int& r, int route[]) {
+void reverseToJunction(int junc, int& r, int route[]) {
     
         while (junc < r ) {
             
@@ -189,8 +188,24 @@
                 //if (j > 0) {j--;}
                 printf("Kreuzung %d Schritt %d geloscht\n", j, r);
                 j--;
-                reverseToJunction(junction[j], r, route);
-                 
+                //reverseToJunction(junction[j], r, route);
+                
+                while (junction[r] < r ) {
+                    
+                    //invert rotation
+                    if (route[r] == LEFT) {
+                        
+                        route[r] = RIGHT;
+                        
+                    }else if (route[r] == RIGHT) {
+                        
+                        route[r] = LEFT;
+                    }
+                    
+                    motion.runTask(route[r]);
+                    route[r] = 0;
+                    r--;
+                }  
             }else{
                 
                 route[r] = LEFT;
@@ -213,9 +228,9 @@
                 route[r] = RIGHT;
                 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
                 r++;
-                /*route[r] = MOVE;
+                route[r] = MOVE;
                 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
-                r++;*/
+                r++;
             
                 motion.scanMove();
                 motion.stop();
@@ -226,7 +241,24 @@
                 //if (j > 0) {j--;}
                 printf("Kreuzung %d Schritt %d geloscht\n", j, r);
                 j--;
-                reverseToJunction(junction[j], r, route);
+                //reverseToJunction(junction[j], r, route);
+                
+                while (junction[r] < r ) {
+                    
+                    //invert rotation
+                    if (route[r] == LEFT) {
+                        
+                        route[r] = RIGHT;
+                        
+                    }else if (route[r] == RIGHT) {
+                        
+                        route[r] = LEFT;
+                    }
+                    
+                    motion.runTask(route[r]);
+                    route[r] = 0;
+                    r--;
+                }  
                 
             }else{
                 
@@ -259,7 +291,7 @@
             //Return to last junction
             while (junction[j] <= r ) {
                 
-                if (junction[j] == r && (route[r] == LEFT || route[r] == RIGHT)) {
+                if (junction[j] == r) {
                     
                     switch (route[r]) {
                         case MOVE: 
@@ -308,13 +340,16 @@
                 }
                 
                 motion.runTask(route[r]);
-                route[r] = 0;
                 r--;    
-            }*/
+            }
+            
+            motion.runTask(route[r])
+            */
             
             Ziel = 1;  
             r = 0;  
             start = 0;
+            controller.counterReset();
         }else{
             Ziel = 0;
         }
@@ -334,6 +369,11 @@
             //Auf längere Strecke schneller fahren
             route[r+1] = SPEED;  
         }*/
+        
+        float distanceL = irSensorL.readL();
+        float distanceC = irSensorC.readC();
+        float distanceR = irSensorR.readR();
+        
         motion.runTask(route[r]);
         r++;