Gruppe 3 / Mbed 2 deprecated PES3

hallo

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

Roboter.cpp@18:3ee1b02ed3aa, 2017-05-09 (annotated)

Committer:
itslinear
Date:
Tue May 09 15:25:54 2017 +0000
Revision:
18:3ee1b02ed3aa
Parent:
17:caf5ae550f2e
Child:
19:adae367247d4
new

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:3ee1b02ed3aa 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:3ee1b02ed3aa 17 this->servo1 = servo1;
itslinear 18:3ee1b02ed3aa 18 this->servo2 = servo2;
itslinear 18:3ee1b02ed3aa 19 this->counterLeft = counterLeft;
itslinear 18:3ee1b02ed3aa 20 this->counterRight = counterRight;
itslinear 18:3ee1b02ed3aa 21 this->enableMotorDriver = enableMotorDriver;
itslinear 4:fdcf3b5009c6 22
itslinear 18:3ee1b02ed3aa 23 initSpeedcontrol();
itslinear 18:3ee1b02ed3aa 24
itslinear 18:3ee1b02ed3aa 25 t1.attach(this, &Roboter::speedCtrl, PERIOD);
itslinear 1:4e84271a70c6 26
itslinear 1:4e84271a70c6 27 }
itslinear 1:4e84271a70c6 28
itslinear 18:3ee1b02ed3aa 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:3ee1b02ed3aa 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:3ee1b02ed3aa 41 desiredSpeedLeft = -v;
itslinear 18:3ee1b02ed3aa 42 desiredSpeedRight = v;
itslinear 9:d9e46f9c9e40 43
itslinear 9:d9e46f9c9e40 44 }
itslinear 9:d9e46f9c9e40 45
itslinear 18:3ee1b02ed3aa 46 while(sensors[0] < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
itslinear 18:3ee1b02ed3aa 47 desiredSpeedLeft = -v;
itslinear 18:3ee1b02ed3aa 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:3ee1b02ed3aa 53 desiredSpeedLeft = -v;
itslinear 18:3ee1b02ed3aa 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:3ee1b02ed3aa 59 desiredSpeedLeft = v;
itslinear 18:3ee1b02ed3aa 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:3ee1b02ed3aa 67 int Roboter::pickup() // Klotz aufnehmen
itslinear 18:3ee1b02ed3aa 68 {
itslinear 18:3ee1b02ed3aa 69 static int ii = 0;
itslinear 18:3ee1b02ed3aa 70 int r; // Rückegabewert
itslinear 18:3ee1b02ed3aa 71 desiredSpeedLeft = 0;
itslinear 18:3ee1b02ed3aa 72 desiredSpeedRight = 0;
schneju2 2:953263712480 73
itslinear 18:3ee1b02ed3aa 74 if(ii < 70) { // Arm nach unten fahren
itslinear 18:3ee1b02ed3aa 75 servo2->SetPosition(2250);
itslinear 18:3ee1b02ed3aa 76 }
itslinear 18:3ee1b02ed3aa 77 if(ii > 69 && ii < 110) { // gerade fahren
itslinear 18:3ee1b02ed3aa 78 desiredSpeedLeft = v;
itslinear 18:3ee1b02ed3aa 79 desiredSpeedRight = -v;
itslinear 18:3ee1b02ed3aa 80 }
itslinear 18:3ee1b02ed3aa 81 if(ii > 109 && ii < 160) { // gerade fahren + Greifer schliessen
itslinear 18:3ee1b02ed3aa 82 desiredSpeedLeft = v;
itslinear 18:3ee1b02ed3aa 83 desiredSpeedRight = -v;
itslinear 18:3ee1b02ed3aa 84 servo1->SetPosition(500);
itslinear 18:3ee1b02ed3aa 85 }
itslinear 18:3ee1b02ed3aa 86 if(ii > 159 && ii < 230) { // Arm nach oben fahren
itslinear 18:3ee1b02ed3aa 87 desiredSpeedLeft = 0;
itslinear 18:3ee1b02ed3aa 88 desiredSpeedRight = 0;
itslinear 18:3ee1b02ed3aa 89 servo2->SetPosition(700);
itslinear 18:3ee1b02ed3aa 90 }
itslinear 18:3ee1b02ed3aa 91 if(ii > 229 && ii < 280) { // Greifer öffnen
itslinear 18:3ee1b02ed3aa 92 servo1->SetPosition(1500);
itslinear 18:3ee1b02ed3aa 93 }
itslinear 18:3ee1b02ed3aa 94 ii++;
itslinear 18:3ee1b02ed3aa 95 r = 5; // Rückegabewert
itslinear 18:3ee1b02ed3aa 96
itslinear 18:3ee1b02ed3aa 97 if( ii > 279 ) {
itslinear 18:3ee1b02ed3aa 98 r = 1;
itslinear 18:3ee1b02ed3aa 99 ii = 0;
itslinear 18:3ee1b02ed3aa 100 }
itslinear 18:3ee1b02ed3aa 101 return r;
itslinear 18:3ee1b02ed3aa 102 }
itslinear 1:4e84271a70c6 103
itslinear 18:3ee1b02ed3aa 104 void Roboter::anfahren(float cam)
itslinear 18:3ee1b02ed3aa 105 {
itslinear 18:3ee1b02ed3aa 106 float speedValue;
itslinear 18:3ee1b02ed3aa 107 float maxWert = 20;
itslinear 18:3ee1b02ed3aa 108 if(cam == 100) {
itslinear 18:3ee1b02ed3aa 109 desiredSpeedLeft = -v/2;
itslinear 18:3ee1b02ed3aa 110 desiredSpeedRight = -v/2;
itslinear 18:3ee1b02ed3aa 111 }
itslinear 18:3ee1b02ed3aa 112 if(cam == 200) {
itslinear 18:3ee1b02ed3aa 113 desiredSpeedLeft = v/2;
itslinear 18:3ee1b02ed3aa 114 desiredSpeedRight = v/2;
itslinear 18:3ee1b02ed3aa 115 }
itslinear 18:3ee1b02ed3aa 116 if(cam > 100 && cam <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben
itslinear 18:3ee1b02ed3aa 117 cam = cam -99.0f; // cam nimmt die Werte von 1 bis 100 an
itslinear 18:3ee1b02ed3aa 118 speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
itslinear 18:3ee1b02ed3aa 119 desiredSpeedLeft = -speedValue;
itslinear 18:3ee1b02ed3aa 120 desiredSpeedRight = -speedValue;
itslinear 18:3ee1b02ed3aa 121 }
itslinear 18:3ee1b02ed3aa 122 if(cam > 200 && cam < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben
itslinear 18:3ee1b02ed3aa 123 cam = cam -199.0f; // cam nimmt die Werte von 1 bis 100 an
itslinear 18:3ee1b02ed3aa 124 speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
itslinear 18:3ee1b02ed3aa 125 desiredSpeedLeft = speedValue;
itslinear 18:3ee1b02ed3aa 126 desiredSpeedRight = speedValue;
itslinear 18:3ee1b02ed3aa 127 }
itslinear 18:3ee1b02ed3aa 128 if(cam >= 300 && cam <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben
itslinear 18:3ee1b02ed3aa 129 cam = cam-299.0f; // cam nimmt die Werte von 1 bis 100 an
itslinear 18:3ee1b02ed3aa 130 speedValue = v*1.5f + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
itslinear 18:3ee1b02ed3aa 131 desiredSpeedLeft = speedValue;
itslinear 18:3ee1b02ed3aa 132 desiredSpeedRight = -speedValue;
itslinear 18:3ee1b02ed3aa 133 }
itslinear 18:3ee1b02ed3aa 134 }
itslinear 18:3ee1b02ed3aa 135
itslinear 18:3ee1b02ed3aa 136 int Roboter::geradeFahren()
itslinear 3:24e098715b78 137 {
itslinear 18:3ee1b02ed3aa 138 int r;
itslinear 18:3ee1b02ed3aa 139 static int time2 = 0;
itslinear 18:3ee1b02ed3aa 140 if(time2 < 200) { // 1s gerade aus fahren
itslinear 18:3ee1b02ed3aa 141 desiredSpeedLeft = v*3;
itslinear 18:3ee1b02ed3aa 142 desiredSpeedRight = -v*3;
itslinear 18:3ee1b02ed3aa 143 time2 ++;
itslinear 18:3ee1b02ed3aa 144 r = 1; // state
itslinear 18:3ee1b02ed3aa 145 wait(0.01f);
itslinear 18:3ee1b02ed3aa 146 } else {
itslinear 18:3ee1b02ed3aa 147 time2 = 0;
itslinear 18:3ee1b02ed3aa 148 desiredSpeedLeft = 0.0f;
itslinear 18:3ee1b02ed3aa 149 desiredSpeedRight = 0.0f;
itslinear 18:3ee1b02ed3aa 150 r = 2; //state
itslinear 18:3ee1b02ed3aa 151 }
itslinear 18:3ee1b02ed3aa 152 return r;
itslinear 18:3ee1b02ed3aa 153 }
itslinear 3:24e098715b78 154
itslinear 18:3ee1b02ed3aa 155 int Roboter::kreisFahren()
itslinear 18:3ee1b02ed3aa 156 {
itslinear 18:3ee1b02ed3aa 157 int r;
itslinear 18:3ee1b02ed3aa 158 static int time1 = 0;
itslinear 18:3ee1b02ed3aa 159 if(time1 < 310) { // 1s im Kreis drehen
itslinear 18:3ee1b02ed3aa 160 desiredSpeedLeft = v*1.7f;
itslinear 18:3ee1b02ed3aa 161 desiredSpeedRight = v*1.7f;
itslinear 18:3ee1b02ed3aa 162 time1 ++;
itslinear 18:3ee1b02ed3aa 163 r = 1; // state
itslinear 18:3ee1b02ed3aa 164 wait(0.01f);
itslinear 18:3ee1b02ed3aa 165 } else {
itslinear 18:3ee1b02ed3aa 166 time1 = 0;
itslinear 18:3ee1b02ed3aa 167 desiredSpeedLeft = 0.0f;
itslinear 18:3ee1b02ed3aa 168 desiredSpeedRight = 0.0f;
itslinear 18:3ee1b02ed3aa 169 r = 3; // state
itslinear 18:3ee1b02ed3aa 170 }
itslinear 18:3ee1b02ed3aa 171 return r;
itslinear 18:3ee1b02ed3aa 172 }
itslinear 18:3ee1b02ed3aa 173
itslinear 18:3ee1b02ed3aa 174 int Roboter::stateAuswahl(float cam, int temp)
itslinear 18:3ee1b02ed3aa 175 {
itslinear 18:3ee1b02ed3aa 176 int s;
itslinear 18:3ee1b02ed3aa 177 if(cam >= 100.0f && cam < 400.0f ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399
itslinear 18:3ee1b02ed3aa 178 s = 4;
itslinear 18:3ee1b02ed3aa 179 }
itslinear 18:3ee1b02ed3aa 180 if(cam == 400.0f) { // Roboter in Position
itslinear 18:3ee1b02ed3aa 181 s = 5;
itslinear 18:3ee1b02ed3aa 182 }
itslinear 18:3ee1b02ed3aa 183 if(cam == 0.0f) {
itslinear 18:3ee1b02ed3aa 184 s = temp;
itslinear 18:3ee1b02ed3aa 185 }
itslinear 18:3ee1b02ed3aa 186 return s;
itslinear 0:306a2438de17 187
schneju2 2:953263712480 188
itslinear 3:24e098715b78 189 }
itslinear 18:3ee1b02ed3aa 190
itslinear 18:3ee1b02ed3aa 191 void Roboter::initSpeedcontrol()
itslinear 18:3ee1b02ed3aa 192 {
itslinear 18:3ee1b02ed3aa 193 // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
itslinear 18:3ee1b02ed3aa 194 pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
itslinear 18:3ee1b02ed3aa 195 pwmR->period(0.00005f);
itslinear 18:3ee1b02ed3aa 196 *pwmL = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
itslinear 18:3ee1b02ed3aa 197 *pwmR = 0.5f; // Duty-Cycle von 50%
itslinear 18:3ee1b02ed3aa 198
itslinear 18:3ee1b02ed3aa 199 // Initialisieren von lokalen Variabeln
itslinear 18:3ee1b02ed3aa 200 previousValueCounterLeft = counterLeft->read();
itslinear 18:3ee1b02ed3aa 201 previousValueCounterRight = counterRight->read();
itslinear 18:3ee1b02ed3aa 202 speedLeftFilter.setPeriod(PERIOD);
itslinear 18:3ee1b02ed3aa 203 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
itslinear 18:3ee1b02ed3aa 204 speedRightFilter.setPeriod(PERIOD);
itslinear 18:3ee1b02ed3aa 205 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
itslinear 18:3ee1b02ed3aa 206
itslinear 18:3ee1b02ed3aa 207 desiredSpeedLeft = 0.0f;
itslinear 18:3ee1b02ed3aa 208 desiredSpeedRight = 0.0f;
itslinear 18:3ee1b02ed3aa 209 actualSpeedLeft = 0.0f;
itslinear 18:3ee1b02ed3aa 210 actualSpeedRight = 0.0f;
itslinear 18:3ee1b02ed3aa 211
itslinear 18:3ee1b02ed3aa 212 *enableMotorDriver = 1;
itslinear 18:3ee1b02ed3aa 213 }
itslinear 18:3ee1b02ed3aa 214
itslinear 18:3ee1b02ed3aa 215 void Roboter::speedCtrl()
itslinear 18:3ee1b02ed3aa 216 {
itslinear 18:3ee1b02ed3aa 217 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
itslinear 18:3ee1b02ed3aa 218 short valueCounterLeft = counterLeft->read();
itslinear 18:3ee1b02ed3aa 219 short valueCounterRight = counterRight->read();
itslinear 18:3ee1b02ed3aa 220 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
itslinear 18:3ee1b02ed3aa 221 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
itslinear 18:3ee1b02ed3aa 222
itslinear 18:3ee1b02ed3aa 223 previousValueCounterLeft = valueCounterLeft;
itslinear 18:3ee1b02ed3aa 224 previousValueCounterRight = valueCounterRight;
itslinear 18:3ee1b02ed3aa 225 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
itslinear 18:3ee1b02ed3aa 226 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
itslinear 18:3ee1b02ed3aa 227
itslinear 18:3ee1b02ed3aa 228 // Berechnen der Motorspannungen Uout
itslinear 18:3ee1b02ed3aa 229 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
itslinear 18:3ee1b02ed3aa 230 float voltageRight = KP*(desiredSpeedRight*correction-actualSpeedRight)+desiredSpeedRight*correction/KN;
itslinear 18:3ee1b02ed3aa 231
itslinear 18:3ee1b02ed3aa 232 // Berechnen, Limitieren und Setzen der Duty-Cycle
itslinear 18:3ee1b02ed3aa 233 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
itslinear 18:3ee1b02ed3aa 234 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
itslinear 18:3ee1b02ed3aa 235 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
itslinear 18:3ee1b02ed3aa 236
itslinear 18:3ee1b02ed3aa 237 *pwmL = dutyCycleLeft;
itslinear 18:3ee1b02ed3aa 238 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
itslinear 18:3ee1b02ed3aa 239 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
itslinear 18:3ee1b02ed3aa 240 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
itslinear 18:3ee1b02ed3aa 241
itslinear 18:3ee1b02ed3aa 242 *pwmR = dutyCycleRight;
itslinear 18:3ee1b02ed3aa 243 }