PES2_R2D2.0
---
Dependencies: mbed
Fork of
MicroMouse_MASTER_FIVE
by 
PES2_R2D2.0
Diff: main.cpp
- Revision:
- 9:ab19796bf14a
- Parent:
- 8:1c8a747c49c8
- Child:
- 10:e8110fb94686
--- a/main.cpp Wed May 16 12:15:23 2018 +0000
+++ b/main.cpp Wed May 16 16:41:44 2018 +0000
@@ -11,21 +11,10 @@
Serial pc(USBTX,USBRX,460800);
-//Initialisierung LEDs Blinker/Surri/Button
+//Initialisierung LED/Button
DigitalIn button(USER_BUTTON); //Moduswählknopf
DigitalOut myled(LED1); //Heartbeat (evt auch Anzeige für Modus, vor start
-DigitalOut blinker0(PA_4); //Blinker links
-DigitalOut blinker1(PC_1); //Blinker rechts
-DigitalOut surri(PC_0); //
-//Initialisierung LEDs Nightrider
-PwmOut led0(PB_0); //von links nach rechts
-PwmOut led1(PB_8);
-PwmOut led2(PB_3);
-PwmOut led3(PB_5);
-PwmOut led4(PB_4);
-PwmOut led5(PB_10);
-PwmOut led6(PB_9);
//Initialisierung IR-Sensoren
AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
@@ -57,20 +46,6 @@
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;
@@ -79,46 +54,67 @@
int wallLeft = 0;
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
-
+ int lab[5][10];
+ int path = 0;
+ int pathNext = 0;
+ int pathArr[50];
+ int directions[50];
+ int modeStart = 0;
+ int startx = 0;
+ int starty = 10;
+ 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); //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, 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) {
- //printf("WARTE AUF 1.START...");
- //controller.setDesiredSpeedRight(0.5f);
- //controller.setDesiredSpeedLeft(-0.5f);
wait(1.0f); //Die Ruhe vor dem Sturm...
}
-
- //if(modeStart == 1) {
- // state = 1;
- //}
-
+ firstMove = 1;
state = 1;
+
break;
}
case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
-
-
+
checkWalls.check(); //prueft wo Waende vorhanden sind
+
+
turn.turning();
-
- //merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth verändert und speichert diese in ein Array
+
+ if(firstMove == 1){
+ dontStop = 1;
+ firstMove = 0;
+
+ /* **************************** */
+ /* Anpassung 16.02.18 */
+ if (wallFront == 0) {
+ ausr = 1;
+ lab[startx][starty] = ausr;
+ starty--;
+ }else if (wallRight == 0) {
+ 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: {
@@ -128,21 +124,18 @@
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++;
@@ -153,28 +146,24 @@
} 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++;
@@ -184,28 +173,24 @@
} 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--;
@@ -215,28 +200,24 @@
} 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--;
@@ -244,35 +225,29 @@
}
}
}
-
-
- //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;
}
- case 2: { //Ziel erreicht und Wartemodus fuer 2. Lauf
+
+ 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;
-
-
- //wait(30.4f);
+
int num;
- c=0;
+ 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;
@@ -301,7 +276,6 @@
myled =! myled; // LED is ON Heartbeat
wait(0.4f);
- //printf("Array = %d , num = %d\n\r", pathArr[c],num);
c++;
}//While Ende
@@ -313,6 +287,7 @@
//ü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) {
@@ -320,10 +295,8 @@
} 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) {
@@ -367,68 +340,59 @@
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;
+ c = 0;
while (button) {
- //printf("WARTE AUF 2.START...");
wait(1.0f);
}
blackLine = 0;
+ firstMove = 1;
state = 3;
- myled =0; // LED is ON Heartbeat
-
-
- c=0;
+ myled = 0; // LED is ON Heartbeat
+ c = 0;
break;
}
case 3: {//2. Lauf Fahrzyklus
- //übergibt Anweisung an turn
- modeStart=1;
+
+ //uebergibt Anweisung an turn
- path=directions[c];
+ modeStart=1;
+ path = directions[c];
+ pathNext = directions[c+1];
- printf("path %d, modeStart %d \r\n", path, modeStart);
+
+ turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
- //pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden
- //checkWalls.check(); //prueft wo Waende vorhanden sind
- //
- //integer fuer kuerzesten weg...3 rechts 1 gerade aus 2 links und 4 180 Grad drehung
- // wallRight = 0;
- // wallFront = 0;
- // wallLeft = 0;
-
- turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
-
- //dontStop = 2;
-
+
+ /**************************/
+ 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) {
- //printf("SCHLUSS...");
wait(1.0f);
}
-
state = 5;
break;
}
@@ -437,28 +401,7 @@
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("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);
-
-
-
-
}
}