Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
Drive.cpp
- Committer:
- ruesipat
- Date:
- 2018-03-31
- Revision:
- 1:d9e840c48b1e
- Child:
- 2:592f01278db4
File content as of revision 1:d9e840c48b1e:
#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); }