PES2_R2D2.0
Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of
MicroMouse_MASTER_THREE
by 
PES2_R2D2.0
Diff: Drive.cpp
- Revision:
- 9:ab19796bf14a
- Parent:
- 7:5ef09519a6e9
--- 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...
+ }