mit pathdingsbums
Dependencies: mbed
Fork of MicroMouse_MASTER_TWO by
Drive.cpp@8:1c8a747c49c8, 2018-05-16 (annotated)
- Committer:
- TheDarkDurzo
- Date:
- Wed May 16 12:15:23 2018 +0000
- Revision:
- 8:1c8a747c49c8
- Parent:
- 7:5ef09519a6e9
mit pathfinding
Who changed what in which revision?
User | Revision | Line number | New 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 | } |