PES2_R2D2.0
mit pathdingsbums
Dependencies: mbed
Fork of
MicroMouse_MASTER_TWO
by 
PES2_R2D2.0
Revision 8:1c8a747c49c8, committed 2018-05-16
- Comitter:
- TheDarkDurzo
- Date:
- Wed May 16 12:15:23 2018 +0000
- Parent:
- 7:5ef09519a6e9
- Commit message:
- mit pathfinding
Changed in this revision
--- a/Turn.cpp Mon May 07 18:52:04 2018 +0000
+++ b/Turn.cpp Wed May 16 12:15:23 2018 +0000
@@ -1,19 +1,21 @@
#include <cmath>
#include "Turn.h"
-
+//ausr startx und starty kann aus turn.cpp/turn.h entfernt werden
using namespace std;
const float Turn::TURNINGSPEED = 70.0f;//Drehgeschwindgkeit Drehzahl in [rpm]
const int Turn::TURNINGCOUNTS = 950; //Entspricht Drehung um 90Grad //DONT TOUCH //940//1070
-Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop):
+Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop, int& modeStart,int& path):
counterLeft(counterLeft),
counterRight(counterRight),
controller(controller),
wallRight(wallRight),
wallFront(wallFront),
wallLeft(wallLeft),
- dontStop(dontStop)
+ dontStop(dontStop),
+ path(path),
+ modeStart(modeStart)
{}
Turn::~Turn() {}
@@ -35,7 +37,7 @@
//Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
- if (wallLeft == 0){ //Nach Links Drehen
+ if ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen
//printf("Links ist frei\n");
@@ -50,15 +52,19 @@
controller.setDesiredSpeedRight(0.5f);
controller.setDesiredSpeedLeft(-0.5f);
+
+
dontStop = 1;
- }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus
+ }else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus
//printf("Vorne ist frei\n");
dontStop = 2;
- }else if (wallRight == 0) { //Nach Rechts Drehen
+
+
+ }else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen
//printf("Rechts ist frei\n");
@@ -73,10 +79,12 @@
controller.setDesiredSpeedRight(0.5f);
controller.setDesiredSpeedLeft(-0.5f);
+
+
dontStop = 3;
- }else{ //Alle Wege versperrt-> Wenden
+ }else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden
//printf("Alles versperrt...zurueck\n");
@@ -91,6 +99,8 @@
controller.setDesiredSpeedRight(0.5f); //0.0f
controller.setDesiredSpeedLeft(-0.5f); //0.0f
+
+
dontStop = 4;
}
}
--- a/Turn.h Mon May 07 18:52:04 2018 +0000
+++ b/Turn.h Wed May 16 12:15:23 2018 +0000
@@ -12,10 +12,16 @@
public:
- Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop);
+ Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft, int& dontStop, int& modeStart,int& path);
virtual ~Turn();
void turning();
+
+ int& modeStart;
+
+ int& path;
+
+
private:
@@ -29,6 +35,7 @@
int& wallFront;
int& wallLeft;
int& dontStop;
+
};
#endif /* TURN_H_ */
\ No newline at end of file
--- 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);
-
+
}