PES2_R2D2.0
Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of
MicroMouse_MASTER_THREE
by 
PES2_R2D2.0
Revision 9:ab19796bf14a, committed 2018-05-16
- Comitter:
- ruesipat
- Date:
- Wed May 16 16:41:44 2018 +0000
- Parent:
- 8:1c8a747c49c8
- Commit message:
- ;
Changed in this revision
--- a/CheckWalls.cpp Wed May 16 12:15:23 2018 +0000
+++ b/CheckWalls.cpp Wed May 16 16:41:44 2018 +0000
@@ -17,10 +17,8 @@
}
-
CheckWalls::~CheckWalls() {}
-
void CheckWalls::check(){
if (irSensor0.read() < WALL_DISTANCE){ //Abstand zur RECHTEN WAND
--- a/Drive.cpp Wed May 16 12:15:23 2018 +0000
+++ b/Drive.cpp Wed May 16 16:41:44 2018 +0000
@@ -9,9 +9,9 @@
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 /1370
+const int Drive::DRIVINGCOUNTS = 1390; //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):
+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),
counterLeft(counterLeft),
counterRight(counterRight),
@@ -20,7 +20,10 @@
irSensor1(irSensor1),
irSensor2(irSensor2),
irSensor3(irSensor3),
- dontStop(dontStop)
+ dontStop(dontStop),
+ modeStart(modeStart),
+ path(path),
+ pathNext(pathNext)
{
@@ -35,12 +38,7 @@
controller.reset();
int countsRight = counterRight.read(); //EncoderCounts auslesen
- int countsLeft = counterLeft.read(); //0 - 32767
-
- //printf("CountsRight%d\n\r", countsRight);
- //printf(" CountsLeft%d\n\r", countsLeft);
-
-
+ int countsLeft = counterLeft.read();
int countsRight0 = countsRight; //ReferenzCounts setzten
int countsLeft0 = countsLeft;
@@ -51,16 +49,13 @@
float speedCorrection = 0.0f;
int countCorrection = 0;
-
-
+
float softStart = DRIVINGSPEED;
float slowdown = 0.0f;
-
- if(dontStop == 2){ //geradeaus
+ if(dontStop == 2) { //geradeaus
softStart =0.0f;
}
-
int drive;
@@ -73,9 +68,6 @@
drive = 1;
}
-
- //printf("Los gehts\n");
-
while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) {
kontrastSensor.check();
@@ -84,28 +76,12 @@
controller.setDesiredSpeedRight(-softStart + DRIVINGSPEED - speedCorrection - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an //slowdown
controller.setDesiredSpeedLeft(softStart - DRIVINGSPEED - speedCorrection + slowdown); //slowdown
- //printf("CountsRight%d\n\r", countsRight);
- //printf(" CountsLeft%d\n\r", countsLeft);
-
-
- //printf("correction: %.0f\n\r", correction);
-
-
-
- //Bereit fuer neuen Durchgang
- //speedCorrection = 0.0f;
- //leftDif = 0.0f;
- //rightDif = 0.0f;
- //slowdown = 0.0f;
-
//Ermittlung der Differenz Hinten-Vorne (PARALLEL)
+
if((irSensor3.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
parallelDif = irSensor3.read()-irSensor2.read(); //differenz hinten vorne bestimmen
- //printf(" DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
- //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
- //printf(" parallelDif: %.0f \n\r", parallelDif);
} else { //ist wand eine wand nicht vorhanden => keine korrektur
@@ -114,12 +90,10 @@
//Ermittlung der Differenz Rechts-Links (LINKS UND RECHTS WAND VORHANDEN)
+
if((irSensor0.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor0 => sensorRight irSensor2 => sensorLeftFornt
rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen
- //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
- //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
- //printf(" rightLeftDif: %.0f \n\r", rightLeftDif);
} else { //ist wand eine wand nicht vorhanden => keine korrektur
@@ -129,92 +103,71 @@
if(irSensor2.read() < 120.0f) { //irSensor2 => sensorLeftFornt
leftDif = 60.0f - irSensor2.read(); //differenz links rechts bestimmen
- //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
- //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
- //printf(" leftDif: %.0f \n\r", leftDif);
} else { //ist wand eine wand nicht vorhanden => keine korrektur
leftDif = 0;
-
}
//Ermittlung der Differenz rechts auf Referenzwert
+
if(irSensor0.read() < 120.0f) { //irSensor0 => sensorRight
rightDif = irSensor0.read() - 60.0f; //differenz links rechts bestimmen
- //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
- //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
- //printf(" rightDif: %.0f \n\r", rightDif);
} else { //ist wand eine wand nicht vorhanden => keine korrektur
rightDif = 0;
-
}
-
-
-
-
}
+ //Berechung Korrektur
- //Berechung Korrektur
- speedCorrection = ((0.35f * rightLeftDif) + (0.7f * parallelDif) + (0.7f * leftDif) + (0.7f * rightDif)); //DONT TOUCH 0.2 0.5
-
+ speedCorrection = ((0.3f * rightLeftDif) + (0.6f * parallelDif) + (0.6f * leftDif) + (0.6f * rightDif)); //DONT TOUCH 0.35 0.7 0.7 0.7
//Anfahrrampe damit die Raeder nicht durchdrehen
+
softStart = softStart - 3.5f;
if (softStart <= 0.0f) {
softStart = 0.0f;
}
- //printf(" softStart: %.0f\n\r", softStart);
-
-
-
-
-
//Kontrolle ob vorne Wand...wenn vorhanden nach vorderer Wand ausrichten und anhalten ansonst nur nach counts anhalten
+
if(irSensor1.read() < 150.0f) { //slow down
countCorrection = 5000;
- //slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED; //vorderer max abstand
slowdown = slowdown + 5.0f;
if (slowdown >= DRIVINGSPEED - 20.0f) {
+
slowdown = DRIVINGSPEED - 20.0f;
}
if (irSensor1.read() < FRONTDISTANCE) {
drive = 0;
+ }
- }
-
- } else if(((DRIVINGCOUNTS - countsRight) < 300 || (countsLeft + DRIVINGCOUNTS) < 300) && (irSensor2.read() > 120.0f)) { //Anhaltrampe wenn nach counts gefahren
+ } else if((((DRIVINGCOUNTS - countsRight) < 220) || ((countsLeft + DRIVINGCOUNTS) < 220)) && ((irSensor2.read() > 120.0f) && ((modeStart != 1) || (pathNext == 2)))) { //Anhaltrampe wenn nach counts gefahren
slowdown = slowdown + 5.0f;
if (slowdown >= DRIVINGSPEED - 20.0f) {
+
slowdown = DRIVINGSPEED - 20.0f;
+}
+ } else if((((DRIVINGCOUNTS - countsRight) < 220) || ((countsLeft + DRIVINGCOUNTS) < 220)) && ((irSensor0.read() > 120.0f) && ((modeStart == 1) && (pathNext == 3)))) { //Anhaltrampe wenn nach counts gefahren
+ slowdown = slowdown + 5.0f;
+ if (slowdown >= DRIVINGSPEED - 20.0f) {
+
+ slowdown = DRIVINGSPEED - 20.0f;
+ }
+ } else {
+
+ slowdown = 0.0f;
}
-
- } else{
- slowdown = 0.0f;
- }
-
-
- //if(dontStop == 2){ //geradeaus
- // slowdown = 0.0f;
- //}
-
-
- wait(0.01f);
- }//Ende Whileschleife Drive...
-
- //controller.setDesiredSpeedRight(0.5f); //0.5f
- //controller.setDesiredSpeedLeft(-0.5f); //-0.5f
-
-}
+ wait(0.01f);
+ }//Ende Whileschleife Drive...
+ }
--- a/Drive.h Wed May 16 12:15:23 2018 +0000
+++ b/Drive.h Wed May 16 16:41:44 2018 +0000
@@ -15,7 +15,7 @@
public:
- Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop);
+ 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);
virtual ~Drive();
void driving();
private:
@@ -39,7 +39,9 @@
IRSensor& irSensor3;
int& dontStop;
-
+ int& modeStart;
+ int& path;
+ int& pathNext;
};
--- a/IRSensor.cpp Wed May 16 12:15:23 2018 +0000
+++ b/IRSensor.cpp Wed May 16 16:41:44 2018 +0000
@@ -13,9 +13,6 @@
float IRSensor::read()
{
double d = distance * 3.3f; // Change the value to be in the 0 to 3300 range
-
- //DONT TOUCH
- //y = 14.098x6 - 147.35x5 + 627.68x4 - 1403.5x3 + 1765.1x2 - 1236.9x + 430.44 POLYNOMFUNKTION AUS EXCEL
float f = 14.098f*((d)*(d)*(d)*(d)*(d)*(d)) - 147.35f*((d)*(d)*(d)*(d)*(d)) + 627.68*((d)*(d)*(d)*(d)) - 1403.5f*((d)*(d)*(d)) + 1765.1f*((d)*(d)) - 1236.9f*(d) + 430.44f; // Lesen der Distanz in [mm]
return f;
--- a/KontrastSensor.cpp Wed May 16 12:15:23 2018 +0000
+++ b/KontrastSensor.cpp Wed May 16 16:41:44 2018 +0000
@@ -3,28 +3,20 @@
using namespace std;
-//IRSensor::IRSensor(AnalogIn& distance, DigitalOut& bit0, DigitalOut& bit1, DigitalOut& bit2, int number) :
-// distance(distance), bit0(bit0), bit1(bit1), bit2(bit2)
KontrastSensor::KontrastSensor(AnalogIn& kontrast, int& blackLine) :
kontrast(kontrast),
blackLine(blackLine)
{
- // this->number = number;
+
}
KontrastSensor::~KontrastSensor() {}
void KontrastSensor::check()
{
+ float k = kontrast*3300.0f; //sodass in Range bis 3300mV
-
- float k = kontrast*3300.0f; //sodass in Range bis 3300mV
- //printf("%.0f\n\r", k);
-
if(k >2500.0f){
blackLine = 1;
}
- //else{//Auschalten im Betrieb (nur zur Kontrolle)
- // blackLine = 0;
- //}
}
--- a/Turn.cpp Wed May 16 12:15:23 2018 +0000
+++ b/Turn.cpp Wed May 16 16:41:44 2018 +0000
@@ -14,8 +14,9 @@
wallFront(wallFront),
wallLeft(wallLeft),
dontStop(dontStop),
- path(path),
- modeStart(modeStart)
+ modeStart(modeStart),
+ path(path)
+
{}
Turn::~Turn() {}
@@ -28,79 +29,51 @@
int countsLeft = counterLeft.read();
int countsLeft0 = countsLeft;
- //vor dem abbiegen halten
+ //Vor dem abbiegen halten
+
controller.setDesiredSpeedRight(0.5f);
controller.setDesiredSpeedLeft(-0.5f);
- //wait(0.1f);
-
-
-
- //Entscheiden welche Richtung, Drehen und Stoppen wenn die gewuenschte Anzahl Counts erreicht sind
if ((modeStart != 1 && wallLeft == 0) || (modeStart == 1 && path==2)){ //Nach Links Drehen
-
- //printf("Links ist frei\n");
-
while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){
controller.setDesiredSpeedRight(TURNINGSPEED);
controller.setDesiredSpeedLeft(TURNINGSPEED);
countsRight = counterRight.read();
countsLeft = counterLeft.read();
- //printf("%d\n\r", countsRight);
- //printf("%d\n\r", countsLeft);
}
+
controller.setDesiredSpeedRight(0.5f);
controller.setDesiredSpeedLeft(-0.5f);
-
-
dontStop = 1;
}else if ((modeStart != 1 && wallFront == 0) || (modeStart == 1 && path==1)){ //Nicht Drehen-> weiter Geradeaus
- //printf("Vorne ist frei\n");
-
dontStop = 2;
-
-
}else if ((modeStart != 1 && wallRight == 0) || (modeStart == 1 && path==3)) { //Nach Rechts Drehen
- //printf("Rechts ist frei\n");
-
while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){
controller.setDesiredSpeedRight(-TURNINGSPEED);
controller.setDesiredSpeedLeft(-TURNINGSPEED);
countsRight = counterRight.read();
countsLeft = counterLeft.read();
- //printf("%d\n", countsRight);
- //printf("%d\n", countsLeft);
}
controller.setDesiredSpeedRight(0.5f);
controller.setDesiredSpeedLeft(-0.5f);
-
-
-
dontStop = 3;
}else if((modeStart != 1) || (modeStart == 1 && path==4)){ //Alle Wege versperrt-> Wenden
-
- //printf("Alles versperrt...zurueck\n");
-
while((countsRight <= countsRight0 + 2*TURNINGCOUNTS - 0) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS - 0)){
controller.setDesiredSpeedRight(TURNINGSPEED);
controller.setDesiredSpeedLeft(TURNINGSPEED);
countsRight = counterRight.read();
countsLeft = counterLeft.read();
- //printf("%d\n", countsRight);
- //printf("%d\n", countsLeft);
}
controller.setDesiredSpeedRight(0.5f); //0.0f
controller.setDesiredSpeedLeft(-0.5f); //0.0f
-
-
dontStop = 4;
}
}
--- a/Turn.h Wed May 16 12:15:23 2018 +0000
+++ b/Turn.h Wed May 16 16:41:44 2018 +0000
@@ -17,9 +17,6 @@
virtual ~Turn();
void turning();
- int& modeStart;
-
- int& path;
@@ -35,6 +32,8 @@
int& wallFront;
int& wallLeft;
int& dontStop;
+ int& modeStart;
+ int& path;
};
--- 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);
-
-
-
-
}
}