PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FOUR

Pathfinding nach rechts funktioniert noch nicht...der rest schon

Dependencies:   mbed

Fork of MicroMouse_MASTER_THREE by PES2_R2D2.0

Drive.cpp@1:d9e840c48b1e, 2018-03-31 (annotated)

Committer:
ruesipat
Date:
Sat Mar 31 16:45:57 2018 +0000
Revision:
1:d9e840c48b1e
Child:
2:592f01278db4
j

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ruesipat 1:d9e840c48b1e 1 #include <cmath>
ruesipat 1:d9e840c48b1e 2 #include "Drive.h"
ruesipat 1:d9e840c48b1e 3
ruesipat 1:d9e840c48b1e 4
ruesipat 1:d9e840c48b1e 5
ruesipat 1:d9e840c48b1e 6 using namespace std;
ruesipat 1:d9e840c48b1e 7
ruesipat 1:d9e840c48b1e 8 const float Drive::DRIVINGSPEED = 1000.0f;//Fahrgeschwindigkeit
ruesipat 1:d9e840c48b1e 9 const int Drive::DRIVINGCOUNTS = 10000; //Entspricht Strecke von 20cm
ruesipat 1:d9e840c48b1e 10
ruesipat 1:d9e840c48b1e 11 Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront):
ruesipat 1:d9e840c48b1e 12 kontrastSensor(kontrastSensor),
ruesipat 1:d9e840c48b1e 13 counterLeft(counterLeft),
ruesipat 1:d9e840c48b1e 14 counterRight(counterRight),
ruesipat 1:d9e840c48b1e 15 controller(controller)
ruesipat 1:d9e840c48b1e 16 {
ruesipat 1:d9e840c48b1e 17 this->distanceRight = distanceRight;
ruesipat 1:d9e840c48b1e 18 this->distanceFront = distanceFront;
ruesipat 1:d9e840c48b1e 19 this->distanceLeftFront = distanceLeftFront;
ruesipat 1:d9e840c48b1e 20 }
ruesipat 1:d9e840c48b1e 21
ruesipat 1:d9e840c48b1e 22 Drive::~Drive() {}
ruesipat 1:d9e840c48b1e 23
ruesipat 1:d9e840c48b1e 24
ruesipat 1:d9e840c48b1e 25 void Drive::driving()
ruesipat 1:d9e840c48b1e 26 {
ruesipat 1:d9e840c48b1e 27
ruesipat 1:d9e840c48b1e 28 int countsRight = counterRight.read(); //EncoderCounts auslesen
ruesipat 1:d9e840c48b1e 29 int countsRight0 = countsRight; //ReferenzCounts setzten
ruesipat 1:d9e840c48b1e 30 int countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 31 int countsLeft0 = countsLeft;
ruesipat 1:d9e840c48b1e 32
ruesipat 1:d9e840c48b1e 33 printf("Los gehts\n");
ruesipat 1:d9e840c48b1e 34
ruesipat 1:d9e840c48b1e 35 while((countsRight >= countsRight0 - DRIVINGCOUNTS) && (countsLeft <= countsLeft0 + DRIVINGCOUNTS)){
ruesipat 1:d9e840c48b1e 36
ruesipat 1:d9e840c48b1e 37 //kontrastSensor.check();
ruesipat 1:d9e840c48b1e 38 controller.setDesiredSpeedRight(DRIVINGSPEED);
ruesipat 1:d9e840c48b1e 39 controller.setDesiredSpeedLeft(-DRIVINGSPEED);
ruesipat 1:d9e840c48b1e 40 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 41 countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 42 //printf("CountsRight%d\n", countsRight);
ruesipat 1:d9e840c48b1e 43 //printf("CountsLeft%d\n", countsLeft);
ruesipat 1:d9e840c48b1e 44
ruesipat 1:d9e840c48b1e 45 //parallelitaet und center pruefen!!!!!!!!!!!!
ruesipat 1:d9e840c48b1e 46
ruesipat 1:d9e840c48b1e 47 }
ruesipat 1:d9e840c48b1e 48 controller.setDesiredSpeedRight(0.0f);
ruesipat 1:d9e840c48b1e 49 controller.setDesiredSpeedLeft(0.0f);
ruesipat 1:d9e840c48b1e 50
ruesipat 1:d9e840c48b1e 51 }