Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
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... + }