READY TO RUMBLE
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
Diff: Controller.cpp
- Revision:
- 2:592f01278db4
- Parent:
- 1:d9e840c48b1e
- Child:
- 5:b8b1a979b0d5
diff -r d9e840c48b1e -r 592f01278db4 Controller.cpp --- a/Controller.cpp Sat Mar 31 16:45:57 2018 +0000 +++ b/Controller.cpp Wed Apr 04 15:24:28 2018 +0000 @@ -2,27 +2,26 @@ using namespace std; -const float Controller::PERIOD = 0.001f; // Periode von 1 ms -const float Controller::COUNTS_PER_TURN = 1200.0f; // Encoder-Aufloesung +const float Controller::PERIOD = 0.001f; // Periode von 1 ms //const float Controller::PERIOD = 0.001f; // Periode von 1 ms +const float Controller::COUNTS_PER_TURN = 1560.0f; // Encoder-Aufloesung //const float Controller::COUNTS_PER_TURN = 1200.0f; // Encoder-Aufloesung (20Counts rp Umdrehung und Getriebe 78:1) => 1560 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s] -const float Controller::KN = 40.0f; // Drehzahlkonstante in [rpm/V] -const float Controller::KP = 0.25f; // KP Regler-Parameter -const float Controller::KI = 4.0f; // KI Regler-Parameter +const float Controller::KN = 15.0f; // Drehzahlkonstante in [rpm/V] //const float Controller::KN = 40.0f; // Drehzahlkonstante in [rpm/V] Nenndrehzahl durch max Spannung = 180Rpm/12V +const float Controller::KP = 0.25f; // KP Regler-Parameter //const float Controller::KP = 0.25f; // KP Regler-Parameter +const float Controller::KI = 4.0f; // KI Regler-Parameter //const float Controller::KI = 4.0f; // KI Regler-Parameter const float Controller::I_MAX = 10000.0f; // KI Regler-Parameter Saettigung const float Controller::MAX_VOLTAGE = 12.0f; // Batteriespannung in [V] const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimale Duty-Cycle const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximale Duty-Cycle -int ii =0; + 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 @@ -95,11 +94,10 @@ return (desiredSpeedRight-actualSpeedRight); } -void Controller::run() -{ +void Controller::run() { // Berechnen die effektiven Drehzahlen der Motoren in [rpm] - + short valueCounterLeft = counterLeft.read(); short valueCounterRight = counterRight.read(); @@ -115,34 +113,36 @@ /COUNTS_PER_TURN/PERIOD*60.0f); + //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