![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fertige Version mit geschwindigkeit 100
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Drive.cpp
- Committer:
- ruesipat
- Date:
- 2018-05-02
- Revision:
- 7:5ff551b098f8
- Parent:
- 6:a09d2ee3b82e
File content as of revision 7:5ff551b098f8:
#include <cmath> #include "mbed.h" #include "Drive.h" using namespace std; const float Drive::FRONTDISTANCE = 80.0f; //Abstand Sensor zur VorderWand //DONT TOUCH //62.0f //55.0 const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit Drehzahl in [rpm] const int Drive::DRIVINGCOUNTS = 1480; //Entspricht Strecke von 20cm //DONT TOUCH /1773 //1800 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() { controller.reset(); int countsRight = counterRight.read(); //EncoderCounts auslesen int countsLeft = counterLeft.read(); //0 - 32767 //printf("CountsRight%d\n\r", countsRight); //printf(" CountsLeft%d\n\r", countsLeft); int countsRight0 = countsRight; //ReferenzCounts setzten int countsLeft0 = countsLeft; float parallelDif = 0; float rightLeftDif = 0; float speedCorrection = 0; float slowdown = 0; int countCorrection = 0; int drive; //Abfangen wenn Wand vorne dass sicher nicht vorwärts gefahren wird if(irSensor1.read() < FRONTDISTANCE) { drive = 0; } else { drive = 1; } //printf("Los gehts\n"); while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) { kontrastSensor.check(); countsRight = counterRight.read(); countsLeft = counterLeft.read(); controller.setDesiredSpeedRight(DRIVINGSPEED - speedCorrection - slowdown/3); //Korrektur passt Geschwindigkeit an beiden Raedern an controller.setDesiredSpeedLeft(-DRIVINGSPEED - speedCorrection + slowdown/3); //printf("CountsRight%d\n\r", countsRight); //printf(" CountsLeft%d\n\r", countsLeft); //printf("correction: %.0f\n\r", correction); //Bereit fuer neuen Durchgang speedCorrection = 0; slowdown = 0; //Ermittlung der Differenz Hinten-Vorne if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront parallelDif = irSensor3.read()-irSensor2.read(); //differenz hinten vorne bestimmen //printf(" DistanzLinksVorne = %.0f mm\n\r", irSensor2.read()); //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor3.read()); //printf(" parallelDif: %.0f \n\r", parallelDif); } else { //ist wand eine wand nicht vorhanden => keine korrektur parallelDif = 0; } //Ermittlung der Differenz Rechts-Links if((irSensor0.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor0 => sensorRight irSensor2 => sensorLeftFornt rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read()); //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read()); //printf(" rightLeftDif: %.0f \n\r", rightLeftDif); } else { //ist wand eine wand nicht vorhanden => keine korrektur rightLeftDif = 0; } //Berechung Korrektur speedCorrection = ((0.2f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH //Kontrolle ob vorne Wand...Verlangsamen und Anhalten if(irSensor1.read() < 150.0f) { //slow down countCorrection = 500; // ermoeglicht an eine gesichtete wand anzugleichen slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED; //vorderer max abstand if ( slowdown > DRIVINGSPEED) { drive = 0; } } }//Ende Whileschleife Drive... //controller.setDesiredSpeedRight(0.5f); //0.5f //controller.setDesiredSpeedLeft(-0.5f); //-0.5f }