Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
Diff: Drive.cpp
- Revision:
- 2:592f01278db4
- Parent:
- 1:d9e840c48b1e
- Child:
- 3:2ec7cf8bc3fc
diff -r d9e840c48b1e -r 592f01278db4 Drive.cpp --- a/Drive.cpp Sat Mar 31 16:45:57 2018 +0000 +++ b/Drive.cpp Wed Apr 04 15:24:28 2018 +0000 @@ -5,10 +5,10 @@ using namespace std; -const float Drive::DRIVINGSPEED = 1000.0f;//Fahrgeschwindigkeit -const int Drive::DRIVINGCOUNTS = 10000; //Entspricht Strecke von 20cm +const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit +const int Drive::DRIVINGCOUNTS = 1712; //Entspricht Strecke von 20cm -Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront): +Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack): kontrastSensor(kontrastSensor), counterLeft(counterLeft), counterRight(counterRight), @@ -17,6 +17,7 @@ this->distanceRight = distanceRight; this->distanceFront = distanceFront; this->distanceLeftFront = distanceLeftFront; + this->distanceLeftBack = distanceLeftBack; } Drive::~Drive() {} @@ -32,17 +33,19 @@ printf("Los gehts\n"); - while((countsRight >= countsRight0 - DRIVINGCOUNTS) && (countsLeft <= countsLeft0 + DRIVINGCOUNTS)){ + while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS)){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! //kontrastSensor.check(); controller.setDesiredSpeedRight(DRIVINGSPEED); controller.setDesiredSpeedLeft(-DRIVINGSPEED); countsRight = counterRight.read(); countsLeft = counterLeft.read(); - //printf("CountsRight%d\n", countsRight); - //printf("CountsLeft%d\n", countsLeft); + //printf("CountsRight%d\n\r", countsRight); + //printf("CountsLeft%d\n\r", countsLeft); - //parallelitaet und center pruefen!!!!!!!!!!!! + + + //(distanceLeftFront - distanceRight) /rotationsgeschwindigkeit und allenfalls korrektur!!!!!!!!!!!!!! } controller.setDesiredSpeedRight(0.0f);