![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ENDLÖSUNG:)
Dependencies: mbed
Fork of MicroMouse_MASTER_FOUR by
main.cpp
- Committer:
- ruesipat
- Date:
- 2018-05-22
- Revision:
- 10:e8110fb94686
- Parent:
- 9:ab19796bf14a
File content as of revision 10:e8110fb94686:
#include "mbed.h" #include "IRSensor.h" #include "EncoderCounter.h" #include "LowpassFilter.h" #include "Controller.h" #include "KontrastSensor.h" #include "Drive.h" #include "CheckWalls.h" #include "Turn.h" Serial pc(USBTX,USBRX,460800); //Initialisierung LED/Button DigitalIn button(USER_BUTTON); //Moduswählknopf DigitalOut myled(LED1); //Heartbeat (evt auch Anzeige für Modus, vor start //Initialisierung IR-Sensoren AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte AnalogIn distance1(PC_3); AnalogIn distance2(PC_5); AnalogIn distance3(PB_1); IRSensor irSensor0(distance0); //rechts IRSensor irSensor1(distance1); //vorne IRSensor irSensor2(distance2); //links-vorne IRSensor irSensor3(distance3); //links-hinten //Initialisierung Kontrastsensor AnalogIn kontrast(PC_0); //Initialisierung Motor und Encoder DigitalOut enableMotorDriver(PB_2); PwmOut pwmLeft(PA_8); //Motor links PwmOut pwmRight(PA_9); //Motor rechts EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts DigitalOut enable(PC_1); Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); int state = 0; int mode = 1; int main() { //printf("--------\n\rREADY\n\r--------\n\r"); enableMotorDriver = 1; // Schaltet den Leistungstreiber ein enable = 1; int wallRight = 0; int wallFront = 0; int wallLeft = 0; int blackLine = 0; int dontStop = 0; int lab[5][10]; int path = 0; int pathNext = 0; int pathArr[51]; int directions[51]; int modeStart = 0; int startx = 0; int starty = 9; int ausr = 1; int c = 0; int firstMove = 0; //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, 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, modeStart, path, pathNext); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt... while(1) { // Wiederholungsschleife switch(state) { //Wartemodus 1. Lauf case 0: { while (button) { wait(1.0f); //Die Ruhe vor dem Sturm... } firstMove = 1; state = 1; break; } case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus checkWalls.check(); //prueft wo Waende vorhanden sind turn.turning(); if(firstMove == 1) { dontStop = 1; firstMove = 0; /* **************************** */ /* Anpassung 16.02.18 */ //if (wallFront == 0) { // ausr = 1; // lab[startx][starty] = ausr; // starty--; //} else if (wallRight == 20) { // ausr = 3; // Ausrichtung zu kontrollieren!!!! // lab[startx][starty] = ausr; // startx++; //} /***********************/ } //merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth veraendert und speichert diese in ein Array if(wallLeft == 0) { switch(ausr) { case 1: { ausr = 2; lab[startx][starty] = ausr; startx--; break; } case 2: { ausr = 4; lab[startx][starty] = ausr; starty++; break; } case 3: { ausr = 1; lab[startx][starty] = ausr; starty--; break; } case 4: { ausr = 3; lab[startx][starty] = ausr; startx++; break; } } } else if(wallFront == 0) { switch(ausr) { case 1: { ausr = 1; lab[startx][starty] = ausr; starty--; break; } case 2: { ausr = 2; lab[startx][starty] = ausr; startx--; break; } case 3: { ausr = 3; lab[startx][starty] = ausr; startx++; break; } case 4: { ausr = 4; lab[startx][starty] = ausr; starty++; break; } } } else if(wallRight == 0) { switch(ausr) { case 1: { ausr = 3; lab[startx][starty] = ausr; startx++; break; } case 2: { ausr = 1; lab[startx][starty] = ausr; starty--; break; } case 3: { ausr = 4; lab[startx][starty] = ausr; starty++; break; } case 4: { ausr = 2; lab[startx][starty] = ausr; startx--; break; } } } else { switch(ausr) { case 1: { ausr = 4; lab[startx][starty] = ausr; starty++; break; } case 2: { ausr = 3; lab[startx][starty] = ausr; startx++; break; } case 3: { ausr = 2; lab[startx][starty] = ausr; startx--; break; } case 4: { ausr = 1; lab[startx][starty] = ausr; starty--; break; } } } 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; } case 2: { //Ziel erreicht => Wartemodus fuer 2. Lauf und Berechnung der schnellsten Weges controller.setDesiredSpeedRight(0.0f); controller.setDesiredSpeedLeft(0.0f); startx = 0; starty = 9; 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); 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; } } else if(pathArr[c-1]==pathArr[c]) { directions[c]=1; } 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; } } } c++; myled =! myled; // LED is ON Heartbeat wait(0.1f); } c = 0; for(int p=0; p<20; p++) { //printf("Anw: %d\r\n", directions[p]); } for(int p=0; p<9; p++) { for(int q=0; q<4; q++) { //printf(" %d ", lab[q][p]); } //printf("\r\n"); } while (button) { wait(1.0f); } blackLine = 0; firstMove = 1; state = 3; myled = 0; // LED is ON Heartbeat c = 0; break; } case 3: {//2. Lauf Fahrzyklus //uebergibt Anweisung an turn modeStart=1; path = directions[c]; pathNext = directions[c+1]; turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... /**************************/ if(firstMove == 1) { dontStop = 1; firstMove = 0; } /***********************/ 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; } break; } case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf controller.setDesiredSpeedRight(0.0f); controller.setDesiredSpeedLeft(0.0f); while (button) { wait(1.0f); } state = 5; break; } default: { state = 0; break; } } myled =! myled; // LED is ON Heartbeat wait(0.1f); } }