Helvijs Kiselis
/
Micromouse
Algorithmus
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++;