PES2_R2D2.0 / Mbed 2 deprecated MICROMOUSE_KOMPLETT_MIT_ZIEL_SCHNELLER

Fertige Version mit geschwindigkeit 100

Dependencies:   mbed

Fork of Micromouse_alpha_copy_copy by PES2_R2D2.0

Drive.cpp@7:5ff551b098f8, 2018-05-02 (annotated)

Committer:
ruesipat
Date:
Wed May 02 11:53:42 2018 +0000
Revision:
7:5ff551b098f8
Parent:
6:a09d2ee3b82e 
A

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:5ff551b098f8 10 const float Drive::FRONTDISTANCE = 80.0f; //Abstand Sensor zur VorderWand //DONT TOUCH //62.0f //55.0
ruesipat 7:5ff551b098f8 11 const float Drive::DRIVINGSPEED = 100.0f;//Fahrgeschwindigkeit Drehzahl in [rpm]
ruesipat 7:5ff551b098f8 12 const int Drive::DRIVINGCOUNTS = 1480; //Entspricht Strecke von 20cm //DONT TOUCH /1773 //1800
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 7:5ff551b098f8 49 float speedCorrection = 0;
ruesipat 7:5ff551b098f8 50
ruesipat 5:b8b1a979b0d5 51 float slowdown = 0;
ruesipat 7:5ff551b098f8 52 int countCorrection = 0;
ruesipat 5:b8b1a979b0d5 53
ruesipat 5:b8b1a979b0d5 54 int drive;
ruesipat 5:b8b1a979b0d5 55
ruesipat 5:b8b1a979b0d5 56
ruesipat 5:b8b1a979b0d5 57 //Abfangen wenn Wand vorne dass sicher nicht vorwärts gefahren wird
ruesipat 5:b8b1a979b0d5 58
ruesipat 5:b8b1a979b0d5 59 if(irSensor1.read() < FRONTDISTANCE) {
ruesipat 5:b8b1a979b0d5 60 drive = 0;
ruesipat 5:b8b1a979b0d5 61 } else {
ruesipat 5:b8b1a979b0d5 62 drive = 1;
ruesipat 5:b8b1a979b0d5 63 }
ruesipat 5:b8b1a979b0d5 64
ruesipat 5:b8b1a979b0d5 65
ruesipat 4:e3f388933954 66 //printf("Los gehts\n");
ruesipat 5:b8b1a979b0d5 67
ruesipat 7:5ff551b098f8 68 while(((countsRight <= countsRight0 + DRIVINGCOUNTS + countCorrection) || (countsLeft >= countsLeft0 - DRIVINGCOUNTS - countCorrection)) && (drive == 1) ) {
ruesipat 5:b8b1a979b0d5 69
ruesipat 6:a09d2ee3b82e 70 kontrastSensor.check();
ruesipat 1:d9e840c48b1e 71 countsRight = counterRight.read();
ruesipat 1:d9e840c48b1e 72 countsLeft = counterLeft.read();
ruesipat 7:5ff551b098f8 73 controller.setDesiredSpeedRight(DRIVINGSPEED - speedCorrection - slowdown/3); //Korrektur passt Geschwindigkeit an beiden Raedern an
ruesipat 7:5ff551b098f8 74 controller.setDesiredSpeedLeft(-DRIVINGSPEED - speedCorrection + slowdown/3);
ruesipat 5:b8b1a979b0d5 75
ruesipat 2:592f01278db4 76 //printf("CountsRight%d\n\r", countsRight);
ruesipat 4:e3f388933954 77 //printf(" CountsLeft%d\n\r", countsLeft);
ruesipat 5:b8b1a979b0d5 78
ruesipat 5:b8b1a979b0d5 79
ruesipat 5:b8b1a979b0d5 80 //printf("correction: %.0f\n\r", correction);
ruesipat 5:b8b1a979b0d5 81
ruesipat 5:b8b1a979b0d5 82
ruesipat 5:b8b1a979b0d5 83 //Bereit fuer neuen Durchgang
ruesipat 7:5ff551b098f8 84 speedCorrection = 0;
ruesipat 5:b8b1a979b0d5 85 slowdown = 0;
ruesipat 5:b8b1a979b0d5 86
ruesipat 5:b8b1a979b0d5 87
ruesipat 5:b8b1a979b0d5 88 //Ermittlung der Differenz Hinten-Vorne
ruesipat 5:b8b1a979b0d5 89 if((irSensor3.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor3 => sensorLeftBack , irSensor2 => sensorLeftFront
ruesipat 1:d9e840c48b1e 90
ruesipat 5:b8b1a979b0d5 91 parallelDif = irSensor3.read()-irSensor2.read(); //differenz hinten vorne bestimmen
ruesipat 5:b8b1a979b0d5 92 //printf(" DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
ruesipat 5:b8b1a979b0d5 93 //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
ruesipat 5:b8b1a979b0d5 94 //printf(" parallelDif: %.0f \n\r", parallelDif);
ruesipat 4:e3f388933954 95
ruesipat 5:b8b1a979b0d5 96 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 5:b8b1a979b0d5 97
ruesipat 5:b8b1a979b0d5 98 parallelDif = 0;
ruesipat 5:b8b1a979b0d5 99 }
ruesipat 5:b8b1a979b0d5 100
ruesipat 5:b8b1a979b0d5 101
ruesipat 5:b8b1a979b0d5 102 //Ermittlung der Differenz Rechts-Links
ruesipat 5:b8b1a979b0d5 103 if((irSensor0.read() < 100.0f) && (irSensor2.read() < 100.0f)) { //irSensor0 => sensorRight irSensor2 => sensorLeftFornt
ruesipat 5:b8b1a979b0d5 104
ruesipat 5:b8b1a979b0d5 105 rightLeftDif = irSensor0.read()-irSensor2.read(); //differenz links rechts bestimmen
ruesipat 5:b8b1a979b0d5 106 //printf(" DistanzRechts = %.0f mm\n\r", irSensor0.read());
ruesipat 5:b8b1a979b0d5 107 //printf(" DistanzLinksHinten = %.0f mm\n\r", irSensor2.read());
ruesipat 5:b8b1a979b0d5 108 //printf(" rightLeftDif: %.0f \n\r", rightLeftDif);
ruesipat 5:b8b1a979b0d5 109
ruesipat 5:b8b1a979b0d5 110 } else { //ist wand eine wand nicht vorhanden => keine korrektur
ruesipat 5:b8b1a979b0d5 111
ruesipat 5:b8b1a979b0d5 112 rightLeftDif = 0;
ruesipat 5:b8b1a979b0d5 113
ruesipat 5:b8b1a979b0d5 114 }
ruesipat 5:b8b1a979b0d5 115
ruesipat 5:b8b1a979b0d5 116
ruesipat 5:b8b1a979b0d5 117
ruesipat 5:b8b1a979b0d5 118 //Berechung Korrektur
ruesipat 7:5ff551b098f8 119 speedCorrection = ((0.2f * rightLeftDif) + (0.5f * parallelDif)); //DONT TOUCH
ruesipat 5:b8b1a979b0d5 120
ruesipat 5:b8b1a979b0d5 121
ruesipat 5:b8b1a979b0d5 122
ruesipat 5:b8b1a979b0d5 123
ruesipat 5:b8b1a979b0d5 124 //Kontrolle ob vorne Wand...Verlangsamen und Anhalten
ruesipat 5:b8b1a979b0d5 125
ruesipat 5:b8b1a979b0d5 126 if(irSensor1.read() < 150.0f) { //slow down
ruesipat 5:b8b1a979b0d5 127
ruesipat 7:5ff551b098f8 128 countCorrection = 500; // ermoeglicht an eine gesichtete wand anzugleichen
ruesipat 5:b8b1a979b0d5 129 slowdown = FRONTDISTANCE/irSensor1.read() * DRIVINGSPEED; //vorderer max abstand
ruesipat 5:b8b1a979b0d5 130
ruesipat 5:b8b1a979b0d5 131 if ( slowdown > DRIVINGSPEED) {
ruesipat 5:b8b1a979b0d5 132
ruesipat 5:b8b1a979b0d5 133 drive = 0;
ruesipat 5:b8b1a979b0d5 134
TheDarkDurzo 3:2ec7cf8bc3fc 135 }
ruesipat 5:b8b1a979b0d5 136 }
ruesipat 5:b8b1a979b0d5 137
ruesipat 5:b8b1a979b0d5 138
ruesipat 5:b8b1a979b0d5 139 }//Ende Whileschleife Drive...
ruesipat 5:b8b1a979b0d5 140
ruesipat 7:5ff551b098f8 141 //controller.setDesiredSpeedRight(0.5f); //0.5f
ruesipat 7:5ff551b098f8 142 //controller.setDesiredSpeedLeft(-0.5f); //-0.5f
ruesipat 1:d9e840c48b1e 143
ruesipat 1:d9e840c48b1e 144 }