![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Pathfinding nach rechts funktioniert noch nicht...der rest schon
Dependencies: mbed
Fork of MicroMouse_MASTER_THREE by
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?
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 | 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 | } |