Diff: Controller.cpp

Revision:

2:592f01278db4

Parent:

1:d9e840c48b1e

Child:

5:b8b1a979b0d5

--- 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