Diff: main.cpp
- Revision:
- 31:2c54f8304ef5
- Parent:
- 30:bdb8c92434a0
- Child:
- 32:e984b7959cb0
--- a/main.cpp Tue May 22 16:40:36 2018 +0000
+++ b/main.cpp Tue May 22 23:35:53 2018 +0000
@@ -54,6 +54,8 @@
volatile int start = 0;
+ const int debuging = 1;
+
const int MOVE = 1;
const int LEFT = 2;
const int RIGHT = 3;
@@ -63,9 +65,9 @@
const int EMPTY = 7;
//Sensor tresholds [mm]
- const float thresholdL = 60;
- const float thresholdR = 60;
- const float thresholdC = 100;
+ const float thresholdL = 70;
+ const float thresholdR = 70;
+ const float thresholdC = 80;
//------------------------------------------------------------------------------
@@ -129,7 +131,7 @@
}
}
-// printf("Kreuzung: %d Schritt: %d\n", j, r);
+ if (debuging == 1) printf("Kreuzung: %d Schritt: %d\n", j, r);
}
//No wall left
@@ -138,7 +140,7 @@
if (route[r] == LEFT) {
route[r] = MOVE;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateL();
@@ -147,10 +149,10 @@
}else if (route[r] == MOVE) {
route[r] = RIGHT;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateL();
@@ -159,37 +161,66 @@
}else if (route[r] == RIGHT) {
// Kreuzung führt zu Sackgassen -> löschen
junction[j] = 0;
-// printf("Kreuzung %d Schritt %d geloscht\n", j, r);
+ printf("Kreuzung %d Schritt %d geloscht\n", j, r);
j -= 1;
- while (junction[j] < r ) {
+ while (junction[j] <= r ) {
//invert rotation
- if (route[r] == LEFT) {
- route[r] = RIGHT;
+ if (junction[j] == r) {
- }else if (route[r] == RIGHT) {
+ switch (route[r]) {
+ case MOVE:
+ motion.runTask(route, r, true, junction[j]);
+ if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
+ r--;
+ break;
+ case LEFT:
+ r--;
+ break;
+ case RIGHT:
+ r--;
+ break;
+ }
+ motion.stop();
+ t1.reset();
+ t1.start();
+ while (t1 < 0.5f) {}
+ t1.stop();
+
+ }else{
+ if (route[r] == LEFT) {
+
+ route[r] = RIGHT;
+
+ }else if (route[r] == RIGHT) {
+
+ route[r] = LEFT;
+ }
- route[r] = LEFT;
- }
+ motion.runTask(route, r, true, junction[j]);
+ if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
+ route[r] = 0;
+ r--;
+ }
+ }
+ r++;
- motion.runTask(route, r, true, junction[j]);
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
- route[r] = 0;
- r--;
-
- }
motion.stop();
-// printf("Loop stop\n");
+ t1.reset();
+ t1.start();
+ while (t1 < 0.5f) {}
+ t1.stop();
+ if (debuging == 1) printf("Loop stop\n");
}else{
route[r] = LEFT;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
@@ -203,10 +234,10 @@
if (route[r] == LEFT) {
route[r] = RIGHT;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.scanMove();
@@ -214,34 +245,62 @@
}else if (route[r] == MOVE) {
junction[j] = 0;
-// printf("Kreuzung %d Schritt %d geloscht\n", j, r);
+ if (debuging == 1) printf("Kreuzung %d Schritt %d geloscht\n", j, r);
j -= 1;
- while (junction[j] < r ) {
+ while (junction[j] <= r ) {
//invert rotation
- if (route[r] == LEFT) {
- route[r] = RIGHT;
-
- }else if (route[r] == RIGHT) {
+ if (junction[j] == r) {
- route[r] = LEFT;
- }
-
- motion.runTask(route, r, true, junction[j]);
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
- route[r] = 0;
- r--;
+ switch (route[r]) {
+ case MOVE:
+ motion.runTask(route, r, true, junction[j]);
+ if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
+ r--;
+ break;
+ case LEFT:
+ r--;
+ break;
+ case RIGHT:
+ r--;
+ break;
+ }
+ motion.stop();
+ t1.reset();
+ t1.start();
+ while (t1 < 0.5f) {}
+ t1.stop();
+
+ }else{
+ if (route[r] == LEFT) {
+
+ route[r] = RIGHT;
+
+ }else if (route[r] == RIGHT) {
+
+ route[r] = LEFT;
+ }
+ motion.runTask(route, r, true, junction[j]);
+ if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
+ route[r] = 0;
+ r--;
+ }
}
+ r++;
motion.stop();
-// printf("Loop stop\n");
+ t1.reset();
+ t1.start();
+ while (t1 < 0.5f) {}
+ t1.stop();
+ if (debuging == 1) printf("Loop stop\n");
}else{
route[r] = MOVE;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.scanMove();
@@ -251,10 +310,10 @@
}else if (rWall == 0) {
route[r] = RIGHT;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
route[r] = MOVE;
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
r++;
motion.rotateR();
@@ -264,7 +323,7 @@
}else if ((lWall + cWall + rWall) == 3) {
motion.rotate180();
-// printf("Sackgasse Schritt: %d\n", r);
+ if (debuging == 1) printf("Sackgasse Schritt: %d\n", r);
r--;
t1.reset();
t1.start();
@@ -278,7 +337,7 @@
switch (route[r]) {
case MOVE:
motion.runTask(route, r, true, junction[j]);
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
r--;
break;
case LEFT:
@@ -306,7 +365,7 @@
}
//Run tasks in declining order
motion.runTask(route, r, true, junction[j]);
-// printf("Schritt: %d, Befehl: %d\n", r, route[r]);
+ if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
route[r] = 0;
r--;
}
Helvijs Kiselis
