PES2_R2D2.0
---
Dependencies: mbed
Fork of
MicroMouse_MASTER_FIVE
by 
PES2_R2D2.0
Diff: main.cpp
- Revision:
- 8:1c8a747c49c8
- Parent:
- 7:5ef09519a6e9
- Child:
- 9:ab19796bf14a
--- 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);
-
+
}