Alexander Lüthard
/
Micromouse
Alle Prinf's auskommentiert
Fork of Micromouse by
main.cpp
- Committer:
- Helvis
- Date:
- 2018-05-07
- Revision:
- 17:8a8758bfe3c5
- Parent:
- 16:c5b864804632
- Child:
- 18:3309329d5f42
File content as of revision 17:8a8758bfe3c5:
/** * Micromouse PES2 * * Suchfahrtalg. + Schnellfahrtalg. * */ /* todo: - Beschleunigung: Wann & Wo? - Zentrieren Optimieren: - Beschleunigung - Zentrieren - über längere Strecke schneller fahren - Abbiegen */ #include <mbed.h> #include "EncoderCounter.h" #include "Controller.h" #include "IRSensor.h" #include "Motion.h" //User Button InterruptIn button(USER_BUTTON); //Sensors: AnalogIn lineSensor(PA_4); AnalogIn distance2(PC_3); AnalogIn distance4(PB_1); AnalogIn distance1(PC_2); AnalogIn distance3(PC_5); IRSensor irSensorL (distance2); IRSensor irSensorC (distance4); IRSensor irSensorR (distance1); IRSensor irSensorB (distance3); Timer t1; //Motors: DigitalOut myled(LED1); DigitalOut enableMotorDriver(PB_2); DigitalIn motorDriverFault(PB_14); DigitalIn motorDriverWarning(PB_15); PwmOut pwmRight(PA_8); PwmOut pwmLeft(PA_10); EncoderCounter counterRight(PB_6, PB_7); EncoderCounter counterLeft(PA_0, PA_1); Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR, lineSensor, enableMotorDriver); //------------------------------------------------------------------------------ volatile int start = 0; const int MOVE = 1; const int LEFT = 2; const int RIGHT = 3; const int SPEED = 4; //Sensor tresholds [mm] const float thresholdL = 80; const float thresholdR = 80; const float thresholdC = 100; //------------------------------------------------------------------------------ //User button toggle void press() { 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() { //Init int route[200] = {0}; int r = 0; volatile int junction[20] = {0}; volatile int j = 0; short lWall; short cWall; short rWall; short Ziel = 0; //infinite loop while(1) { button.fall(&press); //User button einlesen /** * * Search run * */ while(start == 1 && Ziel == 0) { /** * Entscheidung und Bewegung */ 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; //Junction Check if ((lWall + cWall + rWall) < 2) { if (junction[j] != r) { if (junction[j] > 0) { j += 1; junction[j] = r; }else{ junction[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]); r++; motion.rotateL(); motion.scanMove(); //motion.stop(); }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 -= 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 { //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]); //At work!!!!! route[r] = 0; r--; } while (junction[j] < r ) ; motion.stop(); printf("Loop stop\n"); }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++; motion.rotateL(); motion.scanMove(); // motion.stop(); } //No wall center }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++; 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 -= 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 { //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 (junction[j] < r ); motion.stop(); printf("Loop stop\n"); }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--; t1.reset(); t1.start(); while (t1 < 0.5f) {} 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); 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 < 1.0f) {} t1.stop(); }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,true); 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]) */ Ziel = 1; r = 0; start = 0; controller.counterReset(); }else{ Ziel = 0; } } /** * * 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); r++; if (route[r] == 0) { //Weg fertig motion.stop(); start = 0; } } } }