Version12.04.18
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Diff: Drive.cpp
- Revision:
- 3:2ec7cf8bc3fc
- Parent:
- 2:592f01278db4
- Child:
- 4:e3f388933954
diff -r 592f01278db4 -r 2ec7cf8bc3fc Drive.cpp --- a/Drive.cpp Wed Apr 04 15:24:28 2018 +0000 +++ b/Drive.cpp Sun Apr 08 19:17:01 2018 +0000 @@ -3,6 +3,7 @@ + using namespace std; const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit @@ -30,23 +31,40 @@ int countsRight0 = countsRight; //ReferenzCounts setzten int countsLeft = counterLeft.read(); int countsLeft0 = countsLeft; + int totCLeft, totCRight, counter, sumCorrection; + int corLeft = 0; + int corRight = 0; + printf("Los gehts\n"); - while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS)){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! + while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) || distanceFront < 1.5f){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! //kontrastSensor.check(); - controller.setDesiredSpeedRight(DRIVINGSPEED); - controller.setDesiredSpeedLeft(-DRIVINGSPEED); + controller.setDesiredSpeedRight(DRIVINGSPEED + corRight); + controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft); countsRight = counterRight.read(); countsLeft = counterLeft.read(); //printf("CountsRight%d\n\r", countsRight); //printf("CountsLeft%d\n\r", countsLeft); + totCLeft =+countsLeft; + totCRight =+countsRight; + counter++; - - //(distanceLeftFront - distanceRight) /rotationsgeschwindigkeit und allenfalls korrektur!!!!!!!!!!!!!! - + if(counter < 100){ //Wait Time until corretion value added + sumCorrection = totCLeft-totCRight; + if(sumCorrection <0){ + corLeft =+1; //Driving Speed Left Tire Addition + corRight=0; + }else if(sumCorrection >0){ + corLeft=0; + corRight=+1; //Driving Speed Right Tire Addition + } + counter = 0; + totCLeft = 0; + totCRight = 0; + } } controller.setDesiredSpeedRight(0.0f); controller.setDesiredSpeedLeft(0.0f);