---
Dependencies: mbed
Fork of MicroMouse_MASTER_FIVE by
Diff: main.cpp
- Revision:
- 8:1c8a747c49c8
- Parent:
- 7:5ef09519a6e9
- Child:
- 9:ab19796bf14a
diff -r 5ef09519a6e9 -r 1c8a747c49c8 main.cpp --- a/main.cpp Mon May 07 18:52:04 2018 +0000 +++ b/main.cpp Wed May 16 12:15:23 2018 +0000 @@ -9,6 +9,8 @@ #include "CheckWalls.h" #include "Turn.h" +Serial pc(USBTX,USBRX,460800); + //Initialisierung LEDs Blinker/Surri/Button DigitalIn button(USER_BUTTON); //Moduswählknopf DigitalOut myled(LED1); //Heartbeat (evt auch Anzeige für Modus, vor start @@ -50,10 +52,25 @@ Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); int state = 0; +int mode = 1; + int main() { - + printf("Start main\r\n"); + + + + + int lab[5][10]; + int path = 0; + int pathArr[50]; + int directions[50]; + int modeStart=0; + int startx = 0; + int starty = 10; + int ausr = 1; + int c = 0; enableMotorDriver = 1; // Schaltet den Leistungstreiber ein enable = 1; @@ -63,14 +80,14 @@ int blackLine = 0; int dontStop = 0; //int way = 220; - + // int way[5][10][4] = {{0},{0},{0}}; // Abspeicherung wird in roboterkoordinaten gespeichert vierstellige Zahl => 1.Zahl gefahrene Richtung, 2.Zahl Wand rechts, 3.Zahl Wand vorne, 4.Zahl Wand links - + //Funktionsdeklarationen CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1) - Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft, dontStop); //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne + Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft, dontStop, modeStart, path); //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne KontrastSensor kontrastSensor(kontrast, blackLine); - Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3, dontStop); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt... + Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3, dontStop); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt... while(1) { // Wiederholungsschleife @@ -87,6 +104,10 @@ wait(1.0f); //Die Ruhe vor dem Sturm... } + //if(modeStart == 1) { + // state = 1; + //} + state = 1; break; } @@ -95,37 +116,146 @@ checkWalls.check(); //prueft wo Waende vorhanden sind - turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... + turn.turning(); + + //merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth verändert und speichert diese in ein Array + if(wallLeft == 0) { + switch(ausr) { + case 1: { + ausr = 2; + lab[startx][starty] = ausr; + startx--; + break; + } + case 2: { + //lab[startx][starty] =2; + ausr = 4; + lab[startx][starty] = ausr; + starty++; + break; + } + case 3: { + //lab[startx][starty]=2; + ausr = 1; + lab[startx][starty] = ausr; + starty--; + break; + } + case 4: { + //lab[startx][starty]=2; + ausr = 3; + lab[startx][starty] = ausr; + startx++; + break; + } + } + + } else if(wallFront == 0) { + switch(ausr) { + case 1: { + //lab[startx][starty]=1; + ausr = 1; + lab[startx][starty] = ausr; + starty--; + break; + } + case 2: { + //lab[startx][starty]=1; + ausr = 2; + lab[startx][starty] = ausr; + startx--; + break; + } + case 3: { + //lab[startx][starty]=1; + ausr = 3; + lab[startx][starty] = ausr; + startx++; + break; + } + case 4: { + //lab[startx][starty]=1; + ausr = 4; + lab[startx][starty] = ausr; + starty++; + break; + } + } + } else if(wallRight == 0) { + switch(ausr) { + case 1: { + //lab[startx][starty]=3; + ausr = 3; + lab[startx][starty] = ausr; + startx++; + break; + } + case 2: { + //lab[startx][starty]=3; + ausr = 1; + lab[startx][starty] = ausr; + starty--; + break; + } + case 3: { + //lab[startx][starty]=3; + ausr = 4; + lab[startx][starty] = ausr; + starty++; + break; + } + case 4: { + //lab[startx][starty]=3; + ausr = 2; + lab[startx][starty] = ausr; + startx--; + break; + } + } + } else { + switch(ausr) { + case 1: { + //lab[startx][starty]=4; + ausr = 4; + lab[startx][starty] = ausr; + starty++; + break; + } + case 2: { + //lab[startx][starty]=4; + ausr = 3; + lab[startx][starty] = ausr; + startx++; + break; + } + case 3: { + //lab[startx][starty]=4; + ausr = 2; + lab[startx][starty] = ausr; + startx--; + break; + } + case 4: { + //lab[startx][starty]=4; + ausr = 1; + lab[startx][starty] = ausr; + starty--; + break; + } + } + } - //saveWay - //Abspeicherung der Waende... -// if(wallRight == 1){ -// way[0][0][1] = 1; -// } else { -// way[0][0][1] = 0; -// } -// -// if(wallRight == 1){ -// way[0][0][2] = 1; -// } else{ -// way[0][0][2] = 0; -// } -// -// if(wallRight == 1){ -// way[0][0][3] = 1; -// } else{ -// way[0][0][3] = 0; -// } - //Abspeicherung des entschiedenen Wegs und Anpassung des + //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... + //dontStop = way%10; //way = way/10; - + drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier. - + lab[startx][starty] = 5; state = 2; } break; @@ -135,21 +265,142 @@ controller.setDesiredSpeedRight(0.0f); controller.setDesiredSpeedLeft(0.0f); + startx = 0; + starty = 9; + + + //wait(30.4f); + int num; + c=0; + //Speichert die 2-Dimensionale Karte in ein 1D-Array welche nun aus anweisungen besteht + while(lab[startx][starty] != 5) { + num = lab[startx][starty]; + pathArr[c] = num; + + switch(num) { + case 1: { + starty--; + break; + } + case 2: { + startx--; + break; + } + case 3: { + startx++; + break; + } + case 4: { + starty++; + break; + } + case 5: { + + } + }//Switch End + + myled =! myled; // LED is ON Heartbeat + wait(0.4f); + //printf("Array = %d , num = %d\n\r", pathArr[c],num); + c++; + }//While Ende + + pathArr[c]= 5; //setzt end bit + + + c=0; + + + + //übersetzt den 1-Dimensionalen Array der Ausrichtung in einen Array nun Anweisungen zur Lösung darstellt + while(pathArr[c]!= 5) { + if(c==0) { + if(pathArr[c]==1) { + directions[c]=1; + } else { + directions[c]=3; + } + //printf("pathArr: %d, directions: %d \r\n", pathArr[c], directions[c]); + } else if(pathArr[c-1]==pathArr[c]) { + directions[c]=1; + //printf("pathArr: %d, directions: %d \r\n", pathArr[c], directions[c]); + } else { + + if(pathArr[c]==1) { + if(pathArr[c-1]==3) { + directions[c]=2; + } + if(pathArr[c-1]==2) { + directions[c]=3; + } + if(pathArr[c-1]==4) { + directions[c-1]=4; + } + } else if(pathArr[c]==2) { + if(pathArr[c-1]==1) { + directions[c]=2; + } + if(pathArr[c-1]==3) { + directions[c]=4; + } + if(pathArr[c-1]==4) { + directions[c]=3; + } + } else if(pathArr[c]==3) { + if(pathArr[c-1]==1) { + directions[c]=3; + } + if(pathArr[c-1]==2) { + directions[c]=4; + } + if(pathArr[c-1]==4) { + directions[c]=2; + } + } else if(pathArr[c]==4) { + if(pathArr[c-1]==1) { + directions[c]=4; + } + if(pathArr[c-1]==2) { + directions[c]=2; + } + if(pathArr[c-1]==3) { + directions[c]=3; + } + } + //printf("pathArr: %d, directions: %d \r\n", pathArr[c], directions[c]); + } + c++; + myled =! myled; // LED is ON Heartbeat + wait(0.1f); + //printf("Array = %d \n\r", directions[c]); + + } + c=0; + while (button) { //printf("WARTE AUF 2.START..."); wait(1.0f); } blackLine = 0; state = 3; + myled =0; // LED is ON Heartbeat + + + c=0; break; } case 3: {//2. Lauf Fahrzyklus - - + //übergibt Anweisung an turn + modeStart=1; + + path=directions[c]; + + printf("path %d, modeStart %d \r\n", path, modeStart); + //pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden //checkWalls.check(); //prueft wo Waende vorhanden sind // - //integer fuer kuerzesten weg...1 rechts 2 gerade aus 3 links + //integer fuer kuerzesten weg...3 rechts 1 gerade aus 2 links und 4 180 Grad drehung // wallRight = 0; // wallFront = 0; // wallLeft = 0; @@ -159,7 +410,7 @@ //dontStop = 2; drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden - + c++; if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier. state = 4; @@ -206,7 +457,7 @@ myled =! myled; // LED is ON Heartbeat wait(0.1f); - + }