Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
Drive.cpp
- Committer:
- ruesipat
- Date:
- 2018-04-12
- Revision:
- 4:e3f388933954
- Parent:
- 3:2ec7cf8bc3fc
- Child:
- 5:b8b1a979b0d5
File content as of revision 4:e3f388933954:
#include <cmath> #include "Drive.h" using namespace std; const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit const int Drive::DRIVINGCOUNTS = 1712000; //Entspricht Strecke von 20cm 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), irSensor0(irSensor0), irSensor1(irSensor1), irSensor2(irSensor2), irSensor3(irSensor3) { } Drive::~Drive() {} 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; //printf("Los gehts\n"); while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) ){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! || distanceFront > 35.0f //kontrastSensor.check(); 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); //totCLeft =+countsLeft; //totCRight =+countsRight; 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 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; // } } //counter = 0; sumCorrection =0; // totCLeft = 0; //totCRight = 0; //} //counter++; } controller.setDesiredSpeedRight(0.0f); controller.setDesiredSpeedLeft(0.0f); }