Patrick Ruesi
/
Micromouse_alpha_copy_copy
Version12.04.18
Fork of Micromouse_beta by
Drive.cpp
- Committer:
- TheDarkDurzo
- Date:
- 2018-04-08
- Revision:
- 3:2ec7cf8bc3fc
- Parent:
- 2:592f01278db4
- Child:
- 4:e3f388933954
File content as of revision 3:2ec7cf8bc3fc:
#include <cmath> #include "Drive.h" using namespace std; 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, float distanceLeftBack): kontrastSensor(kontrastSensor), counterLeft(counterLeft), counterRight(counterRight), controller(controller) { this->distanceRight = distanceRight; this->distanceFront = distanceFront; this->distanceLeftFront = distanceLeftFront; this->distanceLeftBack = distanceLeftBack; } 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, counter, sumCorrection; int corLeft = 0; int corRight = 0; printf("Los gehts\n"); while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) || distanceFront < 1.5f){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!! //kontrastSensor.check(); controller.setDesiredSpeedRight(DRIVINGSPEED + corRight); 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; counter++; if(counter < 100){ //Wait Time until corretion value added sumCorrection = totCLeft-totCRight; if(sumCorrection <0){ corLeft =+1; //Driving Speed Left Tire Addition corRight=0; }else if(sumCorrection >0){ corLeft=0; corRight=+1; //Driving Speed Right Tire Addition } counter = 0; totCLeft = 0; totCRight = 0; } } controller.setDesiredSpeedRight(0.0f); controller.setDesiredSpeedLeft(0.0f); }