Diff: Classes/Controller.cpp

Revision:

5:93d3efe46493

Parent:

0:b886f13e4ac6

--- a/Classes/Controller.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/Controller.cpp	Wed May 09 13:33:59 2018 +0000
@@ -3,14 +3,17 @@
 using namespace std;
 
 const float Controller::PERIOD = 0.001f; // Periode von 1 ms
-const float Controller::COUNTS_PER_TURN = 1260.0f; // Encoder-Aufloesung
+const float Controller::COUNTS_PER_TURN = 1560.0f;//1200.0f; // Encoder-Aufloesung
 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // in [rad/s]
-const float Controller::KN = 18.75f; // Drehzahlkonstante in [rpm/V]
-const float Controller::KP = 0.02f; // Regler-Parameter
+const float Controller::KN = 15.0f;//40.0f; // Drehzahlkonstante in [rpm/V]
+const float Controller::KP = 0.25f; // KP Regler-Parameter
+const float Controller::KI = 1.0f; // KI Regler-Parameter
+const float Controller::I_MAX = 1000.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
 
+
 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
                         EncoderCounter& counterLeft, EncoderCounter& counterRight) :
                         pwmLeft(pwmLeft), pwmRight(pwmRight),
@@ -49,11 +52,6 @@
     ticker.detach(); // Stoppt den periodischen Task
 }
 
-
-void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
-{
-    this->desiredSpeedLeft = desiredSpeedLeft;
-}
 void Controller::resetCounter() 
 {
     ticker.detach();
@@ -61,7 +59,12 @@
     counterRight.reset();
     previousValueCounterLeft = counterLeft.read();
     previousValueCounterRight = counterRight.read();
-    ticker.attach(callback(this, &Controller::run), PERIOD);  
+    ticker.attach(callback(this, &Controller::run), PERIOD); 
+}
+
+void Controller::setDesiredSpeedLeft(float desiredSpeedLeft)
+{
+    this->desiredSpeedLeft = desiredSpeedLeft;
 }
 
 void Controller::setDesiredSpeedRight(float desiredSpeedRight)
@@ -69,6 +72,36 @@
     this->desiredSpeedRight = desiredSpeedRight;
 }
 
+float Controller::getSpeedLeft()
+{
+    return actualSpeedLeft;
+}
+
+float Controller::getSpeedRight()
+{
+    return actualSpeedRight;
+}
+
+float Controller::getIntegralLeft()
+{
+    return KI*iSumLeft*PERIOD;
+}
+
+float Controller::getIntegralRight()
+{
+    return KI*iSumRight*PERIOD;
+}
+
+float Controller::getProportionalLeft()
+{
+    return KP*(desiredSpeedLeft-actualSpeedLeft);
+}
+
+float Controller::getProportionalRight()
+{
+    return KP*(desiredSpeedRight-actualSpeedRight);
+}
+
 void Controller::run() {
 
     // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
@@ -87,20 +120,33 @@
     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight
                        /COUNTS_PER_TURN/PERIOD*60.0f);
 
-    // Berechnen der Motorspannungen Uout
+
+    //Berechnung I - Anteil
+
     
-    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
-    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)
-                         +desiredSpeedRight/KN;
+    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;
+    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;
+    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;