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@6:a09d2ee3b82e, 2018-04-25 (annotated)

Committer:
ruesipat
Date:
Wed Apr 25 12:49:48 2018 +0000
Revision:
6:a09d2ee3b82e
Parent:
5:b8b1a979b0d5
Child:
7:5ef09519a6e9
25.04.2018_MIT Zielerkennung

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ruesipat 1:d9e840c48b1e 1 #include <cmath>
ruesipat 5:b8b1a979b0d5 2 #include "mbed.h"
ruesipat 1:d9e840c48b1e 3 #include "Drive.h"
ruesipat 1:d9e840c48b1e 4
ruesipat 1:d9e840c48b1e 5
ruesipat 1:d9e840c48b1e 6
TheDarkDurzo 3:2ec7cf8bc3fc 7
ruesipat 1:d9e840c48b1e 8 using namespace std;
ruesipat 1:d9e840c48b1e 9
ruesipat 5:b8b1a979b0d5 10 const float Drive::FRONTDISTANCE = 62.0f; //Abstand Sensor zur VorderWand //DONT TOUCH
ruesipat 6:a09d2ee3b82e 11 const float Drive::DRIVINGSPEED = 50.0f;//Fahrgeschwindigkeit Drehzahl in [rpm]
ruesipat 5:b8b1a979b0d5 12 const int Drive::DRIVINGCOUNTS = 1773; //Entspricht Strecke von 20cm //DONT TOUCH
ruesipat 1:d9e840c48b1e 13
ruesipat 4:e3f388933954 14 Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3):
ruesipat 1:d9e840c48b1e 15 kontrastSensor(kontrastSensor),
ruesipat 5:b8b1a979b0d5 16 counterLeft(counterLeft),
ruesipat 1:d9e840c48b1e 17 counterRight(counterRight),
ruesipat 4:e3f388933954 18 controller(controller),
ruesipat 4:e3f388933954 19 irSensor0(irSensor0),
ruesipat 4:e3f388933954 20 irSensor1(irSensor1),
ruesipat 4:e3f388933954 21 irSensor2(irSensor2),
ruesipat 4:e3f388933954 22 irSensor3(irSensor3)
ruesipat 5:b8b1a979b0d5 23
ruesipat 1:d9e840c48b1e 24 {
ruesipat 4:e3f388933954 25
ruesipat 1:d9e840c48b1e 26 }
ruesipat 1:d9e840c48b1e 27
ruesipat 1:d9e840c48b1e 28 Drive::~Drive() {}
ruesipat 1:d9e840c48b1e 29
ruesipat 1:d9e840c48b1e 30
ruesipat 1:d9e840c48b1e 31 void Drive::driving()
ruesipat 1:d9e840c48b1e 32 {
ruesipat 1:d9e840c48b1e 33
ruesipat 5:b8b1a979b0d5 34 controller.reset();
TheDarkDurzo 3:2ec7cf8bc3fc 35
ruesipat 5:b8b1a979b0d5 36 int countsRight = counterRight.read(); //EncoderCounts auslesen
ruesipat 5:b8b1a979b0d5 37 int countsLeft = counterLeft.read(); //0 - 32767
ruesipat 5:b8b1a979b0d5 38
ruesipat 5:b8b1a979b0d5 39 //printf("CountsRight%d\n\r", countsRight);
ruesipat 5:b8b1a979b0d5 40 //printf(" CountsLeft%d\n\r", countsLeft);
ruesipat 5:b8b1a979b0d5 41
ruesipat 5:b8b1a979b0d5 42
ruesipat 5:b8b1a979b0d5 43 int countsRight0 = countsRight; //ReferenzCounts setzten
ruesipat 5:b8b1a979b0d5 44 int countsLeft0 = countsLeft;
ruesipat 5:b8b1a979b0d5 45
ruesipat 5:b8b1a979b0d5 46 float parallelDif = 0;
ruesipat 5:b8b1a979b0d5 47 float rightLeftDif = 0;
ruesipat 5:b8b1a979b0d5 48
ruesipat 5:b8b1a979b0d5 49 float correction = 0;
ruesipat 5:b8b1a979b0d5 50 float slowdown = 0;
ruesipat 5:b8b1a979b0d5 51
ruesipat 5:b8b1a979b0d5 52 int drive;
ruesipat 5:b8b1a979b0d5 53
ruesipat 5:b8b1a979b0d5 54
ruesipat 5:b8b1a979b0d5 55 //Abfangen wenn Wand vorne dass sicher nicht vorwärts gefahren wird
ruesipat 5:b8b1a979b0d5 56
ruesipat 5:b8b1a979b0d5 57 if(irSensor1.read() < FRONTDISTANCE) {
ruesipat 5:b8b1a979b0d5 58 drive = 0;
ruesipat 5:b8b1a979b0d5 59 } else {
ruesipat 5:b8b1a979b0d5 60 drive = 1;
ruesipat 5:b8b1a979b0d5 61 }
ruesipat 5:b8b1a979b0d5 62
ruesipat 5:b8b1a979b0d5 63
ruesipat 4:e3f388933954 64 //printf("Los gehts\n");
ruesipat 5:b8b1a979b0d5 65
ruesipat 5:b8b1a979b0d5 66 while(((countsRight <= countsRight0 + DRIVINGCOUNTS) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS)) && (drive == 1) ) {
ruesipat 5:b8b1a979b0d5 67
ruesipat 6:a09d2ee3b82e 68 kontrastSensor.check();
ruesipat 1:d9e840c48b1e 69 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 70 countsLeft = counterLeft.read();
ruesipat 5:b8b1a979b0d5 71 controller.setDesiredSpeedRight(DRIVINGSPEED - correction - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an
ruesipat 5:b8b1a979b0d5 72 controller.setDesiredSpeedLeft(-DRIVINGSPEED - correction + slowdown);
ruesipat 5:b8b1a979b0d5 73
ruesipat 2:592f01278db4 74 //printf("CountsRight%d\n\r", countsRight);
ruesipat 4:e3f388933954 75 //printf(" CountsLeft%d\n\r", countsLeft);
ruesipat 5:b8b1a979b0d5 76
ruesipat 5:b8b1a979b0d5 77
ruesipat 5:b8b1a979b0d5 78 //printf("correction: %.0f\n\r", correction);
ruesipat 5:b8b1a979b0d5 79
ruesipat 5:b8b1a979b0d5 80
ruesipat 5:b8b1a979b0d5 81 //Bereit fuer neuen Durchgang
ruesipat 5:b8b1a979b0d5 82 correction = 0;
ruesipat 5:b8b1a979b0d5 83 slowdown = 0;
ruesipat 5:b8b1a979b0d5 84
ruesipat 5:b8b1a979b0d5 85
ruesipat 5:b8b1a979b0d5 86 //Ermittlung der Differenz Hinten-Vorne
ruesipat 5:b8b1a979b0d5 87 if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
ruesipat 1:d9e840c48b1e 88
ruesipat 5:b8b1a979b0d5 89 parallelDif = irSensor3.read()-irSensor2.read(); //differenz hinten vorne bestimmen
ruesipat 5:b8b1a979b0d5 90 //printf(" DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
ruesipat 5:b8b1a979b0d5 91 //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
ruesipat 5:b8b1a979b0d5 92 //printf(" parallelDif: %.0f \n\r", parallelDif);
ruesipat 4:e3f388933954 93
ruesipat 5:b8b1a979b0d5 94 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 5:b8b1a979b0d5 95
ruesipat 5:b8b1a979b0d5 96 parallelDif = 0;
ruesipat 5:b8b1a979b0d5 97 }
ruesipat 5:b8b1a979b0d5 98
ruesipat 5:b8b1a979b0d5 99
ruesipat 5:b8b1a979b0d5 100 //Ermittlung der Differenz Rechts-Links
ruesipat 5:b8b1a979b0d5 101 if((irSensor0.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor0 => sensorRight irSensor2 => sensorLeftFornt
ruesipat 5:b8b1a979b0d5 102
ruesipat 5:b8b1a979b0d5 103 rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen
ruesipat 5:b8b1a979b0d5 104 //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
ruesipat 5:b8b1a979b0d5 105 //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
ruesipat 5:b8b1a979b0d5 106 //printf(" rightLeftDif: %.0f \n\r", rightLeftDif);
ruesipat 5:b8b1a979b0d5 107
ruesipat 5:b8b1a979b0d5 108 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 5:b8b1a979b0d5 109
ruesipat 5:b8b1a979b0d5 110 rightLeftDif = 0;
ruesipat 5:b8b1a979b0d5 111
ruesipat 5:b8b1a979b0d5 112 }
ruesipat 5:b8b1a979b0d5 113
ruesipat 5:b8b1a979b0d5 114
ruesipat 5:b8b1a979b0d5 115
ruesipat 5:b8b1a979b0d5 116 //Berechung Korrektur
ruesipat 5:b8b1a979b0d5 117 correction = ((0.1f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH
ruesipat 5:b8b1a979b0d5 118
ruesipat 5:b8b1a979b0d5 119
ruesipat 5:b8b1a979b0d5 120
ruesipat 5:b8b1a979b0d5 121
ruesipat 5:b8b1a979b0d5 122 //Kontrolle ob vorne Wand...Verlangsamen und Anhalten
ruesipat 5:b8b1a979b0d5 123
ruesipat 5:b8b1a979b0d5 124 if(irSensor1.read() < 150.0f) { //slow down
ruesipat 5:b8b1a979b0d5 125
ruesipat 5:b8b1a979b0d5 126 slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED; //vorderer max abstand
ruesipat 5:b8b1a979b0d5 127
ruesipat 5:b8b1a979b0d5 128 if ( slowdown > DRIVINGSPEED) {
ruesipat 5:b8b1a979b0d5 129
ruesipat 5:b8b1a979b0d5 130 drive = 0;
ruesipat 5:b8b1a979b0d5 131
TheDarkDurzo 3:2ec7cf8bc3fc 132 }
ruesipat 5:b8b1a979b0d5 133 }
ruesipat 5:b8b1a979b0d5 134
ruesipat 5:b8b1a979b0d5 135
ruesipat 5:b8b1a979b0d5 136 }//Ende Whileschleife Drive...
ruesipat 5:b8b1a979b0d5 137
ruesipat 6:a09d2ee3b82e 138 controller.setDesiredSpeedRight(0.5f); //0.0f
ruesipat 6:a09d2ee3b82e 139 controller.setDesiredSpeedLeft(0.5f); //0.0f
ruesipat 1:d9e840c48b1e 140
ruesipat 1:d9e840c48b1e 141 }