PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_SIX

---

Dependencies:   mbed

Fork of MicroMouse_MASTER_FIVE by PES2_R2D2.0

Drive.cpp@11:8c8dba56bfda, 2018-05-23 (annotated)

Committer:
TheDarkDurzo
Date:
Wed May 23 17:59:26 2018 +0000
Revision:
11:8c8dba56bfda
Parent:
10:e8110fb94686 
---

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
TheDarkDurzo 11:8c8dba56bfda 10 const float Drive::FRONTDISTANCE = 73.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
TheDarkDurzo 11:8c8dba56bfda 12 const int Drive::DRIVINGCOUNTS = 1390; //Entspricht Strecke von 20cm //DONT TOUCH /1773 //1800 //1350 /////1390
ruesipat 1:d9e840c48b1e 13
ruesipat 9:ab19796bf14a 14 Drive::Drive(KontrastSensor& kontrastSensor, EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, int& dontStop, int& modeStart, int& path, int& pathNext):
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 9:ab19796bf14a 23 dontStop(dontStop),
ruesipat 9:ab19796bf14a 24 modeStart(modeStart),
ruesipat 9:ab19796bf14a 25 path(path),
ruesipat 9:ab19796bf14a 26 pathNext(pathNext)
ruesipat 5:b8b1a979b0d5 27
ruesipat 1:d9e840c48b1e 28 {
ruesipat 4:e3f388933954 29
ruesipat 1:d9e840c48b1e 30 }
ruesipat 1:d9e840c48b1e 31
ruesipat 1:d9e840c48b1e 32 Drive::~Drive() {}
ruesipat 1:d9e840c48b1e 33
ruesipat 1:d9e840c48b1e 34
ruesipat 1:d9e840c48b1e 35 void Drive::driving()
ruesipat 1:d9e840c48b1e 36 {
ruesipat 1:d9e840c48b1e 37
ruesipat 5:b8b1a979b0d5 38 controller.reset();
ruesipat 7:5ef09519a6e9 39
ruesipat 5:b8b1a979b0d5 40 int countsRight = counterRight.read(); //EncoderCounts auslesen
ruesipat 9:ab19796bf14a 41 int countsLeft = counterLeft.read();
ruesipat 5:b8b1a979b0d5 42 int countsRight0 = countsRight; //ReferenzCounts setzten
ruesipat 5:b8b1a979b0d5 43 int countsLeft0 = countsLeft;
ruesipat 5:b8b1a979b0d5 44
ruesipat 7:5ef09519a6e9 45 float parallelDif = 0.0f;
ruesipat 7:5ef09519a6e9 46 float rightLeftDif = 0.0f;
ruesipat 7:5ef09519a6e9 47 float leftDif = 0.0f;
ruesipat 7:5ef09519a6e9 48 float rightDif = 0.0f;
ruesipat 5:b8b1a979b0d5 49
ruesipat 7:5ef09519a6e9 50 float speedCorrection = 0.0f;
ruesipat 7:5ef09519a6e9 51 int countCorrection = 0;
ruesipat 9:ab19796bf14a 52
ruesipat 7:5ef09519a6e9 53 float softStart = DRIVINGSPEED;
ruesipat 7:5ef09519a6e9 54 float slowdown = 0.0f;
ruesipat 7:5ef09519a6e9 55
ruesipat 9:ab19796bf14a 56 if(dontStop == 2) { //geradeaus
ruesipat 7:5ef09519a6e9 57 softStart =0.0f;
TheDarkDurzo 11:8c8dba56bfda 58
TheDarkDurzo 11:8c8dba56bfda 59 /********/
TheDarkDurzo 11:8c8dba56bfda 60 //countCorrection = -400.0f;
TheDarkDurzo 11:8c8dba56bfda 61 /*******/
ruesipat 7:5ef09519a6e9 62 }
ruesipat 5:b8b1a979b0d5 63
ruesipat 5:b8b1a979b0d5 64 int drive;
ruesipat 5:b8b1a979b0d5 65
ruesipat 5:b8b1a979b0d5 66
ruesipat 5:b8b1a979b0d5 67 //Abfangen wenn Wand vorne dass sicher nicht vorwärts gefahren wird
ruesipat 5:b8b1a979b0d5 68
ruesipat 5:b8b1a979b0d5 69 if(irSensor1.read() < FRONTDISTANCE) {
ruesipat 5:b8b1a979b0d5 70 drive = 0;
ruesipat 5:b8b1a979b0d5 71 } else {
ruesipat 5:b8b1a979b0d5 72 drive = 1;
ruesipat 5:b8b1a979b0d5 73 }
ruesipat 5:b8b1a979b0d5 74
ruesipat 7:5ef09519a6e9 75 while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) {
ruesipat 5:b8b1a979b0d5 76
ruesipat 6:a09d2ee3b82e 77 kontrastSensor.check();
ruesipat 1:d9e840c48b1e 78 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 79 countsLeft = counterLeft.read();
ruesipat 7:5ef09519a6e9 80 controller.setDesiredSpeedRight(-softStart + DRIVINGSPEED - speedCorrection - slowdown); //Korrektur passt Geschwindigkeit an beiden Raedern an //slowdown
ruesipat 7:5ef09519a6e9 81 controller.setDesiredSpeedLeft(softStart - DRIVINGSPEED - speedCorrection + slowdown); //slowdown
ruesipat 5:b8b1a979b0d5 82
ruesipat 5:b8b1a979b0d5 83
ruesipat 7:5ef09519a6e9 84 //Ermittlung der Differenz Hinten-Vorne (PARALLEL)
ruesipat 9:ab19796bf14a 85
ruesipat 7:5ef09519a6e9 86 if((irSensor3.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
ruesipat 7:5ef09519a6e9 87
ruesipat 5:b8b1a979b0d5 88 parallelDif = irSensor3.read()-irSensor2.read(); //differenz hinten vorne bestimmen
ruesipat 7:5ef09519a6e9 89
ruesipat 5:b8b1a979b0d5 90 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 7:5ef09519a6e9 91
ruesipat 5:b8b1a979b0d5 92 parallelDif = 0;
ruesipat 5:b8b1a979b0d5 93 }
ruesipat 5:b8b1a979b0d5 94
ruesipat 5:b8b1a979b0d5 95
ruesipat 7:5ef09519a6e9 96 //Ermittlung der Differenz Rechts-Links (LINKS UND RECHTS WAND VORHANDEN)
ruesipat 9:ab19796bf14a 97
ruesipat 7:5ef09519a6e9 98 if((irSensor0.read() < 120.0f) && (irSensor2.read() < 120.0f)) { //irSensor0 => sensorRight irSensor2 => sensorLeftFornt
ruesipat 7:5ef09519a6e9 99
ruesipat 5:b8b1a979b0d5 100 rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen
ruesipat 5:b8b1a979b0d5 101
ruesipat 5:b8b1a979b0d5 102 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 5:b8b1a979b0d5 103
ruesipat 5:b8b1a979b0d5 104 rightLeftDif = 0;
ruesipat 5:b8b1a979b0d5 105
ruesipat 7:5ef09519a6e9 106 //Ermittlung der Differenz links auf ReferenzWert
ruesipat 7:5ef09519a6e9 107 if(irSensor2.read() < 120.0f) { //irSensor2 => sensorLeftFornt
ruesipat 7:5ef09519a6e9 108
ruesipat 7:5ef09519a6e9 109 leftDif = 60.0f - irSensor2.read(); //differenz links rechts bestimmen
ruesipat 7:5ef09519a6e9 110
ruesipat 7:5ef09519a6e9 111 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 7:5ef09519a6e9 112
ruesipat 7:5ef09519a6e9 113 leftDif = 0;
ruesipat 7:5ef09519a6e9 114 }
ruesipat 7:5ef09519a6e9 115
ruesipat 7:5ef09519a6e9 116
ruesipat 7:5ef09519a6e9 117 //Ermittlung der Differenz rechts auf Referenzwert
ruesipat 9:ab19796bf14a 118
ruesipat 7:5ef09519a6e9 119 if(irSensor0.read() < 120.0f) { //irSensor0 => sensorRight
ruesipat 7:5ef09519a6e9 120
ruesipat 7:5ef09519a6e9 121 rightDif = irSensor0.read() - 60.0f; //differenz links rechts bestimmen
ruesipat 7:5ef09519a6e9 122
ruesipat 7:5ef09519a6e9 123 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 7:5ef09519a6e9 124
ruesipat 7:5ef09519a6e9 125 rightDif = 0;
ruesipat 7:5ef09519a6e9 126 }
ruesipat 5:b8b1a979b0d5 127 }
ruesipat 5:b8b1a979b0d5 128
ruesipat 5:b8b1a979b0d5 129
ruesipat 9:ab19796bf14a 130 //Berechung Korrektur
ruesipat 5:b8b1a979b0d5 131
TheDarkDurzo 11:8c8dba56bfda 132 speedCorrection = ((0.45f * rightLeftDif) + (0.7f * parallelDif) + (0.9f * leftDif) + (0.9f * rightDif)); //DONT TOUCH 0.35 0.7 0.7 0.7
ruesipat 7:5ef09519a6e9 133
ruesipat 7:5ef09519a6e9 134
ruesipat 7:5ef09519a6e9 135 //Anfahrrampe damit die Raeder nicht durchdrehen
ruesipat 9:ab19796bf14a 136
ruesipat 7:5ef09519a6e9 137 softStart = softStart - 3.5f;
ruesipat 7:5ef09519a6e9 138 if (softStart <= 0.0f) {
ruesipat 7:5ef09519a6e9 139 softStart = 0.0f;
ruesipat 7:5ef09519a6e9 140 }
ruesipat 5:b8b1a979b0d5 141
ruesipat 5:b8b1a979b0d5 142
ruesipat 7:5ef09519a6e9 143 //Kontrolle ob vorne Wand...wenn vorhanden nach vorderer Wand ausrichten und anhalten ansonst nur nach counts anhalten
ruesipat 9:ab19796bf14a 144
ruesipat 5:b8b1a979b0d5 145 if(irSensor1.read() < 150.0f) { //slow down
ruesipat 5:b8b1a979b0d5 146
ruesipat 7:5ef09519a6e9 147 countCorrection = 5000;
ruesipat 7:5ef09519a6e9 148 slowdown = slowdown + 5.0f;
ruesipat 7:5ef09519a6e9 149 if (slowdown >= DRIVINGSPEED - 20.0f) {
ruesipat 9:ab19796bf14a 150
ruesipat 7:5ef09519a6e9 151 slowdown = DRIVINGSPEED - 20.0f;
ruesipat 7:5ef09519a6e9 152 }
ruesipat 7:5ef09519a6e9 153 if (irSensor1.read() < FRONTDISTANCE) {
ruesipat 5:b8b1a979b0d5 154
ruesipat 5:b8b1a979b0d5 155 drive = 0;
ruesipat 9:ab19796bf14a 156 }
ruesipat 5:b8b1a979b0d5 157
TheDarkDurzo 11:8c8dba56bfda 158 } else if((((DRIVINGCOUNTS - countsRight) < 250) || ((countsLeft + DRIVINGCOUNTS) < 250)) && ((irSensor2.read() > 120.0f) && ((modeStart != 1) || (pathNext == 2)))) { //Anhaltrampe wenn nach counts gefahren
ruesipat 7:5ef09519a6e9 159 slowdown = slowdown + 5.0f;
ruesipat 7:5ef09519a6e9 160 if (slowdown >= DRIVINGSPEED - 20.0f) {
ruesipat 9:ab19796bf14a 161
ruesipat 7:5ef09519a6e9 162 slowdown = DRIVINGSPEED - 20.0f;
ruesipat 9:ab19796bf14a 163 }
TheDarkDurzo 11:8c8dba56bfda 164 } else if((((DRIVINGCOUNTS - countsRight) < 250) || ((countsLeft + DRIVINGCOUNTS) < 250)) && ((irSensor0.read() > 120.0f) && ((modeStart == 1) && (pathNext == 3)))) { //Anhaltrampe wenn nach counts gefahren
ruesipat 9:ab19796bf14a 165 slowdown = slowdown + 5.0f;
ruesipat 9:ab19796bf14a 166 if (slowdown >= DRIVINGSPEED - 20.0f) {
ruesipat 9:ab19796bf14a 167
ruesipat 9:ab19796bf14a 168 slowdown = DRIVINGSPEED - 20.0f;
ruesipat 9:ab19796bf14a 169 }
ruesipat 9:ab19796bf14a 170 } else {
ruesipat 9:ab19796bf14a 171
ruesipat 9:ab19796bf14a 172 slowdown = 0.0f;
ruesipat 7:5ef09519a6e9 173 }
ruesipat 5:b8b1a979b0d5 174
ruesipat 9:ab19796bf14a 175 wait(0.01f);
ruesipat 9:ab19796bf14a 176 }//Ende Whileschleife Drive...
ruesipat 9:ab19796bf14a 177 }