Version12.04.18
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Revision 4:e3f388933954, committed 2018-04-12
- Comitter:
- ruesipat
- Date:
- Thu Apr 12 16:14:02 2018 +0000
- Parent:
- 3:2ec7cf8bc3fc
- Commit message:
- Version12.04.18
Changed in this revision
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); }
diff -r 2ec7cf8bc3fc -r e3f388933954 Drive.h --- a/Drive.h Sun Apr 08 19:17:01 2018 +0000 +++ b/Drive.h Thu Apr 12 16:14:02 2018 +0000 @@ -15,7 +15,7 @@ public: - Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack); + Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3); virtual ~Drive(); void driving(); private: @@ -32,10 +32,10 @@ Controller& controller; - float distanceRight; - float distanceFront; - float distanceLeftFront; - float distanceLeftBack; + IRSensor& irSensor0; + IRSensor& irSensor1; + IRSensor& irSensor2; + IRSensor& irSensor3;
diff -r 2ec7cf8bc3fc -r e3f388933954 Turn.cpp --- a/Turn.cpp Sun Apr 08 19:17:01 2018 +0000 +++ b/Turn.cpp Thu Apr 12 16:14:02 2018 +0000 @@ -4,7 +4,7 @@ using namespace std; const float Turn::TURNINGSPEED = 50.0f;//Drehgeschwindgkeit -const int Turn::TURNINGCOUNTS = 941; //Entspricht Drehung um 90Grad +const int Turn::TURNINGCOUNTS = 946; //Entspricht Drehung um 90Grad //941; Turn::Turn(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, int& wallRight, int& wallFront, int& wallLeft): counterLeft(counterLeft), @@ -29,26 +29,26 @@ if (wallLeft == 0){ //Nach Links Drehen - printf("Links ist frei\n"); + //printf("Links ist frei\n"); while((countsRight <= countsRight0 + TURNINGCOUNTS) && (countsLeft <= countsLeft0 + TURNINGCOUNTS)){ controller.setDesiredSpeedRight(TURNINGSPEED); controller.setDesiredSpeedLeft(TURNINGSPEED); countsRight = counterRight.read(); countsLeft = counterLeft.read(); - printf("%d\n\r", countsRight); - printf("%d\n\r", countsLeft); + //printf("%d\n\r", countsRight); + //printf("%d\n\r", countsLeft); } controller.setDesiredSpeedRight(0.0f); controller.setDesiredSpeedLeft(0.0f); }else if (wallFront == 0){ //Nicht Drehen-> weiter Geradeaus - printf("Vorne ist frei\n"); + //printf("Vorne ist frei\n"); }else if (wallRight == 0) { //Nach Rechts Drehen - printf("Rechts ist frei\n"); + //printf("Rechts ist frei\n"); while((countsRight >= countsRight0 - TURNINGCOUNTS) && (countsLeft >= countsLeft0 - TURNINGCOUNTS)){ controller.setDesiredSpeedRight(-TURNINGSPEED); @@ -64,7 +64,7 @@ }else{ //Alle Wege versperrt-> Wenden - printf("Alles versperrt...zurueck\n"); + //printf("Alles versperrt...zurueck\n"); while((countsRight <= countsRight0 + 2*TURNINGCOUNTS) && (countsLeft <= countsLeft0 + 2*TURNINGCOUNTS)){ controller.setDesiredSpeedRight(TURNINGSPEED);
diff -r 2ec7cf8bc3fc -r e3f388933954 main.cpp --- a/main.cpp Sun Apr 08 19:17:01 2018 +0000 +++ b/main.cpp Thu Apr 12 16:14:02 2018 +0000 @@ -37,7 +37,7 @@ IRSensor irSensor0(distance0); //rechts IRSensor irSensor1(distance1); //vorne IRSensor irSensor2(distance2); //links-vorne -IRSensor irSensor3(distance3); //links-hinten +IRSensor irSensor3(distance3); //links-hinten //AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte @@ -74,7 +74,10 @@ int main() { - + int wallRight = 0; + int wallFront = 0; + int wallLeft = 0; + int blackLine = 0; enableMotorDriver = 1; // Schaltet den Leistungstreiber ein @@ -101,16 +104,13 @@ float distanceLeftFront = irSensor2.read(); float distanceLeftBack = irSensor3.read(); - int wallRight; - int wallFront; - int wallLeft; - int blackLine; + CheckWalls checkWalls(distanceRight, distanceFront, distanceLeftFront, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1) //Decide?? Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft); //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne KontrastSensor kontrastSensor(kontrast, blackLine); - Drive drive(kontrastSensor, counterLeft, counterRight, controller, distanceRight, distanceFront, distanceLeftFront, distanceLeftBack); //20cm nach vorne... + Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3); //20cm nach vorne... switch(state) { //Ausrichten @@ -127,7 +127,7 @@ //wallFront = 1; //wallLeft = 1; - turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... + //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser... drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden