PES2_R2D2.0
ENDLÖSUNG:)
Dependencies: mbed
Fork of
MicroMouse_MASTER_FOUR
by 
PES2_R2D2.0
Revision 10:e8110fb94686, committed 2018-05-22
- Comitter:
- ruesipat
- Date:
- Tue May 22 14:20:20 2018 +0000
- Parent:
- 9:ab19796bf14a
- Commit message:
- s
Changed in this revision
| Drive.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Drive.cpp Wed May 16 16:41:44 2018 +0000
+++ b/Drive.cpp Tue May 22 14:20:20 2018 +0000
@@ -9,7 +9,7 @@
const float Drive::FRONTDISTANCE = 70.0f; //Abstand Sensor zur VorderWand //DONT TOUCH //62.0f //55.0 110.0
const float Drive::DRIVINGSPEED = 130.0f;//Fahrgeschwindigkeit Drehzahl in [rpm] //130.0f
-const int Drive::DRIVINGCOUNTS = 1390; //Entspricht Strecke von 20cm //DONT TOUCH /1773 //1800 //1350 /////1390
+const int Drive::DRIVINGCOUNTS = 1380; //Entspricht Strecke von 20cm //DONT TOUCH /1773 //1800 //1350 /////1390
Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop, int& modeStart, int& path, int& pathNext):
kontrastSensor(kontrastSensor),
--- a/main.cpp Wed May 16 16:41:44 2018 +0000
+++ b/main.cpp Tue May 22 14:20:20 2018 +0000
@@ -46,6 +46,7 @@
int main()
{
+ //printf("--------\n\rREADY\n\r--------\n\r");
enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
enable = 1;
@@ -57,25 +58,25 @@
int lab[5][10];
int path = 0;
int pathNext = 0;
- int pathArr[50];
- int directions[50];
+ int pathArr[51];
+ int directions[51];
int modeStart = 0;
int startx = 0;
- int starty = 10;
+ 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...
+ 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) {
@@ -83,38 +84,37 @@
}
firstMove = 1;
state = 1;
-
+
break;
}
case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
-
+
checkWalls.check(); //prueft wo Waende vorhanden sind
-
+
-
+
turn.turning();
-
- if(firstMove == 1){
+
+ 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++;
- }
- /***********************/
+ //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: {
@@ -225,16 +225,16 @@
}
}
}
-
+
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);
@@ -242,12 +242,12 @@
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;
@@ -273,21 +273,21 @@
}
}//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) {
@@ -298,7 +298,7 @@
} else if(pathArr[c-1]==pathArr[c]) {
directions[c]=1;
} else {
-
+
if(pathArr[c]==1) {
if(pathArr[c-1]==3) {
directions[c]=2;
@@ -347,6 +347,21 @@
}
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);
}
@@ -358,29 +373,31 @@
break;
}
case 3: {//2. Lauf Fahrzyklus
-
+
//uebergibt Anweisung an turn
-
+
modeStart=1;
path = directions[c];
- pathNext = directions[c+1];
-
-
+ pathNext = directions[c+1];
+
+
turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
-
-
+
+
/**************************/
- if(firstMove == 1){
+ 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;
}