Gruppe 3 / Mbed 2 deprecated PES3

hallo

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

Roboter.cpp@21:69ee872b8ee9, 2017-05-16 (annotated)

Committer:
itslinear
Date:
Tue May 16 16:09:08 2017 +0000
Revision:
21:69ee872b8ee9
Parent:
20:a90e5b54bf7b 
sieg

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 1:4e84271a70c6 7 sensors[0].init( distance, bit0, bit1, bit2, 0);
itslinear 1:4e84271a70c6 8 sensors[1].init( distance, bit0, bit1, bit2, 1);
itslinear 1:4e84271a70c6 9 sensors[2].init( distance, bit0, bit1, bit2, 2);
itslinear 1:4e84271a70c6 10 sensors[3].init( distance, bit0, bit1, bit2, 3);
itslinear 1:4e84271a70c6 11 sensors[4].init( distance, bit0, bit1, bit2, 4);
itslinear 1:4e84271a70c6 12 sensors[5].init( distance, bit0, bit1, bit2, 5);
itslinear 0:306a2438de17 13
itslinear 1:4e84271a70c6 14 this->pwmR = pwmR;
itslinear 1:4e84271a70c6 15 this->pwmL = pwmL;
itslinear 18:3ee1b02ed3aa 16 this->servo1 = servo1;
itslinear 18:3ee1b02ed3aa 17 this->servo2 = servo2;
itslinear 18:3ee1b02ed3aa 18 this->counterLeft = counterLeft;
itslinear 18:3ee1b02ed3aa 19 this->counterRight = counterRight;
itslinear 18:3ee1b02ed3aa 20 this->enableMotorDriver = enableMotorDriver;
itslinear 4:fdcf3b5009c6 21
itslinear 18:3ee1b02ed3aa 22 initSpeedcontrol();
itslinear 18:3ee1b02ed3aa 23
itslinear 18:3ee1b02ed3aa 24 t1.attach(this, &Roboter::speedCtrl, PERIOD);
itslinear 1:4e84271a70c6 25
itslinear 1:4e84271a70c6 26 }
itslinear 1:4e84271a70c6 27
itslinear 19:adae367247d4 28 int Roboter::readSensors() // Sensoren auslesen
itslinear 0:306a2438de17 29 {
itslinear 20:a90e5b54bf7b 30 int lr = 1;
itslinear 21:69ee872b8ee9 31 float x = 0.1; // Distanz ab welcher sensoren reagieren sollen
itslinear 19:adae367247d4 32
itslinear 19:adae367247d4 33 if(sensors[1] < x || sensors[5] < x) { // Sensor rechts und links
itslinear 20:a90e5b54bf7b 34 lr = 6;
itslinear 19:adae367247d4 35 }
itslinear 20:a90e5b54bf7b 36 return lr;
itslinear 1:4e84271a70c6 37
itslinear 0:306a2438de17 38 }
itslinear 0:306a2438de17 39
itslinear 1:4e84271a70c6 40
itslinear 19:adae367247d4 41 void Roboter::ausweichen1() // Hindernissen ausweichen
itslinear 0:306a2438de17 42 {
itslinear 21:69ee872b8ee9 43 float x = 0.14; // Distanz ab welcher sensoren reagieren sollen
itslinear 1:4e84271a70c6 44
itslinear 21:69ee872b8ee9 45 while(sensors[0] < (x-0.01f) && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten
itslinear 21:69ee872b8ee9 46 desiredSpeedLeft = -v;
itslinear 21:69ee872b8ee9 47 desiredSpeedRight = v*4;
itslinear 9:d9e46f9c9e40 48
itslinear 9:d9e46f9c9e40 49 }
itslinear 9:d9e46f9c9e40 50
itslinear 21:69ee872b8ee9 51 while(sensors[0] < x) { // sensor vorne, roboter dreht nach links
itslinear 20:a90e5b54bf7b 52 desiredSpeedLeft = -v*2;
itslinear 20:a90e5b54bf7b 53 desiredSpeedRight = -v*2;
itslinear 9:d9e46f9c9e40 54
itslinear 6:4af735d26b7a 55 }
itslinear 0:306a2438de17 56
itslinear 9:d9e46f9c9e40 57 while(sensors[1] < x) { // sensor rechts, roboter dreht nach links
itslinear 20:a90e5b54bf7b 58 desiredSpeedLeft = -v*2;
itslinear 20:a90e5b54bf7b 59 desiredSpeedRight = -v*2;
itslinear 9:d9e46f9c9e40 60
itslinear 1:4e84271a70c6 61 }
itslinear 1:4e84271a70c6 62
itslinear 21:69ee872b8ee9 63 while(sensors[5] < x) { // sensor links, roboter dreht nach rechts
itslinear 20:a90e5b54bf7b 64 desiredSpeedLeft = v*2;
itslinear 20:a90e5b54bf7b 65 desiredSpeedRight = v*2;
itslinear 9:d9e46f9c9e40 66
itslinear 9:d9e46f9c9e40 67 }
itslinear 1:4e84271a70c6 68
itslinear 3:24e098715b78 69 }
schneju2 2:953263712480 70
itslinear 19:adae367247d4 71 int Roboter::ausweichen2() // Hindernissen ausweichen beim Anfahren des Klotzes
itslinear 19:adae367247d4 72 {
itslinear 21:69ee872b8ee9 73 float x = 0.1; // Distanz ab welcher sensoren reagieren sollen
itslinear 19:adae367247d4 74 static int l = 0;
itslinear 19:adae367247d4 75 static int r = 0;
itslinear 19:adae367247d4 76 static int ii = 0;
itslinear 19:adae367247d4 77 int s = 6;
itslinear 19:adae367247d4 78
itslinear 19:adae367247d4 79 if(sensors[1] < x && l == 0) {
itslinear 19:adae367247d4 80 r = 1;
itslinear 19:adae367247d4 81 }
itslinear 19:adae367247d4 82
itslinear 19:adae367247d4 83 if(sensors[5] < x && r == 0) {
itslinear 19:adae367247d4 84 l = 1;
itslinear 19:adae367247d4 85 }
itslinear 19:adae367247d4 86
itslinear 19:adae367247d4 87 if(r == 1) {
itslinear 21:69ee872b8ee9 88 if(ii < 500) { // rückwärts fahren
itslinear 19:adae367247d4 89 desiredSpeedLeft = -(v+30);
itslinear 20:a90e5b54bf7b 90 desiredSpeedRight = v+30;
itslinear 19:adae367247d4 91 }
itslinear 21:69ee872b8ee9 92 if(ii > 499 && ii < 900) { // links drehen
itslinear 20:a90e5b54bf7b 93 desiredSpeedLeft = -v*2;
itslinear 20:a90e5b54bf7b 94 desiredSpeedRight = -v*2;
itslinear 20:a90e5b54bf7b 95 }
itslinear 21:69ee872b8ee9 96 if(ii > 899 && ii < 1800) { // gerade fahren
itslinear 19:adae367247d4 97 desiredSpeedLeft = v*2;
itslinear 19:adae367247d4 98 desiredSpeedRight = -v*2;
itslinear 19:adae367247d4 99 }
itslinear 21:69ee872b8ee9 100 if(ii > 1799 && ii < 2300) { // rechts drehen
itslinear 20:a90e5b54bf7b 101 desiredSpeedLeft = v*2;
itslinear 20:a90e5b54bf7b 102 desiredSpeedRight = v*2;
itslinear 19:adae367247d4 103 }
itslinear 19:adae367247d4 104 ii++;
itslinear 21:69ee872b8ee9 105 if(ii > 2299) { // rücksetzen
itslinear 19:adae367247d4 106 desiredSpeedLeft = 0;
itslinear 19:adae367247d4 107 desiredSpeedRight = 0;
itslinear 19:adae367247d4 108 r = 0;
itslinear 19:adae367247d4 109 ii = 0;
itslinear 19:adae367247d4 110 s = 1;
itslinear 19:adae367247d4 111 }
itslinear 19:adae367247d4 112 }
itslinear 19:adae367247d4 113
itslinear 19:adae367247d4 114 if(l == 1) {
itslinear 20:a90e5b54bf7b 115 if(ii < 500) { // rückwärts fahren
itslinear 20:a90e5b54bf7b 116 desiredSpeedLeft = -(v+30);
itslinear 19:adae367247d4 117 desiredSpeedRight = v+30;
itslinear 19:adae367247d4 118 }
itslinear 21:69ee872b8ee9 119 if(ii > 499 && ii < 900) { // rechts drehen
itslinear 20:a90e5b54bf7b 120 desiredSpeedLeft = v*2;
itslinear 20:a90e5b54bf7b 121 desiredSpeedRight = v*2;
itslinear 20:a90e5b54bf7b 122 }
itslinear 21:69ee872b8ee9 123 if(ii > 899 && ii < 1800) { // gerade fahren
itslinear 19:adae367247d4 124 desiredSpeedLeft = v*2;
itslinear 19:adae367247d4 125 desiredSpeedRight = -v*2;
itslinear 19:adae367247d4 126 }
itslinear 21:69ee872b8ee9 127 if(ii > 1799 && ii < 2300) { // links drehen
itslinear 20:a90e5b54bf7b 128 desiredSpeedLeft = -v*2;
itslinear 20:a90e5b54bf7b 129 desiredSpeedRight = -v*2;
itslinear 19:adae367247d4 130 }
itslinear 19:adae367247d4 131 ii++;
itslinear 21:69ee872b8ee9 132 if(ii > 2299) {
itslinear 19:adae367247d4 133 desiredSpeedLeft = 0;
itslinear 19:adae367247d4 134 desiredSpeedRight = 0;
itslinear 19:adae367247d4 135 l = 0;
itslinear 19:adae367247d4 136 ii = 0;
itslinear 19:adae367247d4 137 s = 1;
itslinear 19:adae367247d4 138 }
itslinear 19:adae367247d4 139 }
itslinear 19:adae367247d4 140 return s;
itslinear 19:adae367247d4 141 }
itslinear 19:adae367247d4 142
schneju2 2:953263712480 143
itslinear 18:3ee1b02ed3aa 144 int Roboter::pickup() // Klotz aufnehmen
itslinear 18:3ee1b02ed3aa 145 {
itslinear 18:3ee1b02ed3aa 146 static int ii = 0;
itslinear 18:3ee1b02ed3aa 147 int r; // Rückegabewert
itslinear 18:3ee1b02ed3aa 148 desiredSpeedLeft = 0;
itslinear 18:3ee1b02ed3aa 149 desiredSpeedRight = 0;
schneju2 2:953263712480 150
itslinear 21:69ee872b8ee9 151 if(ii < 650) { // Arm nach unten fahren
itslinear 18:3ee1b02ed3aa 152 servo2->SetPosition(2250);
itslinear 18:3ee1b02ed3aa 153 }
itslinear 20:a90e5b54bf7b 154 if(ii > 649 && ii < 1100) { // gerade fahren
itslinear 18:3ee1b02ed3aa 155 desiredSpeedLeft = v;
itslinear 18:3ee1b02ed3aa 156 desiredSpeedRight = -v;
itslinear 18:3ee1b02ed3aa 157 }
itslinear 20:a90e5b54bf7b 158 if(ii > 1099 && ii < 1500) { // gerade fahren + Greifer schliessen
itslinear 20:a90e5b54bf7b 159 if(ii < 1400) {
itslinear 20:a90e5b54bf7b 160 desiredSpeedLeft = v;
itslinear 20:a90e5b54bf7b 161 desiredSpeedRight = -v;
itslinear 20:a90e5b54bf7b 162 }
itslinear 18:3ee1b02ed3aa 163 servo1->SetPosition(500);
itslinear 18:3ee1b02ed3aa 164 }
itslinear 20:a90e5b54bf7b 165 if(ii > 1499 && ii < 1900) { // rückwärts fahren
itslinear 20:a90e5b54bf7b 166 desiredSpeedLeft = -v;
itslinear 20:a90e5b54bf7b 167 desiredSpeedRight = v;
itslinear 19:adae367247d4 168 }
itslinear 20:a90e5b54bf7b 169 if(ii > 1899 && ii < 2550) { // Arm nach oben fahren
itslinear 18:3ee1b02ed3aa 170 desiredSpeedLeft = 0;
itslinear 18:3ee1b02ed3aa 171 desiredSpeedRight = 0;
itslinear 18:3ee1b02ed3aa 172 servo2->SetPosition(700);
itslinear 18:3ee1b02ed3aa 173 }
itslinear 20:a90e5b54bf7b 174 if(ii > 2549 && ii < 3000) { // Greifer öffnen
itslinear 18:3ee1b02ed3aa 175 servo1->SetPosition(1500);
itslinear 18:3ee1b02ed3aa 176 }
itslinear 20:a90e5b54bf7b 177
itslinear 18:3ee1b02ed3aa 178 ii++;
itslinear 21:69ee872b8ee9 179 r = 5; // Rückegabewert
itslinear 18:3ee1b02ed3aa 180
itslinear 20:a90e5b54bf7b 181 if( ii > 2999 ) {
itslinear 18:3ee1b02ed3aa 182 r = 1;
itslinear 18:3ee1b02ed3aa 183 ii = 0;
itslinear 18:3ee1b02ed3aa 184 }
itslinear 20:a90e5b54bf7b 185
itslinear 18:3ee1b02ed3aa 186 return r;
itslinear 18:3ee1b02ed3aa 187 }
itslinear 1:4e84271a70c6 188
itslinear 18:3ee1b02ed3aa 189 void Roboter::anfahren(float cam)
itslinear 18:3ee1b02ed3aa 190 {
itslinear 18:3ee1b02ed3aa 191 float speedValue;
itslinear 18:3ee1b02ed3aa 192 float maxWert = 20;
itslinear 18:3ee1b02ed3aa 193 if(cam == 100) {
itslinear 18:3ee1b02ed3aa 194 desiredSpeedLeft = -v/2;
itslinear 18:3ee1b02ed3aa 195 desiredSpeedRight = -v/2;
itslinear 18:3ee1b02ed3aa 196 }
itslinear 18:3ee1b02ed3aa 197 if(cam == 200) {
itslinear 18:3ee1b02ed3aa 198 desiredSpeedLeft = v/2;
itslinear 18:3ee1b02ed3aa 199 desiredSpeedRight = v/2;
itslinear 18:3ee1b02ed3aa 200 }
itslinear 18:3ee1b02ed3aa 201 if(cam > 100 && cam <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben
itslinear 18:3ee1b02ed3aa 202 cam = cam -99.0f; // cam nimmt die Werte von 1 bis 100 an
itslinear 20:a90e5b54bf7b 203 speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
itslinear 18:3ee1b02ed3aa 204 desiredSpeedLeft = -speedValue;
itslinear 18:3ee1b02ed3aa 205 desiredSpeedRight = -speedValue;
itslinear 18:3ee1b02ed3aa 206 }
itslinear 18:3ee1b02ed3aa 207 if(cam > 200 && cam < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben
itslinear 18:3ee1b02ed3aa 208 cam = cam -199.0f; // cam nimmt die Werte von 1 bis 100 an
itslinear 20:a90e5b54bf7b 209 speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
itslinear 18:3ee1b02ed3aa 210 desiredSpeedLeft = speedValue;
itslinear 18:3ee1b02ed3aa 211 desiredSpeedRight = speedValue;
itslinear 18:3ee1b02ed3aa 212 }
itslinear 18:3ee1b02ed3aa 213 if(cam >= 300 && cam <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben
itslinear 18:3ee1b02ed3aa 214 cam = cam-299.0f; // cam nimmt die Werte von 1 bis 100 an
itslinear 20:a90e5b54bf7b 215 speedValue = (v*3) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
itslinear 18:3ee1b02ed3aa 216 desiredSpeedLeft = speedValue;
itslinear 18:3ee1b02ed3aa 217 desiredSpeedRight = -speedValue;
itslinear 18:3ee1b02ed3aa 218 }
itslinear 18:3ee1b02ed3aa 219 }
itslinear 18:3ee1b02ed3aa 220
itslinear 18:3ee1b02ed3aa 221 int Roboter::geradeFahren()
itslinear 3:24e098715b78 222 {
itslinear 18:3ee1b02ed3aa 223 int r;
itslinear 18:3ee1b02ed3aa 224 static int time2 = 0;
itslinear 21:69ee872b8ee9 225 if(time2 < 25) { // 1s gerade aus fahren
itslinear 20:a90e5b54bf7b 226 desiredSpeedLeft = v*4;
itslinear 20:a90e5b54bf7b 227 desiredSpeedRight = -v*4;
itslinear 18:3ee1b02ed3aa 228 time2 ++;
itslinear 18:3ee1b02ed3aa 229 r = 1; // state
itslinear 18:3ee1b02ed3aa 230 } else {
itslinear 18:3ee1b02ed3aa 231 time2 = 0;
itslinear 18:3ee1b02ed3aa 232 desiredSpeedLeft = 0.0f;
itslinear 18:3ee1b02ed3aa 233 desiredSpeedRight = 0.0f;
itslinear 18:3ee1b02ed3aa 234 r = 2; //state
itslinear 18:3ee1b02ed3aa 235 }
itslinear 18:3ee1b02ed3aa 236 return r;
itslinear 18:3ee1b02ed3aa 237 }
itslinear 3:24e098715b78 238
itslinear 19:adae367247d4 239 int Roboter::back()
itslinear 19:adae367247d4 240 {
itslinear 19:adae367247d4 241 static int ii = 0;
itslinear 20:a90e5b54bf7b 242 int s = 7;
itslinear 19:adae367247d4 243
itslinear 20:a90e5b54bf7b 244 if(ii < 1000) {
itslinear 19:adae367247d4 245 desiredSpeedLeft = -v;
itslinear 19:adae367247d4 246 desiredSpeedRight = v;
itslinear 19:adae367247d4 247 }
itslinear 20:a90e5b54bf7b 248
itslinear 21:69ee872b8ee9 249 if(ii > 999 && ii < 1800) {
itslinear 20:a90e5b54bf7b 250 desiredSpeedLeft = v*2;
itslinear 20:a90e5b54bf7b 251 desiredSpeedRight = v*2;
itslinear 20:a90e5b54bf7b 252 }
itslinear 20:a90e5b54bf7b 253
itslinear 19:adae367247d4 254 ii++;
itslinear 19:adae367247d4 255
itslinear 21:69ee872b8ee9 256 if(ii > 1799) {
itslinear 19:adae367247d4 257 desiredSpeedLeft = 0;
itslinear 19:adae367247d4 258 desiredSpeedRight = 0;
itslinear 19:adae367247d4 259 ii = 0;
itslinear 19:adae367247d4 260 s = 1;
itslinear 19:adae367247d4 261 }
itslinear 20:a90e5b54bf7b 262
itslinear 19:adae367247d4 263 return s;
itslinear 19:adae367247d4 264 }
itslinear 19:adae367247d4 265
itslinear 18:3ee1b02ed3aa 266 int Roboter::kreisFahren()
itslinear 18:3ee1b02ed3aa 267 {
itslinear 18:3ee1b02ed3aa 268 int r;
itslinear 21:69ee872b8ee9 269 static int anzahl = 0;
itslinear 18:3ee1b02ed3aa 270 static int time1 = 0;
itslinear 21:69ee872b8ee9 271
itslinear 21:69ee872b8ee9 272 if(time1 < 6 && anzahl%2 == 0) { // 1s im Kreis drehen
itslinear 21:69ee872b8ee9 273 desiredSpeedLeft = v*2;
itslinear 21:69ee872b8ee9 274 desiredSpeedRight = v*2;
itslinear 18:3ee1b02ed3aa 275 time1 ++;
itslinear 18:3ee1b02ed3aa 276 r = 1; // state
itslinear 18:3ee1b02ed3aa 277 } else {
itslinear 21:69ee872b8ee9 278 desiredSpeedLeft = -(v*2);
itslinear 21:69ee872b8ee9 279 desiredSpeedRight = -(v*2);
itslinear 21:69ee872b8ee9 280 time1 ++;
itslinear 21:69ee872b8ee9 281 r = 1; // state
itslinear 21:69ee872b8ee9 282 }
itslinear 21:69ee872b8ee9 283 if(time1 == 6) {
itslinear 18:3ee1b02ed3aa 284 time1 = 0;
itslinear 21:69ee872b8ee9 285 anzahl ++;
itslinear 18:3ee1b02ed3aa 286 desiredSpeedLeft = 0.0f;
itslinear 18:3ee1b02ed3aa 287 desiredSpeedRight = 0.0f;
itslinear 18:3ee1b02ed3aa 288 r = 3; // state
itslinear 18:3ee1b02ed3aa 289 }
itslinear 18:3ee1b02ed3aa 290 return r;
itslinear 18:3ee1b02ed3aa 291 }
itslinear 18:3ee1b02ed3aa 292
itslinear 20:a90e5b54bf7b 293 int Roboter::stateAuswahl(float cam, int tempState, int temp)
itslinear 18:3ee1b02ed3aa 294 {
itslinear 20:a90e5b54bf7b 295 int s = 1;
itslinear 18:3ee1b02ed3aa 296 if(cam >= 100.0f && cam < 400.0f ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399
itslinear 18:3ee1b02ed3aa 297 s = 4;
itslinear 18:3ee1b02ed3aa 298 }
itslinear 20:a90e5b54bf7b 299 if(cam == 400.0f && temp != 5) { // Roboter in Position
itslinear 18:3ee1b02ed3aa 300 s = 5;
itslinear 18:3ee1b02ed3aa 301 }
itslinear 21:69ee872b8ee9 302 if(cam == 400.0f && temp == 5) { // Roboter immer noch beim selben Klotz -> back()
itslinear 20:a90e5b54bf7b 303 s = 7;
itslinear 20:a90e5b54bf7b 304 }
itslinear 20:a90e5b54bf7b 305 if(cam < 1) {
itslinear 20:a90e5b54bf7b 306 s = tempState;
itslinear 18:3ee1b02ed3aa 307 }
itslinear 19:adae367247d4 308 if(cam == 1) {
itslinear 19:adae367247d4 309 s = 1;
itslinear 19:adae367247d4 310 }
itslinear 18:3ee1b02ed3aa 311 return s;
itslinear 0:306a2438de17 312
schneju2 2:953263712480 313
itslinear 3:24e098715b78 314 }
itslinear 18:3ee1b02ed3aa 315
itslinear 18:3ee1b02ed3aa 316 void Roboter::initSpeedcontrol()
itslinear 18:3ee1b02ed3aa 317 {
itslinear 18:3ee1b02ed3aa 318 // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
itslinear 18:3ee1b02ed3aa 319 pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
itslinear 18:3ee1b02ed3aa 320 pwmR->period(0.00005f);
itslinear 18:3ee1b02ed3aa 321 *pwmL = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
itslinear 18:3ee1b02ed3aa 322 *pwmR = 0.5f; // Duty-Cycle von 50%
itslinear 18:3ee1b02ed3aa 323
itslinear 18:3ee1b02ed3aa 324 // Initialisieren von lokalen Variabeln
itslinear 18:3ee1b02ed3aa 325 previousValueCounterLeft = counterLeft->read();
itslinear 18:3ee1b02ed3aa 326 previousValueCounterRight = counterRight->read();
itslinear 18:3ee1b02ed3aa 327 speedLeftFilter.setPeriod(PERIOD);
itslinear 18:3ee1b02ed3aa 328 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
itslinear 18:3ee1b02ed3aa 329 speedRightFilter.setPeriod(PERIOD);
itslinear 18:3ee1b02ed3aa 330 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
itslinear 18:3ee1b02ed3aa 331
itslinear 18:3ee1b02ed3aa 332 desiredSpeedLeft = 0.0f;
itslinear 18:3ee1b02ed3aa 333 desiredSpeedRight = 0.0f;
itslinear 18:3ee1b02ed3aa 334 actualSpeedLeft = 0.0f;
itslinear 18:3ee1b02ed3aa 335 actualSpeedRight = 0.0f;
itslinear 18:3ee1b02ed3aa 336
itslinear 18:3ee1b02ed3aa 337 *enableMotorDriver = 1;
itslinear 18:3ee1b02ed3aa 338 }
itslinear 18:3ee1b02ed3aa 339
itslinear 18:3ee1b02ed3aa 340 void Roboter::speedCtrl()
itslinear 18:3ee1b02ed3aa 341 {
itslinear 18:3ee1b02ed3aa 342 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
itslinear 18:3ee1b02ed3aa 343 short valueCounterLeft = counterLeft->read();
itslinear 18:3ee1b02ed3aa 344 short valueCounterRight = counterRight->read();
itslinear 18:3ee1b02ed3aa 345 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
itslinear 18:3ee1b02ed3aa 346 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
itslinear 18:3ee1b02ed3aa 347
itslinear 18:3ee1b02ed3aa 348 previousValueCounterLeft = valueCounterLeft;
itslinear 18:3ee1b02ed3aa 349 previousValueCounterRight = valueCounterRight;
itslinear 18:3ee1b02ed3aa 350 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
itslinear 18:3ee1b02ed3aa 351 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
itslinear 18:3ee1b02ed3aa 352
itslinear 18:3ee1b02ed3aa 353 // Berechnen der Motorspannungen Uout
itslinear 18:3ee1b02ed3aa 354 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
itslinear 18:3ee1b02ed3aa 355 float voltageRight = KP*(desiredSpeedRight*correction-actualSpeedRight)+desiredSpeedRight*correction/KN;
itslinear 18:3ee1b02ed3aa 356
itslinear 18:3ee1b02ed3aa 357 // Berechnen, Limitieren und Setzen der Duty-Cycle
itslinear 18:3ee1b02ed3aa 358 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
itslinear 18:3ee1b02ed3aa 359 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
itslinear 18:3ee1b02ed3aa 360 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
itslinear 18:3ee1b02ed3aa 361
itslinear 18:3ee1b02ed3aa 362 *pwmL = dutyCycleLeft;
itslinear 18:3ee1b02ed3aa 363 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
itslinear 18:3ee1b02ed3aa 364 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
itslinear 18:3ee1b02ed3aa 365 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
itslinear 18:3ee1b02ed3aa 366
itslinear 18:3ee1b02ed3aa 367 *pwmR = dutyCycleRight;
itslinear 18:3ee1b02ed3aa 368 }