READY TO RUMBLE
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Diff: Drive.cpp
- Revision:
- 1:d9e840c48b1e
- Child:
- 2:592f01278db4
diff -r a9fe4ef404bf -r d9e840c48b1e Drive.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Drive.cpp Sat Mar 31 16:45:57 2018 +0000 @@ -0,0 +1,51 @@ +#include <cmath> +#include "Drive.h" + + + +using namespace std; + +const float Drive::DRIVINGSPEED = 1000.0f;//Fahrgeschwindigkeit +const int Drive::DRIVINGCOUNTS = 10000; //Entspricht Strecke von 20cm + +Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront): + kontrastSensor(kontrastSensor), + counterLeft(counterLeft), + counterRight(counterRight), + controller(controller) +{ + this->distanceRight = distanceRight; + this->distanceFront = distanceFront; + this->distanceLeftFront = distanceLeftFront; +} + +Drive::~Drive() {} + + +void Drive::driving() +{ + + int countsRight = counterRight.read(); //EncoderCounts auslesen + int countsRight0 = countsRight; //ReferenzCounts setzten + int countsLeft = counterLeft.read(); + int countsLeft0 = countsLeft; + + printf("Los gehts\n"); + + while((countsRight >= countsRight0 - DRIVINGCOUNTS) && (countsLeft <= countsLeft0 + DRIVINGCOUNTS)){ + + //kontrastSensor.check(); + controller.setDesiredSpeedRight(DRIVINGSPEED); + controller.setDesiredSpeedLeft(-DRIVINGSPEED); + countsRight = counterRight.read(); + countsLeft = counterLeft.read(); + //printf("CountsRight%d\n", countsRight); + //printf("CountsLeft%d\n", countsLeft); + + //parallelitaet und center pruefen!!!!!!!!!!!! + + } + controller.setDesiredSpeedRight(0.0f); + controller.setDesiredSpeedLeft(0.0f); + +}