Diff: main.cpp
- Revision:
- 9:a01f90f88920
- Parent:
- 8:8131269dc46e
- Child:
- 10:84534846e1f1
- Child:
- 11:2960fc540616
--- a/main.cpp Fri Apr 27 16:28:34 2018 +0000
+++ b/main.cpp Sat Apr 28 12:29:20 2018 +0000
@@ -81,9 +81,9 @@
}
//Return to last junction
-void reverseToJunction(int junc, int& r, int route[]) {
+void reverseToJunction(int junc[], int j, int r, int route[]) {
- while (junc < r ) {
+ while (junc[j-1] < r ) {
//invert rotation
if (route[r] == LEFT) {
@@ -96,12 +96,12 @@
}
motion.runTask(route[r]);
+ printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
}
-
-
+
//------------------------------------------------------------------------------
int main() {
@@ -120,9 +120,9 @@
short Ziel = 0;
//loop
-while(1) {
+ while(1) {
- button.fall(&press); //User button einlesen
+ button.fall(&press); //User button einlesen
/**
*
@@ -130,127 +130,196 @@
*
*/
- while(start == 1 && Ziel == 0) {
+ while(start == 1 && Ziel == 0) {
- /**
- * Entscheidung und Bewegung
- */
+ /**
+ * Entscheidung und Bewegung
+ */
- float distanceL = irSensorL.readL();
- float distanceC = irSensorC.readC();
- float distanceR = irSensorR.readR();
+ float distanceL = irSensorL.readL();
+ float distanceC = irSensorC.readC();
+ float distanceR = irSensorR.readR();
- //Wall check
- if (distanceL < thresholdL) lWall = 1;
- else lWall = 0;
- if (distanceC < thresholdC) cWall = 1;
- else cWall = 0;
- if (distanceR < thresholdR) rWall = 1;
- else rWall = 0;
+ //Wall check
+ if (distanceL < thresholdL) lWall = 1;
+ else lWall = 0;
+ if (distanceC < thresholdC) cWall = 1;
+ else cWall = 0;
+ if (distanceR < thresholdR) rWall = 1;
+ else rWall = 0;
- //Junction Check
- if ((lWall + cWall + rWall) < 2) {
+ //Junction Check
+ if ((lWall + cWall + rWall) < 2) {
- if (junction[j-1] != r) {
- junction[j] = r;
- j++;
- }
+ if (junction[j-1] != r) {
+ junction[j] = r;
+ j++;
+ }
- printf("Kreuzung: %d Schritt: %d\n", j, r);
- }
+ printf("Kreuzung: %d Schritt: %d\n", j, r);
+ }
//No wall left
- if (lWall == 0) {
+ if (lWall == 0) {
- if (route[r] == LEFT) {
+ if (route[r] == LEFT) {
- route[r] = MOVE;
- 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();
+ motion.rotateL();
+ motion.scanMove();
+ motion.stop();
- }else if (route[r] == MOVE) {
+ }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++;
+ 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();
- motion.stop();
+ motion.rotateL();
+ motion.scanMove();
+ motion.stop();
- }else if (route[r] == RIGHT) {
- // Kreuzung führt zu Sackgassen -> löschen
- junction[j-1] = 0;
- //if (j > 0) {j--;}
- printf("Kreuzung %d Schritt %d geloscht\n", j, r);
- j--;
- //reverseToJunction(junction[j], r, route);
+ }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);
+ j--;
+ //reverseToJunction(junction, j, r, route);
- /*for (int i = r; i > junction[j]; i--) {
- r = i;
- //invert rotation
- if (route[r] == LEFT) {
+ /*for (int i = r; i > junction[j]; i--) {
+ r = i;
+ //invert rotation
+ if (route[r] == LEFT) {
+
+ route[r] = RIGHT;
- route[r] = RIGHT;
+ }else if (route[r] == RIGHT) {
- }else if (route[r] == RIGHT) {
-
- route[r] = LEFT;
+ route[r] = LEFT;
+ }
+
+ motion.runTask(route[r]);
+ printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ route[r] = 0;
}
-
- motion.runTask(route[r]);
+ printf("Loop fertig\n");
+ 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 ) {
+ while (junction[j-1] < r ) {
- //invert rotation
- if (route[r] == LEFT) {
+ //invert rotation
+ if (route[r] == LEFT) {
- route[r] = RIGHT;
+ route[r] = RIGHT;
+
+ }else if (route[r] == RIGHT) {
- }else if (route[r] == RIGHT) {
-
- route[r] = LEFT;
+ route[r] = LEFT;
+ }
+
+ motion.runTask(route[r]);
+ printf("Schritt: %d, Befehl: %d\n", r, route[r]); //At work!!!!!
+ route[r] = 0;
+ r--;
}
-
- motion.runTask(route[r]);
+ printf("loop end");
+ }else{
+ printf("else");
+ route[r] = LEFT;
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
- route[r] = 0;
- r--;
- }
- }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++;
+ r++;
+ route[r] = MOVE;
+ printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ r++;
- motion.rotateL();
- motion.scanMove();
- motion.stop();
- }
+ motion.rotateL();
+ motion.scanMove();
+ motion.stop();
+ }
//No wall center
- }else if (cWall == 0) {
+ }else if (cWall == 0) {
+
+ 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++;
- if (route[r] == LEFT) {
+ 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);
+ j--;
+ //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;
+ }
+ 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) {
+
+ 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;
+ r--;
+ }
+
+ }else{
+
+ route[r] = MOVE;
+ 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]);
r++;
@@ -258,154 +327,86 @@
printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
+ motion.rotateR();
motion.scanMove();
motion.stop();
-
- }else if (route[r] == MOVE) {
-
- junction[j-1] = 0;
- //if (j > 0) {j--;}
- printf("Kreuzung %d Schritt %d geloscht\n", j, r);
- j--;
- //reverseToJunction(junction[j], r, route);
+
+ //Dead end routine
+ }else if ((lWall + cWall + rWall) == 3) {
+
+ motion.rotate180();
+ printf("Sackgasse Schritt: %d\n", r);
+ r--;
+ //Return to last junction
+ 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;
+ case RIGHT:
+ r--;
+ break;
+ }
- /*for (int i = r; i > junction[j]; i--) {
- r = i;
- //invert rotation
- if (route[r] == LEFT) {
+ }else{
+ //invert rotation
+ if (route[r] == LEFT) {
+
+ route[r] = RIGHT;
+
+ }else if (route[r] == RIGHT) {
- route[r] = RIGHT;
-
- }else if (route[r] == RIGHT) {
-
- route[r] = LEFT;
+ 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) {
+
+ /*while(r > 0) {
+ //Zum Start zurückfahren
+ if (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 ) {
+ route[r] = RIGHT;
- //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;
- r--;
- }
-
- }else{
-
- route[r] = MOVE;
- 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]);
- r++;
- route[r] = MOVE;
- 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);
- r--;
- //Return to last junction
- while (junction[j-1] <= r ) {
- //for (int i = r; i >= junction[j]; i--
+ r--;
+ }
- 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;
- case RIGHT:
- r--;
- break;
- }
-
- }else{
- //invert rotation
- 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) {
-
- /*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])
+ motion.runTask(route[r])
*/
- Ziel = 1;
- r = 0;
- start = 0;
- controller.counterReset();
- }else{
- Ziel = 0;
- }
+ Ziel = 1;
+ r = 0;
+ start = 0;
+ controller.counterReset();
+ }else{
+ Ziel = 0;
+ }
- }
+ }
/**
*
@@ -413,33 +414,29 @@
*
*/
- while ( start == 1 && Ziel == 1 ) {
+ 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;
- }*/
+ /*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();
+ float distanceL = irSensorL.readL();
+ float distanceC = irSensorC.readC();
+ float distanceR = irSensorR.readR();
- motion.runTask(route[r]);
- r++;
+ motion.runTask(route[r]);
+ r++;
- if (route[r] == 0) {
- //Weg fertig
- motion.stop();
- start = 0;
- }
- }
+ if (route[r] == 0) {
+ //Weg fertig
+ motion.stop();
+ start = 0;
+ }
+ }
-}
-}
-
-
-
-
\ No newline at end of file
+ }
+}
\ No newline at end of file
Helvijs Kiselis
