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@7:5ef09519a6e9, 2018-05-07 (annotated)

Committer:
ruesipat
Date:
Mon May 07 18:52:04 2018 +0000
Revision:
7:5ef09519a6e9
Parent:
6:a09d2ee3b82e
Child:
9:ab19796bf14a
VERSION 7.5.18

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 7:5ef09519a6e9 10 const float Drive::FRONTDISTANCE = 70.0f; //Abstand Sensor zur VorderWand //DONT TOUCH //62.0f //55.0 110.0
ruesipat 7:5ef09519a6e9 11 const float Drive::DRIVINGSPEED = 130.0f;//Fahrgeschwindigkeit Drehzahl in [rpm] //130.0f
ruesipat 7:5ef09519a6e9 12 const int Drive::DRIVINGCOUNTS = 1390; //Entspricht Strecke von 20cm //DONT TOUCH /1773 //1800 //1350 /1370
ruesipat 1:d9e840c48b1e 13
ruesipat 7:5ef09519a6e9 14 Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop):
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 7:5ef09519a6e9 22 irSensor3(irSensor3),
ruesipat 7:5ef09519a6e9 23 dontStop(dontStop)
ruesipat 5:b8b1a979b0d5 24
ruesipat 1:d9e840c48b1e 25 {
ruesipat 4:e3f388933954 26
ruesipat 1:d9e840c48b1e 27 }
ruesipat 1:d9e840c48b1e 28
ruesipat 1:d9e840c48b1e 29 Drive::~Drive() {}
ruesipat 1:d9e840c48b1e 30
ruesipat 1:d9e840c48b1e 31
ruesipat 1:d9e840c48b1e 32 void Drive::driving()
ruesipat 1:d9e840c48b1e 33 {
ruesipat 1:d9e840c48b1e 34
ruesipat 5:b8b1a979b0d5 35 controller.reset();
ruesipat 7:5ef09519a6e9 36
ruesipat 5:b8b1a979b0d5 37 int countsRight = counterRight.read(); //EncoderCounts auslesen
ruesipat 5:b8b1a979b0d5 38 int countsLeft = counterLeft.read(); //0 - 32767
ruesipat 5:b8b1a979b0d5 39
ruesipat 5:b8b1a979b0d5 40 //printf("CountsRight%d\n\r", countsRight);
ruesipat 5:b8b1a979b0d5 41 //printf(" CountsLeft%d\n\r", countsLeft);
ruesipat 5:b8b1a979b0d5 42
ruesipat 5:b8b1a979b0d5 43
ruesipat 5:b8b1a979b0d5 44 int countsRight0 = countsRight; //ReferenzCounts setzten
ruesipat 5:b8b1a979b0d5 45 int countsLeft0 = countsLeft;
ruesipat 5:b8b1a979b0d5 46
ruesipat 7:5ef09519a6e9 47 float parallelDif = 0.0f;
ruesipat 7:5ef09519a6e9 48 float rightLeftDif = 0.0f;
ruesipat 7:5ef09519a6e9 49 float leftDif = 0.0f;
ruesipat 7:5ef09519a6e9 50 float rightDif = 0.0f;
ruesipat 5:b8b1a979b0d5 51
ruesipat 7:5ef09519a6e9 52 float speedCorrection = 0.0f;
ruesipat 7:5ef09519a6e9 53 int countCorrection = 0;
ruesipat 7:5ef09519a6e9 54
ruesipat 7:5ef09519a6e9 55
ruesipat 7:5ef09519a6e9 56 float softStart = DRIVINGSPEED;
ruesipat 7:5ef09519a6e9 57 float slowdown = 0.0f;
ruesipat 7:5ef09519a6e9 58
ruesipat 7:5ef09519a6e9 59
ruesipat 7:5ef09519a6e9 60 if(dontStop == 2){ //geradeaus
ruesipat 7:5ef09519a6e9 61 softStart =0.0f;
ruesipat 7:5ef09519a6e9 62 }
ruesipat 7:5ef09519a6e9 63
ruesipat 5:b8b1a979b0d5 64
ruesipat 5:b8b1a979b0d5 65 int drive;
ruesipat 5:b8b1a979b0d5 66
ruesipat 5:b8b1a979b0d5 67
ruesipat 5:b8b1a979b0d5 68 //Abfangen wenn Wand vorne dass sicher nicht vorwärts gefahren wird
ruesipat 5:b8b1a979b0d5 69
ruesipat 5:b8b1a979b0d5 70 if(irSensor1.read() < FRONTDISTANCE) {
ruesipat 5:b8b1a979b0d5 71 drive = 0;
ruesipat 5:b8b1a979b0d5 72 } else {
ruesipat 5:b8b1a979b0d5 73 drive = 1;
ruesipat 5:b8b1a979b0d5 74 }
ruesipat 5:b8b1a979b0d5 75
ruesipat 5:b8b1a979b0d5 76
ruesipat 4:e3f388933954 77 //printf("Los gehts\n");
ruesipat 5:b8b1a979b0d5 78
ruesipat 7:5ef09519a6e9 79 while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) {
ruesipat 5:b8b1a979b0d5 80
ruesipat 6:a09d2ee3b82e 81 kontrastSensor.check();
ruesipat 1:d9e840c48b1e 82 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 83 countsLeft = counterLeft.read();
ruesipat 7:5ef09519a6e9 84 controller.setDesiredSpeedRight(-softStart + DRIVINGSPEED - speedCorrection - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an //slowdown
ruesipat 7:5ef09519a6e9 85 controller.setDesiredSpeedLeft(softStart - DRIVINGSPEED - speedCorrection + slowdown); //slowdown
ruesipat 5:b8b1a979b0d5 86
ruesipat 2:592f01278db4 87 //printf("CountsRight%d\n\r", countsRight);
ruesipat 4:e3f388933954 88 //printf(" CountsLeft%d\n\r", countsLeft);
ruesipat 5:b8b1a979b0d5 89
ruesipat 5:b8b1a979b0d5 90
ruesipat 5:b8b1a979b0d5 91 //printf("correction: %.0f\n\r", correction);
ruesipat 5:b8b1a979b0d5 92
ruesipat 5:b8b1a979b0d5 93
ruesipat 7:5ef09519a6e9 94
ruesipat 5:b8b1a979b0d5 95 //Bereit fuer neuen Durchgang
ruesipat 7:5ef09519a6e9 96 //speedCorrection = 0.0f;
ruesipat 7:5ef09519a6e9 97 //leftDif = 0.0f;
ruesipat 7:5ef09519a6e9 98 //rightDif = 0.0f;
ruesipat 7:5ef09519a6e9 99 //slowdown = 0.0f;
ruesipat 5:b8b1a979b0d5 100
ruesipat 5:b8b1a979b0d5 101
ruesipat 7:5ef09519a6e9 102 //Ermittlung der Differenz Hinten-Vorne (PARALLEL)
ruesipat 7:5ef09519a6e9 103 if((irSensor3.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
ruesipat 7:5ef09519a6e9 104
ruesipat 5:b8b1a979b0d5 105 parallelDif = irSensor3.read()-irSensor2.read(); //differenz hinten vorne bestimmen
ruesipat 5:b8b1a979b0d5 106 //printf(" DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
ruesipat 5:b8b1a979b0d5 107 //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
ruesipat 5:b8b1a979b0d5 108 //printf(" parallelDif: %.0f \n\r", parallelDif);
ruesipat 7:5ef09519a6e9 109
ruesipat 5:b8b1a979b0d5 110 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 7:5ef09519a6e9 111
ruesipat 5:b8b1a979b0d5 112 parallelDif = 0;
ruesipat 5:b8b1a979b0d5 113 }
ruesipat 5:b8b1a979b0d5 114
ruesipat 5:b8b1a979b0d5 115
ruesipat 7:5ef09519a6e9 116 //Ermittlung der Differenz Rechts-Links (LINKS UND RECHTS WAND VORHANDEN)
ruesipat 7:5ef09519a6e9 117 if((irSensor0.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor0 => sensorRight irSensor2 => sensorLeftFornt
ruesipat 7:5ef09519a6e9 118
ruesipat 5:b8b1a979b0d5 119 rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen
ruesipat 5:b8b1a979b0d5 120 //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
ruesipat 5:b8b1a979b0d5 121 //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
ruesipat 5:b8b1a979b0d5 122 //printf(" rightLeftDif: %.0f \n\r", rightLeftDif);
ruesipat 5:b8b1a979b0d5 123
ruesipat 5:b8b1a979b0d5 124 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 5:b8b1a979b0d5 125
ruesipat 5:b8b1a979b0d5 126 rightLeftDif = 0;
ruesipat 5:b8b1a979b0d5 127
ruesipat 7:5ef09519a6e9 128 //Ermittlung der Differenz links auf ReferenzWert
ruesipat 7:5ef09519a6e9 129 if(irSensor2.read() < 120.0f) { //irSensor2 => sensorLeftFornt
ruesipat 7:5ef09519a6e9 130
ruesipat 7:5ef09519a6e9 131 leftDif = 60.0f - irSensor2.read(); //differenz links rechts bestimmen
ruesipat 7:5ef09519a6e9 132 //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
ruesipat 7:5ef09519a6e9 133 //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
ruesipat 7:5ef09519a6e9 134 //printf(" leftDif: %.0f \n\r", leftDif);
ruesipat 7:5ef09519a6e9 135
ruesipat 7:5ef09519a6e9 136 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 7:5ef09519a6e9 137
ruesipat 7:5ef09519a6e9 138 leftDif = 0;
ruesipat 7:5ef09519a6e9 139
ruesipat 7:5ef09519a6e9 140 }
ruesipat 7:5ef09519a6e9 141
ruesipat 7:5ef09519a6e9 142
ruesipat 7:5ef09519a6e9 143 //Ermittlung der Differenz rechts auf Referenzwert
ruesipat 7:5ef09519a6e9 144 if(irSensor0.read() < 120.0f) { //irSensor0 => sensorRight
ruesipat 7:5ef09519a6e9 145
ruesipat 7:5ef09519a6e9 146 rightDif = irSensor0.read() - 60.0f; //differenz links rechts bestimmen
ruesipat 7:5ef09519a6e9 147 //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
ruesipat 7:5ef09519a6e9 148 //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
ruesipat 7:5ef09519a6e9 149 //printf(" rightDif: %.0f \n\r", rightDif);
ruesipat 7:5ef09519a6e9 150
ruesipat 7:5ef09519a6e9 151 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 7:5ef09519a6e9 152
ruesipat 7:5ef09519a6e9 153 rightDif = 0;
ruesipat 7:5ef09519a6e9 154
ruesipat 7:5ef09519a6e9 155 }
ruesipat 7:5ef09519a6e9 156
ruesipat 7:5ef09519a6e9 157
ruesipat 7:5ef09519a6e9 158
ruesipat 7:5ef09519a6e9 159
ruesipat 5:b8b1a979b0d5 160 }
ruesipat 5:b8b1a979b0d5 161
ruesipat 5:b8b1a979b0d5 162
ruesipat 5:b8b1a979b0d5 163
ruesipat 5:b8b1a979b0d5 164 //Berechung Korrektur
ruesipat 7:5ef09519a6e9 165 speedCorrection = ((0.35f * rightLeftDif) + (0.7f * parallelDif) + (0.7f * leftDif) + (0.7f * rightDif)); //DONT TOUCH 0.2 0.5
ruesipat 7:5ef09519a6e9 166
ruesipat 7:5ef09519a6e9 167
ruesipat 7:5ef09519a6e9 168
ruesipat 7:5ef09519a6e9 169 //Anfahrrampe damit die Raeder nicht durchdrehen
ruesipat 7:5ef09519a6e9 170 softStart = softStart - 3.5f;
ruesipat 7:5ef09519a6e9 171 if (softStart <= 0.0f) {
ruesipat 7:5ef09519a6e9 172 softStart = 0.0f;
ruesipat 7:5ef09519a6e9 173 }
ruesipat 7:5ef09519a6e9 174 //printf(" softStart: %.0f\n\r", softStart);
ruesipat 7:5ef09519a6e9 175
ruesipat 7:5ef09519a6e9 176
ruesipat 7:5ef09519a6e9 177
ruesipat 5:b8b1a979b0d5 178
ruesipat 5:b8b1a979b0d5 179
ruesipat 5:b8b1a979b0d5 180
ruesipat 5:b8b1a979b0d5 181
ruesipat 7:5ef09519a6e9 182 //Kontrolle ob vorne Wand...wenn vorhanden nach vorderer Wand ausrichten und anhalten ansonst nur nach counts anhalten
ruesipat 5:b8b1a979b0d5 183 if(irSensor1.read() < 150.0f) { //slow down
ruesipat 5:b8b1a979b0d5 184
ruesipat 7:5ef09519a6e9 185 countCorrection = 5000;
ruesipat 7:5ef09519a6e9 186 //slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED; //vorderer max abstand
ruesipat 7:5ef09519a6e9 187 slowdown = slowdown + 5.0f;
ruesipat 7:5ef09519a6e9 188 if (slowdown >= DRIVINGSPEED - 20.0f) {
ruesipat 7:5ef09519a6e9 189 slowdown = DRIVINGSPEED - 20.0f;
ruesipat 7:5ef09519a6e9 190 }
ruesipat 7:5ef09519a6e9 191 if (irSensor1.read() < FRONTDISTANCE) {
ruesipat 5:b8b1a979b0d5 192
ruesipat 5:b8b1a979b0d5 193 drive = 0;
ruesipat 5:b8b1a979b0d5 194
TheDarkDurzo 3:2ec7cf8bc3fc 195 }
ruesipat 7:5ef09519a6e9 196
ruesipat 7:5ef09519a6e9 197 } else if(((DRIVINGCOUNTS - countsRight) < 300 || (countsLeft + DRIVINGCOUNTS) < 300) && (irSensor2.read() > 120.0f)) { //Anhaltrampe wenn nach counts gefahren
ruesipat 7:5ef09519a6e9 198 slowdown = slowdown + 5.0f;
ruesipat 7:5ef09519a6e9 199 if (slowdown >= DRIVINGSPEED - 20.0f) {
ruesipat 7:5ef09519a6e9 200 slowdown = DRIVINGSPEED - 20.0f;
ruesipat 7:5ef09519a6e9 201 }
ruesipat 7:5ef09519a6e9 202
ruesipat 7:5ef09519a6e9 203 } else{
ruesipat 7:5ef09519a6e9 204 slowdown = 0.0f;
ruesipat 5:b8b1a979b0d5 205 }
ruesipat 7:5ef09519a6e9 206
ruesipat 7:5ef09519a6e9 207
ruesipat 7:5ef09519a6e9 208 //if(dontStop == 2){ //geradeaus
ruesipat 7:5ef09519a6e9 209 // slowdown = 0.0f;
ruesipat 7:5ef09519a6e9 210 //}
ruesipat 5:b8b1a979b0d5 211
ruesipat 5:b8b1a979b0d5 212
ruesipat 7:5ef09519a6e9 213
ruesipat 7:5ef09519a6e9 214 wait(0.01f);
ruesipat 5:b8b1a979b0d5 215 }//Ende Whileschleife Drive...
ruesipat 5:b8b1a979b0d5 216
ruesipat 7:5ef09519a6e9 217 //controller.setDesiredSpeedRight(0.5f); //0.5f
ruesipat 7:5ef09519a6e9 218 //controller.setDesiredSpeedLeft(-0.5f); //-0.5f
ruesipat 1:d9e840c48b1e 219
ruesipat 1:d9e840c48b1e 220 }