Helvijs Kiselis
/
Micromouse
Algorithmus
main.cpp
- Committer:
- Helvis
- Date:
- 2018-05-23
- Revision:
- 32:e984b7959cb0
- Parent:
- 31:2c54f8304ef5
- Child:
- 33:836ab2089565
File content as of revision 32:e984b7959cb0:
/*----------------------------------------------------------------------------* * Micromouse PES2 * * PES2 2018, TEAM 3 * * Code entwickelt von: * * *-----------------------------------------------------------------------------*/ #include <mbed.h> #include "EncoderCounter.h" #include "Controller.h" #include "IRSensor.h" #include "Motion.h" //User Button InterruptIn button(USER_BUTTON); //Sensors: AnalogIn lineSensor(PC_5); AnalogIn distance2(PC_3); AnalogIn distance4(PB_1); AnalogIn distance1(PC_2); IRSensor irSensorL (distance2); IRSensor irSensorC (distance4); IRSensor irSensorR (distance1); 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 debuging = 0; const int MOVE = 1; const int LEFT = 2; const int RIGHT = 3; const int HALF_MOVE = 4; const int TURN_LEFT = 5; const int TURN_RIGHT = 6; const int EMPTY = 7; //Sensor tresholds [mm] const float thresholdL = 70; const float thresholdR = 70; const float thresholdC = 80; //------------------------------------------------------------------------------ //User button toggle void press() { start = !start; } //------------------------------------------------------------------------------ int main() { //Init int route[200] = {0}; int r = 0; int junction[20] = {0}; 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) { 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; } } if (debuging == 1) printf("Kreuzung: %d Schritt: %d\n", j, r); } //No wall left if (lWall == 0) { if (route[r] == LEFT) { route[r] = MOVE; if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); r++; motion.rotateL(); motion.scanMove(); }else if (route[r] == MOVE) { route[r] = RIGHT; if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); r++; route[r] = MOVE; if (debuging == 1) 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; printf("Kreuzung %d Schritt %d geloscht\n", j, r); j -= 1; bool looping = false; while (junction[j] <= r ) { //invert rotation if (junction[j] == r) { switch (route[r]) { case MOVE: if (looping == false) { 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; }else if (junction[j] == r-1 && route[r] == MOVE) { looping = true; }else if (junction[j] == r-2 && route[r] == MOVE && route[junction[j]] != MOVE) { route[r] = 0; r--; } 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(); t1.reset(); t1.start(); while (t1 < 0.5f) {} t1.stop(); if (debuging == 1) printf("Loop stop\n"); }else{ route[r] = LEFT; if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); r++; route[r] = MOVE; if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); r++; motion.rotateL(); motion.scanMove(); } //No wall center }else if (cWall == 0) { if (route[r] == LEFT) { route[r] = RIGHT; if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); r++; route[r] = MOVE; if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); r++; motion.scanMove(); }else if (route[r] == MOVE) { junction[j] = 0; if (debuging == 1) printf("Kreuzung %d Schritt %d geloscht\n", j, r); j -= 1; bool looping = false; while (junction[j] <= r ) { //invert rotation if (junction[j] == r) { switch (route[r]) { case MOVE: if (looping == false) { 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; }else if (junction[j] == r-1 && route[r] == MOVE) { looping = true; }else if (junction[j] == r-2 && route[r] == MOVE && route[junction[j]] != MOVE) { route[r] = 0; r--; } 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(); t1.reset(); t1.start(); while (t1 < 0.5f) {} t1.stop(); if (debuging == 1) printf("Loop stop\n"); }else{ route[r] = MOVE; if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); r++; motion.scanMove(); } //No wall right }else if (rWall == 0) { route[r] = RIGHT; if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); r++; route[r] = MOVE; if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); r++; motion.rotateR(); motion.scanMove(); //Dead end routine }else if ((lWall + cWall + rWall) == 3) { motion.rotate180(); if (debuging == 1) 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 ) { if (junction[j] == 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{ //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, junction[j]); if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]); route[r] = 0; r--; } } r++; } //finish line check if (motion.finish() == 1) { Ziel = 1; r = 1; //Convert rotations to smooth turns while (route[r] != 0) { //Convert pattern MOVE - ROTATE - MOVE if (route[r-1] == MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) { route[r-1] = HALF_MOVE; route[r+1] = HALF_MOVE; if (route[r] == LEFT) route[r] = TURN_LEFT; else route[r] = TURN_RIGHT; //Convert consequent ROTATION to a smooth TURN }else if (route[r-1] == HALF_MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) { route[r-1] = EMPTY; route[r+1] = HALF_MOVE; if (route[r] == LEFT) route[r] = TURN_LEFT; else route[r] = TURN_RIGHT; } r++; } r = 0; start = 0; controller.counterReset(); myled = 1; }else{ Ziel = 0; } } /*-------------------------------------------------------------------------* * * Speed run * *-------------------------------------------------------------------------*/ while ( start == 1 && Ziel == 1 ) { motion.runTask(route,r,false,junction[j]); r++; if (route[r] == 0) { //Weg fertig motion.stop(); start = 0; myled = 0; } } } }