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@3:2ec7cf8bc3fc, 2018-04-08 (annotated)

Committer:
TheDarkDurzo
Date:
Sun Apr 08 19:17:01 2018 +0000
Revision:
3:2ec7cf8bc3fc
Parent:
2:592f01278db4
Child:
4:e3f388933954
dunno

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
TheDarkDurzo 3:2ec7cf8bc3fc 6
ruesipat 1:d9e840c48b1e 7 using namespace std;
ruesipat 1:d9e840c48b1e 8
ruesipat 2:592f01278db4 9 const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit
ruesipat 2:592f01278db4 10 const int Drive::DRIVINGCOUNTS = 1712; //Entspricht Strecke von 20cm
ruesipat 1:d9e840c48b1e 11
ruesipat 2:592f01278db4 12 Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, float distanceRight, float distanceFront, float distanceLeftFront, float distanceLeftBack):
ruesipat 1:d9e840c48b1e 13 kontrastSensor(kontrastSensor),
ruesipat 1:d9e840c48b1e 14 counterLeft(counterLeft),
ruesipat 1:d9e840c48b1e 15 counterRight(counterRight),
ruesipat 1:d9e840c48b1e 16 controller(controller)
ruesipat 1:d9e840c48b1e 17 {
ruesipat 1:d9e840c48b1e 18 this->distanceRight = distanceRight;
ruesipat 1:d9e840c48b1e 19 this->distanceFront = distanceFront;
ruesipat 1:d9e840c48b1e 20 this->distanceLeftFront = distanceLeftFront;
ruesipat 2:592f01278db4 21 this->distanceLeftBack = distanceLeftBack;
ruesipat 1:d9e840c48b1e 22 }
ruesipat 1:d9e840c48b1e 23
ruesipat 1:d9e840c48b1e 24 Drive::~Drive() {}
ruesipat 1:d9e840c48b1e 25
ruesipat 1:d9e840c48b1e 26
ruesipat 1:d9e840c48b1e 27 void Drive::driving()
ruesipat 1:d9e840c48b1e 28 {
ruesipat 1:d9e840c48b1e 29
ruesipat 1:d9e840c48b1e 30 int countsRight = counterRight.read(); //EncoderCounts auslesen
ruesipat 1:d9e840c48b1e 31 int countsRight0 = countsRight; //ReferenzCounts setzten
ruesipat 1:d9e840c48b1e 32 int countsLeft = counterLeft.read();
ruesipat 1:d9e840c48b1e 33 int countsLeft0 = countsLeft;
TheDarkDurzo 3:2ec7cf8bc3fc 34 int totCLeft, totCRight, counter, sumCorrection;
TheDarkDurzo 3:2ec7cf8bc3fc 35 int corLeft = 0;
TheDarkDurzo 3:2ec7cf8bc3fc 36 int corRight = 0;
TheDarkDurzo 3:2ec7cf8bc3fc 37
ruesipat 1:d9e840c48b1e 38
ruesipat 1:d9e840c48b1e 39 printf("Los gehts\n");
ruesipat 1:d9e840c48b1e 40
TheDarkDurzo 3:2ec7cf8bc3fc 41 while((countsRight <= countsRight0 + DRIVINGCOUNTS) && (countsLeft >= countsLeft0 - DRIVINGCOUNTS) || distanceFront < 1.5f){ //evt auch noch wand vorne ermitteln und so!!!!!!!!!!!!!
ruesipat 1:d9e840c48b1e 42
ruesipat 1:d9e840c48b1e 43 //kontrastSensor.check();
TheDarkDurzo 3:2ec7cf8bc3fc 44 controller.setDesiredSpeedRight(DRIVINGSPEED + corRight);
TheDarkDurzo 3:2ec7cf8bc3fc 45 controller.setDesiredSpeedLeft(-DRIVINGSPEED + corLeft);
ruesipat 1:d9e840c48b1e 46 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 47 countsLeft = counterLeft.read();
ruesipat 2:592f01278db4 48 //printf("CountsRight%d\n\r", countsRight);
ruesipat 2:592f01278db4 49 //printf("CountsLeft%d\n\r", countsLeft);
ruesipat 1:d9e840c48b1e 50
TheDarkDurzo 3:2ec7cf8bc3fc 51 totCLeft =+countsLeft;
TheDarkDurzo 3:2ec7cf8bc3fc 52 totCRight =+countsRight;
TheDarkDurzo 3:2ec7cf8bc3fc 53 counter++;
ruesipat 2:592f01278db4 54
TheDarkDurzo 3:2ec7cf8bc3fc 55 if(counter < 100){ //Wait Time until corretion value added
TheDarkDurzo 3:2ec7cf8bc3fc 56 sumCorrection = totCLeft-totCRight;
TheDarkDurzo 3:2ec7cf8bc3fc 57 if(sumCorrection <0){
TheDarkDurzo 3:2ec7cf8bc3fc 58 corLeft =+1; //Driving Speed Left Tire Addition
TheDarkDurzo 3:2ec7cf8bc3fc 59 corRight=0;
TheDarkDurzo 3:2ec7cf8bc3fc 60 }else if(sumCorrection >0){
TheDarkDurzo 3:2ec7cf8bc3fc 61 corLeft=0;
TheDarkDurzo 3:2ec7cf8bc3fc 62 corRight=+1; //Driving Speed Right Tire Addition
TheDarkDurzo 3:2ec7cf8bc3fc 63 }
TheDarkDurzo 3:2ec7cf8bc3fc 64 counter = 0;
TheDarkDurzo 3:2ec7cf8bc3fc 65 totCLeft = 0;
TheDarkDurzo 3:2ec7cf8bc3fc 66 totCRight = 0;
TheDarkDurzo 3:2ec7cf8bc3fc 67 }
ruesipat 1:d9e840c48b1e 68 }
ruesipat 1:d9e840c48b1e 69 controller.setDesiredSpeedRight(0.0f);
ruesipat 1:d9e840c48b1e 70 controller.setDesiredSpeedLeft(0.0f);
ruesipat 1:d9e840c48b1e 71
ruesipat 1:d9e840c48b1e 72 }