mit pathdingsbums
Dependencies: mbed
Fork of MicroMouse_MASTER_TWO by
Diff: main.cpp
- Revision:
- 5:b8b1a979b0d5
- Parent:
- 4:e3f388933954
- Child:
- 7:5ef09519a6e9
--- a/main.cpp Thu Apr 12 16:14:02 2018 +0000 +++ b/main.cpp Wed Apr 25 12:07:03 2018 +0000 @@ -37,7 +37,7 @@ IRSensor irSensor0(distance0); //rechts IRSensor irSensor1(distance1); //vorne IRSensor irSensor2(distance2); //links-vorne -IRSensor irSensor3(distance3); //links-hinten +IRSensor irSensor3(distance3); //links-hinten //AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte @@ -66,117 +66,161 @@ Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); - -//Turn turn(); //GEHT NICHT!!! int state = 0; -int mode = 1; int main() { + enableMotorDriver = 1; // Schaltet den Leistungstreiber ein + enable = 1; + int wallRight = 0; int wallFront = 0; - int wallLeft = 0; + int wallLeft = 0; int blackLine = 0; +// 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 - enableMotorDriver = 1; // Schaltet den Leistungstreiber ein - enable = 1; + 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); //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); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt... while(1) { // Wiederholungsschleife - //Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt... - - //printf("WARTE AUF START...."); + + switch(state) { //Wartemodus 1. Lauf + case 0: { + + + while (button) { + //printf("WARTE AUF 1.START..."); + wait(1.0f); //Die Ruhe vor dem Sturm... + } + + state = 1; + break; + } + case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus + + checkWalls.check(); //prueft wo Waende vorhanden sind + + //wallRight = 1; + //wallFront = 1; + //wallLeft = 1; + + turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... + + + //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 + + +// printf("%d", way[0][0][0]); + + drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden + + + + //blackLine = 1; + if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier. - while (button) { - wait(1.0f); //Die Ruhe vor dem Sturm... + state = 2; + } + break; + } + case 2: { //Ziel erreicht und Wartemodus fuer 2. Lauf + + controller.setDesiredSpeedRight(0.0f); + controller.setDesiredSpeedLeft(0.0f); + + while (button) { + //printf("WARTE AUF 2.START..."); + wait(1.0f); + } + blackLine = 0; + state = 3; + break; + } + case 3: {//2. Lauf Fahrzyklus + + + //pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden + checkWalls.check(); //prueft wo Waende vorhanden sind + + turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... + + + + 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. + + state = 4; + } + + break; + + } + case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf + + controller.setDesiredSpeedRight(0.0f); + controller.setDesiredSpeedLeft(0.0f); + + while (button) { + //printf("SCHLUSS..."); + wait(1.0f); + } + + state = 5; + break; + } + default: { + state = 0; + break; + } } - while(mode==1) { - //Ermittlung der Abstaende rund ums Fahrzeug - float distanceRight = irSensor0.read(); - float distanceFront = irSensor1.read(); - float distanceLeftFront = irSensor2.read(); - float distanceLeftBack = irSensor3.read(); - - - - CheckWalls checkWalls(distanceRight, distanceFront, distanceLeftFront, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1) - //Decide?? - Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft); //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); //20cm nach vorne... - - - switch(state) { //Ausrichten - case 0: { - // case 0 wird evtl nicht benötigt - state = 1; - break; - } - case 1: {//Fahrzyklus - - checkWalls.check(); //prueft wo Waende vorhanden sind - - //wallRight = 1; - //wallFront = 1; - //wallLeft = 1; - - //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... - - - drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden - - - - //blackLine = 1; - if(blackLine == 1){ //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier. - - state = 2; - } - break; - } - case 2: { //Ziel erreicht - - controller.setDesiredSpeedRight(0.0f); - controller.setDesiredSpeedLeft(0.0f); - - //"Stop" - //mode=2 - break; - } - default: { // - state = 0; - break; - } - } - - + //printf("DistanzRechts = %.0f mm\n\r", irSensor0.read()); + //printf("DistanzVorne = %.0f mm\n\r", irSensor1.read()); + //printf("DistanzLinksVorne = %.0f mm\n\r", irSensor2.read()); + //printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read()); + //printf("WandRechts = %d \n\r", wallRight); + //printf("WandVorne = %d \n\r", wallFront); + //printf("WandLinks = %d \n\r", wallLeft); - printf("DistanzRechts = %.0f mm\n\r", distanceRight); - printf("DistanzVorne = %.0f mm\n\r", distanceFront); - printf("DistanzLinksVorne = %.0f mm\n\r", distanceLeftFront); - printf("DistanzLinksHinten = %.0f mm\n\r", distanceLeftBack); - printf("WandRechts = %d \n\r", wallRight); - printf("WandVorne = %d \n\r", wallFront); - printf("WandLinks = %d \n\r", wallLeft); - - - // printf("CountsRight = %d\n", counterRight.read()); - // printf("CountsLeft = %d\n", counterLeft.read()); - //kontrastSensor.check(); - //printf("SchwarzeLinie = %d", blackLine); - myled =! myled; // LED is ON Heartbeat - // wait(0.5f); + // printf("CountsRight = %d\n", counterRight.read()); + // printf("CountsLeft = %d\n", counterLeft.read()); + //kontrastSensor.check(); + //printf("SchwarzeLinie = %d", blackLine); + myled =! myled; // LED is ON Heartbeat + wait(0.1f); - }//Ende while mode==1 + } }