READY TO RUMBLE
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Diff: Drive.cpp
- Revision:
- 4:e3f388933954
- Parent:
- 3:2ec7cf8bc3fc
- Child:
- 5:b8b1a979b0d5
diff -r 2ec7cf8bc3fc -r e3f388933954 Drive.cpp --- a/Drive.cpp Sun Apr 08 19:17:01 2018 +0000 +++ b/Drive.cpp Thu Apr 12 16:14:02 2018 +0000 @@ -7,18 +7,20 @@ using namespace std; const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit -const int Drive::DRIVINGCOUNTS = 1712; //Entspricht Strecke von 20cm +const int Drive::DRIVINGCOUNTS = 1712000; //Entspricht Strecke von 20cm -Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack): +Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3): kontrastSensor(kontrastSensor), counterLeft(counterLeft), counterRight(counterRight), - controller(controller) + controller(controller), + irSensor0(irSensor0), + irSensor1(irSensor1), + irSensor2(irSensor2), + irSensor3(irSensor3) + { - this->distanceRight = distanceRight; - this->distanceFront = distanceFront; - this->distanceLeftFront = distanceLeftFront; - this->distanceLeftBack = distanceLeftBack; + } Drive::~Drive() {} @@ -31,42 +33,94 @@ int countsRight0 = countsRight; //ReferenzCounts setzten int countsLeft = counterLeft.read(); int countsLeft0 = countsLeft; - int totCLeft, totCRight, counter, sumCorrection; - int corLeft = 0; - int corRight = 0; + //int totCLeft, totCRight, + int sumCorrection =0; + //int sumCor = 0; + float corLeft = 0; + float corRight = 0; + //int counter = 0; - printf("Los gehts\n"); + + + //printf("Los gehts\n"); - while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) || distanceFront < 1.5f){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! + while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) ){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! || distanceFront > 35.0f //kontrastSensor.check(); - controller.setDesiredSpeedRight(DRIVINGSPEED + corRight); + controller.setDesiredSpeedRight(DRIVINGSPEED - corRight); //Korrektur verringert Geschwindigkeit controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft); countsRight = counterRight.read(); countsLeft = counterLeft.read(); //printf("CountsRight%d\n\r", countsRight); - //printf("CountsLeft%d\n\r", countsLeft); + //printf(" CountsLeft%d\n\r", countsLeft); + + //totCLeft =+countsLeft; + //totCRight =+countsRight; - totCLeft =+countsLeft; - totCRight =+countsRight; - counter++; + 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 < 100){ //Wait Time until corretion value added - sumCorrection = totCLeft-totCRight; - if(sumCorrection <0){ - corLeft =+1; //Driving Speed Left Tire Addition + //if(counter > 10){ //Wait Time until corretion value added + + + + 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 >0){ + }else if(sumCorrection > 1){ // (steht naeher an linker Wand) Arsch zu weit rechts corLeft=0; - corRight=+1; //Driving Speed Right Tire Addition + 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; + // } } - counter = 0; - totCLeft = 0; - totCRight = 0; - } + + //counter = 0; + sumCorrection =0; + // totCLeft = 0; + //totCRight = 0; + //} + //counter++; + + } - controller.setDesiredSpeedRight(0.0f); - controller.setDesiredSpeedLeft(0.0f); + controller.setDesiredSpeedRight(0.0f); + controller.setDesiredSpeedLeft(0.0f); }