Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
Diff: Drive.cpp
- Revision:
- 5:b8b1a979b0d5
- Parent:
- 4:e3f388933954
- Child:
- 6:a09d2ee3b82e
diff -r e3f388933954 -r b8b1a979b0d5 Drive.cpp --- a/Drive.cpp Thu Apr 12 16:14:02 2018 +0000 +++ b/Drive.cpp Wed Apr 25 12:07:03 2018 +0000 @@ -1,4 +1,5 @@ #include <cmath> +#include "mbed.h" #include "Drive.h" @@ -6,19 +7,20 @@ using namespace std; -const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit -const int Drive::DRIVINGCOUNTS = 1712000; //Entspricht Strecke von 20cm +const float Drive::FRONTDISTANCE = 62.0f; //Abstand Sensor zur VorderWand //DONT TOUCH +const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit Drehzahl in [rpm] +const int Drive::DRIVINGCOUNTS = 1773; //Entspricht Strecke von 20cm //DONT TOUCH Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3): kontrastSensor(kontrastSensor), - counterLeft(counterLeft), + counterLeft(counterLeft), counterRight(counterRight), controller(controller), irSensor0(irSensor0), irSensor1(irSensor1), irSensor2(irSensor2), irSensor3(irSensor3) - + { } @@ -29,98 +31,111 @@ void Drive::driving() { - int countsRight = counterRight.read(); //EncoderCounts auslesen - int countsRight0 = countsRight; //ReferenzCounts setzten - int countsLeft = counterLeft.read(); - int countsLeft0 = countsLeft; - //int totCLeft, totCRight, - int sumCorrection =0; - //int sumCor = 0; - float corLeft = 0; - float corRight = 0; - //int counter = 0; + 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 countsRight0 = countsRight; //ReferenzCounts setzten + int countsLeft0 = countsLeft; + + float parallelDif = 0; + float rightLeftDif = 0; + + float correction = 0; + float slowdown = 0; + + int drive; + + + //Abfangen wenn Wand vorne dass sicher nicht vorwärts gefahren wird + + if(irSensor1.read() < FRONTDISTANCE) { + drive = 0; + } else { + drive = 1; + } + + //printf("Los gehts\n"); - - while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) ){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! || distanceFront > 35.0f - + + while(((countsRight <= countsRight0 + DRIVINGCOUNTS) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS)) && (drive == 1) ) { + //kontrastSensor.check(); - controller.setDesiredSpeedRight(DRIVINGSPEED - corRight); //Korrektur verringert Geschwindigkeit - controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft); countsRight = counterRight.read(); countsLeft = counterLeft.read(); + controller.setDesiredSpeedRight(DRIVINGSPEED - correction - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an + controller.setDesiredSpeedLeft(-DRIVINGSPEED - correction + slowdown); + //printf("CountsRight%d\n\r", countsRight); //printf(" CountsLeft%d\n\r", countsLeft); - - //totCLeft =+countsLeft; - //totCRight =+countsRight; + + + //printf("correction: %.0f\n\r", correction); + + + //Bereit fuer neuen Durchgang + correction = 0; + slowdown = 0; + + + //Ermittlung der Differenz Hinten-Vorne + if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront - if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)){ // irSensor0 durch irSensor3 ersetzt - -// int i = 0; -// while (i < 10){ -// -// sumCorrection += irSensor3.read()-irSensor2.read(); //irSensor0.read() => sensorRight irSensor2.read() => sensorLeftFornt // irSensor0 durch irSensor3 ersetzt -// //sumCor = irSensor2.read()-irSensor3.read(); -// //double AvSensDist; -// i++; -// } -// sumCorrection = sumCorrection/(double)i; - sumCorrection = irSensor2.read()-irSensor3.read(); - printf(" SumCorrection: %d\n\r", sumCorrection); - printf(" DistanzLinksVorne = %.0f mm\n\r", irSensor2.read()); - printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read()); - }else{ - corRight=0; - corLeft=0; - } - - //if(counter > 10){ //Wait Time until corretion value added - - + 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); - if(sumCorrection < -1){ // (steht naeher an rechter Wand) Arsch zu weit links - //corLeft = 1 * -sumCorrection; //Driving Speed Left Tire Addition - corLeft = 20.0f; // Correction neu 20 statt 1 - corRight=0; - }else if(sumCorrection > 1){ // (steht naeher an linker Wand) Arsch zu weit rechts - corLeft=0; - corRight = 20.0f; // Correction neu 20 statt 1 - //corRight = 1 * sumCorrection; //Driving Speed Right Tire Addition - }else{ - - - corLeft=0; - corRight=0; - - - // if(sumCor < -2){ - // corLeft=0; - // corRight = 1 * sumCor; //Driving Speed Right Tire Addition - - // }else if(sumCor > 2){ - // corLeft = 1 * -sumCor; //Driving Speed Left Tire Addition - // corRight=0; - // }else{ - - // corLeft=0; - // corRight=0; - // } + } 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 + + 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 + + rightLeftDif = 0; + + } + + + + //Berechung Korrektur + correction = ((0.1f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH + + + + + //Kontrolle ob vorne Wand...Verlangsamen und Anhalten + + if(irSensor1.read() < 150.0f) { //slow down + + slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED; //vorderer max abstand + + if ( slowdown > DRIVINGSPEED) { + + drive = 0; + } - - //counter = 0; - sumCorrection =0; - // totCLeft = 0; - //totCRight = 0; - //} - //counter++; - - - } - controller.setDesiredSpeedRight(0.0f); - controller.setDesiredSpeedLeft(0.0f); + } + + + }//Ende Whileschleife Drive... + + controller.setDesiredSpeedRight(0.0f); + controller.setDesiredSpeedLeft(0.0f); }