READY TO RUMBLE
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Diff: Controller.cpp
- Revision:
- 5:b8b1a979b0d5
- Parent:
- 2:592f01278db4
diff -r e3f388933954 -r b8b1a979b0d5 Controller.cpp --- a/Controller.cpp Thu Apr 12 16:14:02 2018 +0000 +++ b/Controller.cpp Wed Apr 25 12:07:03 2018 +0000 @@ -16,12 +16,13 @@ Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, - EncoderCounter& counterLeft, EncoderCounter& counterRight) : - pwmLeft(pwmLeft), pwmRight(pwmRight), - counterLeft(counterLeft), counterRight(counterRight) { + EncoderCounter& counterLeft, EncoderCounter& counterRight) : + pwmLeft(pwmLeft), pwmRight(pwmRight), + counterLeft(counterLeft), counterRight(counterRight) +{ // Initialisieren der PWM Ausgaenge - + pwmLeft.period(0.00005f); // PWM Periode von 50 us pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us @@ -94,10 +95,21 @@ return (desiredSpeedRight-actualSpeedRight); } -void Controller::run() { +void Controller::reset() +{ + ticker.detach(); + counterRight.reset(); + counterLeft.reset(); + previousValueCounterLeft = counterLeft.read(); + previousValueCounterRight = counterRight.read(); + ticker.attach(callback(this, &Controller::run), PERIOD); +} + +void Controller::run() +{ // Berechnen die effektiven Drehzahlen der Motoren in [rpm] - + short valueCounterLeft = counterLeft.read(); short valueCounterRight = counterRight.read(); @@ -116,33 +128,33 @@ //Berechnung I - Anteil - - iSumLeft += (desiredSpeedLeft-actualSpeedLeft); - if (iSumLeft > I_MAX) iSumLeft = I_MAX; //Max Saettigung I - Anteil + + iSumLeft += (desiredSpeedLeft-actualSpeedLeft); + if (iSumLeft > I_MAX) iSumLeft = I_MAX; //Max Saettigung I - Anteil if (iSumLeft < -I_MAX) iSumLeft = -I_MAX; //Min Saettigung I - Anteil - iSumRight += (desiredSpeedRight-actualSpeedRight); - if (iSumRight > I_MAX) iSumRight = I_MAX; //Max Saettigung I - Anteil + iSumRight += (desiredSpeedRight-actualSpeedRight); + if (iSumRight > I_MAX) iSumRight = I_MAX; //Max Saettigung I - Anteil if (iSumRight < -I_MAX) iSumRight = -I_MAX; //Min Saettigung I - Anteil - + // Berechnen der Motorspannungen Uout - + float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+KI*iSumLeft*PERIOD +desiredSpeedLeft/KN; float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+KI*iSumRight*PERIOD - +desiredSpeedRight/KN; - + +desiredSpeedRight/KN; + // Berechnen, Limitieren und Setzen der Duty-Cycle - + float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; pwmLeft = dutyCycleLeft; - + float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE; if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE; else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE; pwmRight = dutyCycleRight; - - + + } \ No newline at end of file