Gruppe 3 / Mbed 2 deprecated PES3

hallo

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

Roboter.cpp@20:a90e5b54bf7b, 2017-05-13 (annotated)

Committer:
itslinear
Date:
Sat May 13 16:50:57 2017 +0000
Revision:
20:a90e5b54bf7b
Parent:
19:adae367247d4
Child:
21:69ee872b8ee9
huiiii

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