Diff: MotorDriver/Controller.cpp

Revision:

0:e38b500d6e74

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver/Controller.cpp	Thu Apr 19 11:31:49 2018 +0000
@@ -0,0 +1,160 @@
+#include "Controller.h"
+
+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::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::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) {
+
+    // 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
+    pwmRight = 0.5f; // Duty-Cycle von 50%
+
+    // Initialisieren von lokalen Variabeln
+
+    previousValueCounterLeft = counterLeft.read();
+    previousValueCounterRight = counterRight.read();
+
+    speedLeftFilter.setPeriod(PERIOD);
+    speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+    speedRightFilter.setPeriod(PERIOD);
+    speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+    desiredSpeedLeft = 0.0f;
+    desiredSpeedRight = 0.0f;
+
+    actualSpeedLeft = 0.0f;
+    actualSpeedRight = 0.0f;
+
+    // Starten des periodischen Tasks
+    ticker.attach(callback(this, &Controller::run), PERIOD);
+}
+
+Controller::~Controller()
+{
+    ticker.detach(); // Stoppt den periodischen Task
+}
+
+void Controller::AttachTicker(void)
+{
+    previousValueCounterLeft = counterLeft.read();
+    previousValueCounterRight = counterRight.read();
+
+    //actualSpeedLeft = 0.0f;
+    //actualSpeedRight = 0.0f;
+    ticker.attach(callback(this, &Controller::run), PERIOD);
+}
+
+void Controller::DetachTicker(void)
+{
+    wait(0.5);
+    ticker.detach();
+}
+void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
+{
+    this->desiredSpeedLeft = desiredSpeedLeft;
+}
+
+void Controller::setDesiredSpeedRight(float desiredSpeedRight)
+{
+    this->desiredSpeedRight = desiredSpeedRight;
+}
+
+float Controller::getSpeedLeft()
+{
+    return actualSpeedLeft;
+}
+
+float Controller::getSpeedRight()
+{
+    return actualSpeedRight;
+}
+
+float Controller::getIntegralLeft()
+{
+    return iSumLeft;
+}
+
+float Controller::getIntegralRight()
+{
+    return iSumRight;
+}
+
+float Controller::getProportionalLeft()
+{
+    return (desiredSpeedLeft-actualSpeedLeft);
+}
+
+float Controller::getProportionalRight()
+{
+    return (desiredSpeedRight-actualSpeedRight);
+}
+
+void Controller::run() {
+
+    // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
+    short valueCounterLeft = counterLeft.read();
+    short valueCounterRight = counterRight.read();
+
+    short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
+    short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
+
+    previousValueCounterLeft = valueCounterLeft;
+    previousValueCounterRight = valueCounterRight;
+
+    actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft
+                      /COUNTS_PER_TURN/PERIOD*60.0f);
+    actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
+                       /COUNTS_PER_TURN/PERIOD*60.0f);
+
+
+    //Berechnung 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       
+    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;                                   
+                         
+    // 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;
+    
+}
+