Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: main.cpp
- Revision:
- 1:2b5f79285a3e
- Parent:
- 0:8491169be8fc
- Child:
- 2:f898adf2d817
--- a/main.cpp Mon Apr 16 10:27:10 2018 +0000 +++ b/main.cpp Thu Apr 19 06:19:43 2018 +0000 @@ -1,5 +1,24 @@ +/** + * Micromouse PES2 + * + * Suchfahrtalg. + Schnellfahrtalg. + * + */ + + /* todo: + + - Regler gerade Fahrt + > Mitte Werte L/R + - Suchfahrt testen + > Tresholds bestimmen + - gespeicherten Weg fahren + - Problem: Links beschleunigt schneller + - Ziellinie + - Beschleunigung + - über längere Strecke schneller fahren + +*/ #include <mbed.h> - #include "EncoderCounter.h" #include "Controller.h" #include "IRSensor.h" @@ -38,21 +57,23 @@ Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); - Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR, enableMotorDriver); + Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, + irSensorR, lineSensor, enableMotorDriver); //------------------------------------------------------------------------------ volatile int start = 0; - short countsLeft = 0; - short countsRight = 0; - short countsLeftOld = 0; - short countsRightOld = 0; - //den jeweiligen Test auf 1 setzen - int testSensor = 0; - int testMotion = 0; - int testRotation = 0; - int testCounts = 0; - int testClass = 0; + + const int MOVE = 1; + const int LEFT = 2; + const int RIGHT = 3; + const int SPEED = 4; + const int STOP = 5; + + //Sensor tresholds [mm] + const float thresholdL = 70; + const float thresholdR = 70; + const float thresholdC = 100; //------------------------------------------------------------------------------ @@ -60,173 +81,220 @@ void press() { start = !start; } + +//Return to last junction +void reverseToJunction(int& junc, int& r, int route[]) { + + while (junc < r ) { + + //invert rotation + if (route[r] == LEFT) { + + route[r] = RIGHT; + + }else if (route[r] == RIGHT) { + + route[r] = LEFT; + } + + motion.runTask(route[r]); + route[r] = 0; + r--; + } +} //------------------------------------------------------------------------------ int main() { +//Init -/** - Micromouse Test -*/ + int route[50] = {0}; + int r = 0; + + int junction[5] = {0}; + int j = 0; + short lWall; + short cWall; + short rWall; + short Ziel = 0; + +//loop while(1) { button.fall(&press); //User button einlesen - while(0) { - myled = 1; - printf("Hai\n"); - if (start == 0) break; - } + /** + * + * Search run + * + */ + + while(start == 1 && Ziel == 0) { - - - /* - Sensoren Test - */ - while(1) { - - + /** + * Entscheidung und Bewegung + */ + + float distanceL = irSensorL.readL(); float distanceC = irSensorC.readC(); float distanceR = irSensorR.readR(); - float distanceB = irSensorB.readC(); - printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read()); - - //wait(0.5f); - } - - /* - Motoren Test: Gerade Bewegung - */ - while(start == 1 && testMotion == 1) { - controller.setDesiredSpeedLeft(20.0f); //Drehzahl in [rpm] - controller.setDesiredSpeedRight(-20.0f); - enableMotorDriver = 1; //Schaltet den Leistungstreiber ein - - countsLeft = counterLeft.read(); - countsRight = counterRight.read(); + //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; - printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight); - - if (start == 0) { + //Junction Check + if ((lWall + cWall + rWall) < 2) { - motion.stop(); + if (junction[j] > 0) {j++;} + junction[j] = r; } - } - - /* - Motoren Test: 180° Rotation - */ - - while(start == 1 && testRotation == 1) { + //No wall left + if (lWall == 0) { + if (route[r] == LEFT) { + + route[r] = MOVE; + r++; + + motion.rotateL(); + motion.scanMove(); + motion.stop(); + + }else if (route[r] == MOVE) { + + route[r] = RIGHT; + r++; + route[r] = MOVE; + r++; + + motion.rotateL(); + motion.scanMove(); + motion.stop(); + + }else if (route[r] == RIGHT) { + // Kreuzung führt zu Sackgassen -> löschen + junction[j] = 0; + if (j > 0) {j--;} + reverseToJunction(junction[j], r, route); + + }else{ + + route[r] = LEFT; + r++; + route[r] = MOVE; + r++; + + motion.rotateL(); + motion.scanMove(); + motion.stop(); + } + //No wall center + }else if (cWall == 0) { - while((countsLeft - countsLeftOld) > -1614 || (countsRight - countsRightOld) > -1614) { + if (route[r] == LEFT) { + + route[r] = RIGHT; + r++; + route[r] = MOVE; + r++; - countsLeft = counterLeft.read(); - countsRight = counterRight.read(); - - controller.setDesiredSpeedLeft(-50.0f); - controller.setDesiredSpeedRight(-50.0f); - enableMotorDriver = 1; + motion.scanMove(); + motion.stop(); + + }else if (route[r] == MOVE) { + + junction[j] = 0; + if (j > 0) {j--;} + reverseToJunction(junction[j], r, route); + + }else{ + + route[r] = MOVE; + r++; + + motion.scanMove(); + motion.stop(); + } + //No wall right + }else if (rWall == 0) { + + route[r] = RIGHT; + r++; + route[r] = MOVE; + r++; + + motion.rotateR(); + motion.scanMove(); + motion.stop(); + + //Dead end routine + }else if ((lWall + cWall + rWall) == 3) { + + motion.rotate180(); + r--; + //Return to last junction + while (junction[j] < r ) { + + //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]); + route[r] = 0; + r--; + } } - - motion.stop(); - enableMotorDriver = 0; - printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight); - - if (start == 0) { - countsLeftOld = countsLeft - 100; - countsRightOld = countsRight - 100; - - break; + + if (motion.finish() == 1) { + Ziel = 1; + r = 0; + start = 0; + }else{ + Ziel = 0; } - } -/* Benötigte Ticks für 180* Rotation am Ort: - - mm/count = 0.122 - Dr = 124mm - Rr = 60mm - 180°=> 1597 counts - 90°=> 798 counts - - - RotationsDistanz Rd= DistanzRäder*pi/2 = 194.8 - - UmfangRad = Drad*pi = 188.5 - - - Counts = Rd/mm pro Count + } - => Counts pro Drehung rausfinden */ - while(start == 1 && testCounts == 1) { - - while(countsLeft < 5000 || countsRight > -5000) { - - countsLeft = counterLeft.read(); - countsRight = counterRight.read(); + /** + * + * Speed run + * + */ + + while ( start == 1 && Ziel == 1 ) { - controller.setDesiredSpeedLeft(20.0f); - controller.setDesiredSpeedRight(-20.0f); - enableMotorDriver = 1; + if (route[r] == route[r+1] && route[r] == route[r+2]) { + //Auf längere Strecke schneller fahren + route[r+1] = SPEED; } + motion.runTask(route[r]); + r++; - controller.setDesiredSpeedLeft(0.0f); - controller.setDesiredSpeedRight(0.0f); - enableMotorDriver = 0; - + if (route[r] == 0) { + //Weg fertig + start = 0; + } } - //Class test - if(start == 1 && testClass == 1) { - motion.rotate180(); - counterLeft.reset(); - counterRight.reset(); - start = 0; - } - - //Sensor calib - if(start == 1) { - int i; - myled = !myled; - for (i=0;i<5;i++) { - float distanceL = irSensorL.readL(); - float distanceC = irSensorC.readC(); - float distanceR = irSensorR.readR(); - float distanceB = irSensorB.readC(); - printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read()); - } - printf("------------------------\n"); - start = 0; - - /* while((countsLeft - countsLeftOld) > -82 || (countsRight - countsRightOld) < 82) { - - controller.setDesiredSpeedLeft(-10.0f); //Drehzahl in [rpm] - controller.setDesiredSpeedRight(10.0f); - enableMotorDriver = 1; //Schaltet den Leistungstreiber ein - - countsLeft = counterLeft.read(); - countsRight = counterRight.read(); - } - - controller.setDesiredSpeedLeft(0.0f); - controller.setDesiredSpeedRight(0.0f); - enableMotorDriver = 0; - - float distanceL = irSensorB.read(); - printf("Hinten: %f\n", distanceL); - - countsLeftOld = countsLeft; - countsRightOld = countsRight; - start = 0;*/ - - } + + + } }