Gruppe 3 / Mbed 2 deprecated PES3

hallo

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

Roboter.cpp@19:adae367247d4, 2017-05-12 (annotated)

Committer:
itslinear
Date:
Fri May 12 12:25:34 2017 +0000
Revision:
19:adae367247d4
Parent:
18:3ee1b02ed3aa
Child:
20:a90e5b54bf7b
hallo balz

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