Diff: main.cpp
- Revision:
- 20:20573f55a5fd
- Parent:
- 19:6cd6cc5c8b4c
- Child:
- 21:41997651337a
--- a/main.cpp Thu May 10 18:10:15 2018 +0000
+++ b/main.cpp Fri May 11 13:12:28 2018 +0000
@@ -73,28 +73,6 @@
start = !start;
}
-//Return to last junction
-void reverseToJunction(int junc, int &r, int route[]) {
-
- do {
-
- //invert rotation
- if (route[r] == LEFT) {
-
- route[r] = RIGHT;
-
- }else if (route[r] == RIGHT) {
-
- route[r] = LEFT;
- }
-
- //motion.runTask(route,r,true);
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
- route[r] = 0;
- r--;
- } while (junc < r );
-}
-
//------------------------------------------------------------------------------
int main() {
@@ -154,71 +132,38 @@
}
}
- //printf("Kreuzung: %d Schritt: %d\n", j, r);
+// printf("Kreuzung: %d Schritt: %d\n", j, r);
}
+
//No wall left
if (lWall == 0) {
if (route[r] == LEFT) {
route[r] = MOVE;
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
- r++;
-
- motion.rotateL();
- motion.scanMove();
- // motion.stop();
-
- }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]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateL();
motion.scanMove();
- //motion.stop();
+
+ }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();
+ motion.scanMove();
}else if (route[r] == RIGHT) {
// Kreuzung führt zu Sackgassen -> löschen
- junction[j] = 0; /*Änderung alex*/
- //if (j > 0) {j--;}
- //printf("Kreuzung %d Schritt %d geloscht\n", j, r);
+ junction[j] = 0;
+// printf("Kreuzung %d Schritt %d geloscht\n", j, r);
j -= 1;
- //reverseToJunction(junction[j], r, route);
-
- /*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;
-
- 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]);
- */
do {
@@ -233,79 +178,47 @@
}
motion.runTask(route,r,true,junction[j]);
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]); //At work!!!!!
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
} while (junction[j] < r ) ;
motion.stop();
- //printf("Loop stop\n");
+// printf("Loop stop\n");
}else{
route[r] = LEFT;
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateL();
motion.scanMove();
- // motion.stop();
}
- //No wall center
+
+ //No wall center
}else if (cWall == 0) {
if (route[r] == LEFT) {
route[r] = RIGHT;
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.scanMove();
- // motion.stop();
}else if (route[r] == MOVE) {
- junction[j] = 0; /*Änderung alex*/
- //if (j > 0) {j--;}
- //printf("Kreuzung %d Schritt %d geloscht\n", j, r);
+ junction[j] = 0;
+// printf("Kreuzung %d Schritt %d geloscht\n", j, r);
j -= 1;
- //reverseToJunction(junction[j], r, route);
-
- /*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;
-
- 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]);
- */
do {
@@ -320,41 +233,41 @@
}
motion.runTask(route,r,true,junction[j]);
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
} while (junction[j] < r );
motion.stop();
- //printf("Loop stop\n");
+// printf("Loop stop\n");
}else{
route[r] = MOVE;
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.scanMove();
}
+
//No wall right
}else if (rWall == 0) {
route[r] = RIGHT;
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateR();
motion.scanMove();
- // motion.stop();
//Dead end routine
}else if ((lWall + cWall + rWall) == 3) {
motion.rotate180();
- //printf("Sackgasse Schritt: %d\n", r);
+// printf("Sackgasse Schritt: %d\n", r);
r--;
t1.reset();
t1.start();
@@ -362,14 +275,13 @@
t1.stop();
//Return to last junction
while (junction[j] <= r ) {
- //for (int i = r; i >= junction[j]; i--
if (junction[j] == r) {
switch (route[r]) {
case MOVE:
motion.runTask(route,r,true, junction[j]);
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r--;
break;
case LEFT:
@@ -397,7 +309,7 @@
}
//Run tasks in declining order
motion.runTask(route,r,true, junction[j]);
- //printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
@@ -405,26 +317,9 @@
r++;
}
+ //finish line check
if (motion.finish() == 1) {
- /*while(r > 0) {
- //Zum Start zurückfahren
- if (route[r] == LEFT) {
-
- route[r] = RIGHT;
-
- }else if (route[r] == RIGHT) {
-
- route[r] = LEFT;
- }
-
- motion.runTask(route[r]);
- r--;
- }
-
- motion.runTask(route[r])
- */
-
Ziel = 1;
r = 0;
start = 0;
@@ -436,23 +331,12 @@
}
- /**
- *
+ /*
* Speed run
- *
*/
while ( start == 1 && Ziel == 1 ) {
- /*if (route[r] == route[r+1] && route[r] == route[r+2]) {
- //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,false,junction[j]);
r++;
Helvijs Kiselis
