Gruppe 3 / Mbed 2 deprecated PES4

xx

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

Roboter.cpp@18:6547e54ac803, 2017-05-09 (annotated)

Committer:
itslinear
Date:
Tue May 09 13:21:49 2017 +0000
Revision:
18:6547e54ac803
Parent:
17:caf5ae550f2e 
f?r holdi;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
itslinear 0:306a2438de17 1 #include "Roboter.h"
itslinear 0:306a2438de17 2
itslinear 0:306a2438de17 3
itslinear 18:6547e54ac803 4 Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver)
itslinear 4:fdcf3b5009c6 5
itslinear 1:4e84271a70c6 6 {
itslinear 0:306a2438de17 7
itslinear 1:4e84271a70c6 8 sensors[0].init( distance, bit0, bit1, bit2, 0);
itslinear 1:4e84271a70c6 9 sensors[1].init( distance, bit0, bit1, bit2, 1);
itslinear 1:4e84271a70c6 10 sensors[2].init( distance, bit0, bit1, bit2, 2);
itslinear 1:4e84271a70c6 11 sensors[3].init( distance, bit0, bit1, bit2, 3);
itslinear 1:4e84271a70c6 12 sensors[4].init( distance, bit0, bit1, bit2, 4);
itslinear 1:4e84271a70c6 13 sensors[5].init( distance, bit0, bit1, bit2, 5);
itslinear 0:306a2438de17 14
itslinear 1:4e84271a70c6 15 this->pwmR = pwmR;
itslinear 1:4e84271a70c6 16 this->pwmL = pwmL;
itslinear 18:6547e54ac803 17 this->servo1 = servo1;
itslinear 18:6547e54ac803 18 this->servo2 = servo2;
itslinear 18:6547e54ac803 19 this->counterLeft = counterLeft;
itslinear 18:6547e54ac803 20 this->counterRight = counterRight;
itslinear 18:6547e54ac803 21 this->enableMotorDriver = enableMotorDriver;
itslinear 4:fdcf3b5009c6 22
itslinear 18:6547e54ac803 23 initSpeedcontrol();
itslinear 18:6547e54ac803 24
itslinear 18:6547e54ac803 25 // t1.attach(this, &Roboter::speedCtrl, PERIOD);
itslinear 1:4e84271a70c6 26
itslinear 1:4e84271a70c6 27 }
itslinear 1:4e84271a70c6 28
itslinear 18:6547e54ac803 29 float Roboter::readSensor1() // Sensoren auslesen
itslinear 0:306a2438de17 30 {
itslinear 1:4e84271a70c6 31 return sensors[0].read();
itslinear 1:4e84271a70c6 32
itslinear 0:306a2438de17 33 }
itslinear 0:306a2438de17 34
itslinear 1:4e84271a70c6 35
itslinear 18:6547e54ac803 36 void Roboter::bandeAusweichen() // Hindernissen ausweichen
itslinear 0:306a2438de17 37 {
itslinear 1:4e84271a70c6 38 float x=0.13f; // Distanz ab welcher sensoren reagieren sollen
itslinear 1:4e84271a70c6 39
itslinear 9:d9e46f9c9e40 40 while(sensors[0] < x && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten
itslinear 18:6547e54ac803 41 desiredSpeedLeft = -v;
itslinear 18:6547e54ac803 42 desiredSpeedRight = v;
itslinear 9:d9e46f9c9e40 43
itslinear 9:d9e46f9c9e40 44 }
itslinear 9:d9e46f9c9e40 45
itslinear 18:6547e54ac803 46 while(sensors[0] < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
itslinear 18:6547e54ac803 47 desiredSpeedLeft = -v;
itslinear 18:6547e54ac803 48 desiredSpeedRight = -v;
itslinear 9:d9e46f9c9e40 49
itslinear 6:4af735d26b7a 50 }
itslinear 0:306a2438de17 51
itslinear 9:d9e46f9c9e40 52 while(sensors[1] < x) { // sensor rechts, roboter dreht nach links
itslinear 18:6547e54ac803 53 desiredSpeedLeft = -v;
itslinear 18:6547e54ac803 54 desiredSpeedRight = -v;
itslinear 9:d9e46f9c9e40 55
itslinear 1:4e84271a70c6 56 }
itslinear 1:4e84271a70c6 57
itslinear 9:d9e46f9c9e40 58 while(sensors[5] < x && sensors[1]>(x+0.02f)) { // sensor links, roboter dreht nach rechts
itslinear 18:6547e54ac803 59 desiredSpeedLeft = v;
itslinear 18:6547e54ac803 60 desiredSpeedRight = v;
itslinear 9:d9e46f9c9e40 61
itslinear 9:d9e46f9c9e40 62 }
itslinear 1:4e84271a70c6 63
itslinear 3:24e098715b78 64 }
schneju2 2:953263712480 65
schneju2 2:953263712480 66
itslinear 18:6547e54ac803 67 int Roboter::pickup() // Klotz aufnehmen
itslinear 18:6547e54ac803 68 {
itslinear 18:6547e54ac803 69 static int i = 0;
itslinear 18:6547e54ac803 70 int r; // Rückegabewert
itslinear 18:6547e54ac803 71 desiredSpeedLeft = 0;
itslinear 18:6547e54ac803 72 desiredSpeedRight = 0;
schneju2 2:953263712480 73
itslinear 18:6547e54ac803 74 // while(i < 260) {
itslinear 18:6547e54ac803 75 if(i < 70) { // Arm nach unten fahren
itslinear 18:6547e54ac803 76 servo2->write(0.96f);
itslinear 18:6547e54ac803 77 }
itslinear 18:6547e54ac803 78 if(i > 69 && i < 90) { // gerade fahren
itslinear 18:6547e54ac803 79 desiredSpeedLeft = v;
itslinear 18:6547e54ac803 80 desiredSpeedRight = -v;
itslinear 18:6547e54ac803 81 }
itslinear 18:6547e54ac803 82 if(i > 89 && i < 140) { // gerade fahren + Greifer schliessen
itslinear 18:6547e54ac803 83 desiredSpeedLeft = v;
itslinear 18:6547e54ac803 84 desiredSpeedRight = -v;
itslinear 18:6547e54ac803 85 servo1->write(0.0f);
itslinear 18:6547e54ac803 86 }
itslinear 18:6547e54ac803 87 if(i > 139 && i < 210) { // Arm nach oben fahren
itslinear 18:6547e54ac803 88 desiredSpeedLeft = 0;
itslinear 18:6547e54ac803 89 desiredSpeedRight = 0;
itslinear 18:6547e54ac803 90 servo2->write(0.0f);
itslinear 18:6547e54ac803 91 }
itslinear 18:6547e54ac803 92 if(i > 209 && i < 260) { // Greifer öffnen
itslinear 18:6547e54ac803 93 servo1->write(0.5f);
itslinear 18:6547e54ac803 94 }
itslinear 18:6547e54ac803 95 i++;
itslinear 18:6547e54ac803 96 //wait(0.01f);
itslinear 18:6547e54ac803 97 // }
itslinear 18:6547e54ac803 98 r = 5; // Rückegabewert
itslinear 18:6547e54ac803 99
itslinear 18:6547e54ac803 100 if( i > 260 )
itslinear 18:6547e54ac803 101 r = 0;
itslinear 18:6547e54ac803 102
itslinear 18:6547e54ac803 103 return r;
itslinear 18:6547e54ac803 104 }
itslinear 1:4e84271a70c6 105
itslinear 18:6547e54ac803 106 void Roboter::anfahren(float cam)
itslinear 18:6547e54ac803 107 {
itslinear 18:6547e54ac803 108 float speedValue;
itslinear 18:6547e54ac803 109 float maxWert = 20;
itslinear 18:6547e54ac803 110 if(cam == 100) {
itslinear 18:6547e54ac803 111 desiredSpeedLeft = -v/2;
itslinear 18:6547e54ac803 112 desiredSpeedRight = -v/2;
itslinear 18:6547e54ac803 113 }
itslinear 18:6547e54ac803 114 if(cam == 200) {
itslinear 18:6547e54ac803 115 desiredSpeedLeft = v/2;
itslinear 18:6547e54ac803 116 desiredSpeedRight = v/2;
itslinear 18:6547e54ac803 117 }
itslinear 18:6547e54ac803 118 if(cam > 100 && cam <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben
itslinear 18:6547e54ac803 119 cam = cam -99.0f; // cam nimmt die Werte von 1 bis 100 an
itslinear 18:6547e54ac803 120 speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
itslinear 18:6547e54ac803 121 desiredSpeedLeft = -speedValue;
itslinear 18:6547e54ac803 122 desiredSpeedRight = -speedValue;
itslinear 18:6547e54ac803 123 }
itslinear 18:6547e54ac803 124 if(cam > 200 && cam < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben
itslinear 18:6547e54ac803 125 cam = cam -199.0f; // cam nimmt die Werte von 1 bis 100 an
itslinear 18:6547e54ac803 126 speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
itslinear 18:6547e54ac803 127 desiredSpeedLeft = speedValue;
itslinear 18:6547e54ac803 128 desiredSpeedRight = speedValue;
itslinear 18:6547e54ac803 129 }
itslinear 18:6547e54ac803 130 if(cam >= 300 && cam <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben
itslinear 18:6547e54ac803 131 cam = cam-299.0f; // cam nimmt die Werte von 1 bis 100 an
itslinear 18:6547e54ac803 132 speedValue = v*1.5f + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
itslinear 18:6547e54ac803 133 desiredSpeedLeft = speedValue;
itslinear 18:6547e54ac803 134 desiredSpeedRight = -speedValue;
itslinear 18:6547e54ac803 135 }
itslinear 18:6547e54ac803 136 }
itslinear 18:6547e54ac803 137
itslinear 18:6547e54ac803 138 int Roboter::geradeFahren()
itslinear 18:6547e54ac803 139 {
itslinear 18:6547e54ac803 140 int r;
itslinear 18:6547e54ac803 141 static int time2 = 0;
itslinear 18:6547e54ac803 142 if(time2 < 200) { // 1s gerade aus fahren
itslinear 18:6547e54ac803 143 desiredSpeedLeft = v*3;
itslinear 18:6547e54ac803 144 desiredSpeedRight = -v*3;
itslinear 18:6547e54ac803 145 time2 ++;
itslinear 18:6547e54ac803 146 r = 1; // state
itslinear 18:6547e54ac803 147 wait(0.01f);
itslinear 18:6547e54ac803 148 } else {
itslinear 18:6547e54ac803 149 time2 = 0;
itslinear 18:6547e54ac803 150 desiredSpeedLeft = 0.0f;
itslinear 18:6547e54ac803 151 desiredSpeedRight = 0.0f;
itslinear 18:6547e54ac803 152 r = 2; //state
itslinear 18:6547e54ac803 153 }
itslinear 18:6547e54ac803 154 return r;
itslinear 18:6547e54ac803 155 }
itslinear 18:6547e54ac803 156
itslinear 18:6547e54ac803 157 int Roboter::kreisFahren()
itslinear 3:24e098715b78 158 {
itslinear 18:6547e54ac803 159 int r;
itslinear 18:6547e54ac803 160 static int time1 = 0;
itslinear 18:6547e54ac803 161 if(time1 < 310) { // 1s im Kreis drehen
itslinear 18:6547e54ac803 162 desiredSpeedLeft = v*1.7f;
itslinear 18:6547e54ac803 163 desiredSpeedRight = v*1.7f;
itslinear 18:6547e54ac803 164 time1 ++;
itslinear 18:6547e54ac803 165 r = 1; // state
itslinear 18:6547e54ac803 166 wait(0.01f);
itslinear 18:6547e54ac803 167 } else {
itslinear 18:6547e54ac803 168 time1 = 0;
itslinear 18:6547e54ac803 169 desiredSpeedLeft = 0.0f;
itslinear 18:6547e54ac803 170 desiredSpeedRight = 0.0f;
itslinear 18:6547e54ac803 171 r = 3; // state
itslinear 18:6547e54ac803 172 }
itslinear 18:6547e54ac803 173 return r;
itslinear 18:6547e54ac803 174 }
itslinear 3:24e098715b78 175
itslinear 18:6547e54ac803 176 int Roboter::stateAuswahl(float cam, int temp)
itslinear 18:6547e54ac803 177 {
itslinear 18:6547e54ac803 178 int s;
itslinear 18:6547e54ac803 179 if(cam >= 100.0f && cam < 400.0f ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399
itslinear 18:6547e54ac803 180 s = 4;
itslinear 18:6547e54ac803 181 }
itslinear 18:6547e54ac803 182 if(cam == 400.0f) { // Roboter in Position
itslinear 18:6547e54ac803 183 s = 5;
itslinear 18:6547e54ac803 184 }
itslinear 18:6547e54ac803 185 if(cam == 0.0f) {
itslinear 18:6547e54ac803 186 s = temp;
itslinear 18:6547e54ac803 187 }
itslinear 18:6547e54ac803 188 return s;
itslinear 18:6547e54ac803 189
itslinear 18:6547e54ac803 190 /*int s;
itslinear 18:6547e54ac803 191 if(cam == 1 || cam == 2 || cam == 3) { // Die Kamera erkennt ein grünen Klotz
itslinear 18:6547e54ac803 192 s = 4;
itslinear 18:6547e54ac803 193 }
itslinear 18:6547e54ac803 194 if(cam == 4) { // Roboter in Position
itslinear 18:6547e54ac803 195 s = 5;
itslinear 18:6547e54ac803 196 }
itslinear 18:6547e54ac803 197 if(cam == 0) { // kein Klotz in Sicht
itslinear 18:6547e54ac803 198 s = temp;
itslinear 18:6547e54ac803 199 }
itslinear 18:6547e54ac803 200 return s;*/
itslinear 18:6547e54ac803 201 }
itslinear 0:306a2438de17 202
itslinear 18:6547e54ac803 203 void Roboter::initSpeedcontrol()
itslinear 18:6547e54ac803 204 {
itslinear 18:6547e54ac803 205 // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
itslinear 18:6547e54ac803 206 pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
itslinear 18:6547e54ac803 207 pwmR->period(0.00005f);
itslinear 18:6547e54ac803 208 *pwmL = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
itslinear 18:6547e54ac803 209 *pwmR = 0.5f; // Duty-Cycle von 50%
schneju2 2:953263712480 210
itslinear 18:6547e54ac803 211 // Initialisieren von lokalen Variabeln
itslinear 18:6547e54ac803 212 previousValueCounterLeft = counterLeft->read();
itslinear 18:6547e54ac803 213 previousValueCounterRight = counterRight->read();
itslinear 18:6547e54ac803 214 speedLeftFilter.setPeriod(PERIOD);
itslinear 18:6547e54ac803 215 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
itslinear 18:6547e54ac803 216 speedRightFilter.setPeriod(PERIOD);
itslinear 18:6547e54ac803 217 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
itslinear 18:6547e54ac803 218
itslinear 18:6547e54ac803 219 desiredSpeedLeft = 0.0f;
itslinear 18:6547e54ac803 220 desiredSpeedRight = 0.0f;
itslinear 18:6547e54ac803 221 actualSpeedLeft = 0.0f;
itslinear 18:6547e54ac803 222 actualSpeedRight = 0.0f;
itslinear 18:6547e54ac803 223
itslinear 18:6547e54ac803 224 *enableMotorDriver = 1;
itslinear 3:24e098715b78 225 }
itslinear 18:6547e54ac803 226
itslinear 18:6547e54ac803 227 void Roboter::speedCtrl()
itslinear 18:6547e54ac803 228 {
itslinear 18:6547e54ac803 229 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
itslinear 18:6547e54ac803 230 short valueCounterLeft = counterLeft->read();
itslinear 18:6547e54ac803 231 short valueCounterRight = counterRight->read();
itslinear 18:6547e54ac803 232 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
itslinear 18:6547e54ac803 233 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
itslinear 18:6547e54ac803 234
itslinear 18:6547e54ac803 235 previousValueCounterLeft = valueCounterLeft;
itslinear 18:6547e54ac803 236 previousValueCounterRight = valueCounterRight;
itslinear 18:6547e54ac803 237 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
itslinear 18:6547e54ac803 238 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
itslinear 18:6547e54ac803 239
itslinear 18:6547e54ac803 240 // Berechnen der Motorspannungen Uout
itslinear 18:6547e54ac803 241 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
itslinear 18:6547e54ac803 242 float voltageRight = KP*(desiredSpeedRight*correction-actualSpeedRight)+desiredSpeedRight*correction/KN;
itslinear 18:6547e54ac803 243
itslinear 18:6547e54ac803 244 // Berechnen, Limitieren und Setzen der Duty-Cycle
itslinear 18:6547e54ac803 245 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
itslinear 18:6547e54ac803 246 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
itslinear 18:6547e54ac803 247 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
itslinear 18:6547e54ac803 248
itslinear 18:6547e54ac803 249 *pwmL = dutyCycleLeft;
itslinear 18:6547e54ac803 250 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
itslinear 18:6547e54ac803 251 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
itslinear 18:6547e54ac803 252 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
itslinear 18:6547e54ac803 253
itslinear 18:6547e54ac803 254 *pwmR = dutyCycleRight;
itslinear 18:6547e54ac803 255 }