Diff: main.cpp
- Revision:
- 10:84534846e1f1
- Parent:
- 9:a01f90f88920
--- a/main.cpp Sat Apr 28 12:29:20 2018 +0000
+++ b/main.cpp Mon Apr 30 13:28:28 2018 +0000
@@ -80,38 +80,16 @@
start = !start;
}
-//Return to last junction
-void reverseToJunction(int junc[], int j, int r, int route[]) {
-
- while (junc[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--;
- }
-}
-
//------------------------------------------------------------------------------
int main() {
//Init
- int route[200] = {0};
+ volatile int route[200] = {0};
volatile int r = 0;
- int junction[20] = {0};
- int j = 0;
+ volatile int junction[20] = {0};
+ volatile int j = 0;
short lWall;
short cWall;
@@ -124,18 +102,11 @@
button.fall(&press); //User button einlesen
- /**
- *
- * Search run
- *
- */
+/*---------------------------Search run---------------------------------------*/
while(start == 1 && Ziel == 0) {
- /**
- * Entscheidung und Bewegung
- */
-
+/*---------------------Entscheidung und Bewegung------------------------------*/
float distanceL = irSensorL.readL();
float distanceC = irSensorC.readC();
@@ -157,16 +128,14 @@
junction[j] = r;
j++;
}
+ }
- printf("Kreuzung: %d Schritt: %d\n", j, r);
- }
- //No wall left
+ //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();
@@ -176,10 +145,8 @@
}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();
@@ -189,32 +156,8 @@
}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) {
-
- 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
@@ -228,18 +171,15 @@
}
motion.runTask(route[r]);
- printf("Schritt: %d, Befehl: %d\n", r, route[r]); //At work!!!!!
route[r] = 0;
r--;
}
- printf("loop end");
+
}else{
printf("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++;
@@ -253,10 +193,8 @@
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();
@@ -265,32 +203,8 @@
}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
@@ -304,7 +218,6 @@
}
motion.runTask(route[r]);
- printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
@@ -312,7 +225,6 @@
}else{
route[r] = MOVE;
- printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.scanMove();
@@ -321,10 +233,8 @@
}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();
@@ -335,18 +245,15 @@
}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:
@@ -369,7 +276,6 @@
}
//Run tasks in declining order
motion.runTask(route[r]);
- printf("Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
@@ -379,48 +285,24 @@
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;
controller.counterReset();
+
}else{
+
Ziel = 0;
+
}
}
- /**
- *
- * Speed run
- *
- */
+/*------------------------------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();
@@ -434,9 +316,5 @@
start = 0;
}
}
-
-
-
-
}
}
\ No newline at end of file
Helvijs Kiselis
