READY TO RUMBLE
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Diff: Drive.cpp
- Revision:
- 7:5ef09519a6e9
- Parent:
- 6:a09d2ee3b82e
diff -r a09d2ee3b82e -r 5ef09519a6e9 Drive.cpp --- a/Drive.cpp Wed Apr 25 12:49:48 2018 +0000 +++ b/Drive.cpp Mon May 07 18:52:04 2018 +0000 @@ -7,11 +7,11 @@ using namespace std; -const float Drive::FRONTDISTANCE = 62.0f; //Abstand Sensor zur VorderWand //DONT TOUCH -const float Drive::DRIVINGSPEED = 50.0f;//Fahrgeschwindigkeit Drehzahl in [rpm] -const int Drive::DRIVINGCOUNTS = 1773; //Entspricht Strecke von 20cm //DONT TOUCH +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 -Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3): +Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop): kontrastSensor(kontrastSensor), counterLeft(counterLeft), counterRight(counterRight), @@ -19,7 +19,8 @@ irSensor0(irSensor0), irSensor1(irSensor1), irSensor2(irSensor2), - irSensor3(irSensor3) + irSensor3(irSensor3), + dontStop(dontStop) { @@ -32,7 +33,7 @@ { controller.reset(); - + int countsRight = counterRight.read(); //EncoderCounts auslesen int countsLeft = counterLeft.read(); //0 - 32767 @@ -43,11 +44,23 @@ int countsRight0 = countsRight; //ReferenzCounts setzten int countsLeft0 = countsLeft; - float parallelDif = 0; - float rightLeftDif = 0; + float parallelDif = 0.0f; + float rightLeftDif = 0.0f; + float leftDif = 0.0f; + float rightDif = 0.0f; - float correction = 0; - float slowdown = 0; + float speedCorrection = 0.0f; + int countCorrection = 0; + + + float softStart = DRIVINGSPEED; + float slowdown = 0.0f; + + + if(dontStop == 2){ //geradeaus + softStart =0.0f; + } + int drive; @@ -63,13 +76,13 @@ //printf("Los gehts\n"); - while(((countsRight <= countsRight0 + DRIVINGCOUNTS) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS)) && (drive == 1) ) { + while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) { kontrastSensor.check(); countsRight = counterRight.read(); countsLeft = counterLeft.read(); - controller.setDesiredSpeedRight(DRIVINGSPEED - correction - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an - controller.setDesiredSpeedLeft(-DRIVINGSPEED - correction + slowdown); + 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); @@ -78,28 +91,31 @@ //printf("correction: %.0f\n\r", correction); + //Bereit fuer neuen Durchgang - correction = 0; - slowdown = 0; + //speedCorrection = 0.0f; + //leftDif = 0.0f; + //rightDif = 0.0f; + //slowdown = 0.0f; - //Ermittlung der Differenz Hinten-Vorne - if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront - + //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 - + parallelDif = 0; } - //Ermittlung der Differenz Rechts-Links - if((irSensor0.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor0 => sensorRight irSensor2 => sensorLeftFornt - + //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()); @@ -109,33 +125,96 @@ rightLeftDif = 0; + //Ermittlung der Differenz links auf ReferenzWert + 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 - correction = ((0.1f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH + speedCorrection = ((0.35f * rightLeftDif) + (0.7f * parallelDif) + (0.7f * leftDif) + (0.7f * rightDif)); //DONT TOUCH 0.2 0.5 + + + + //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...Verlangsamen und Anhalten - + //Kontrolle ob vorne Wand...wenn vorhanden nach vorderer Wand ausrichten und anhalten ansonst nur nach counts anhalten if(irSensor1.read() < 150.0f) { //slow down - slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED; //vorderer max abstand - - if ( slowdown > DRIVINGSPEED) { + 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 + slowdown = slowdown + 5.0f; + if (slowdown >= DRIVINGSPEED - 20.0f) { + slowdown = DRIVINGSPEED - 20.0f; + } + + } else{ + slowdown = 0.0f; } + + + //if(dontStop == 2){ //geradeaus + // slowdown = 0.0f; + //} + + wait(0.01f); }//Ende Whileschleife Drive... - controller.setDesiredSpeedRight(0.5f); //0.0f - controller.setDesiredSpeedLeft(0.5f); //0.0f + //controller.setDesiredSpeedRight(0.5f); //0.5f + //controller.setDesiredSpeedLeft(-0.5f); //-0.5f }