Diff: Peripherien/Regler.cpp

Revision:

8:1655d27152e6

Parent:

7:15e6fc689368

Child:

9:56aed8c6779f

--- a/Peripherien/Regler.cpp	Thu May 16 20:42:39 2019 +0000
+++ b/Peripherien/Regler.cpp	Fri May 17 14:35:54 2019 +0000
@@ -5,6 +5,20 @@
 
     this->torqueRatio = 50;
     this->recupRatio = 50;
+    // Setup Regler
+    this->pid.setMode(1);                       // Automode
+    this->Ts_ = 0.005;                          // Zeit interval
+    this->Kp_ = 1.0;                            // Proportional Gain
+    this->Ki_ = 30.0;                           // Integral Gain
+    this->pid.setInterval(this->Ts_);           // Set Ts
+    this->pid.setTunings(this->Kp_,this->Ki_,0);// Set Parameters (Kd = 0)
+    this->setPoint_ = 60.0;                       // Set Default SetPont to 60 RPM  
+    
+    // Default Ellipse Parameters
+    a_ = 1.0f; // % of Torque Max 0..1
+    b_ = 0.6f; // % of Torque Max 0..1
+    beta_ = 0.52f; // 30°
+    
     this->ticker.attach(this,&Regler::ReglerCalculate,0.005);
 }
 
@@ -13,7 +27,7 @@
 }
 
 void Regler::setTorqueProportion(uint8_t torqueRatio){
-    this->torqueRatio = torqueRatio > 100 ? 100 : recupRatio;
+    this->torqueRatio = torqueRatio > 100 ? 100 : torqueRatio;
 }
 
 void Regler::setRecuperationProportion(uint8_t recupRatio){
@@ -52,6 +66,33 @@
     this->linkspedal.setRecuperation(recuperation);
 }
 
+void Regler::setEllipseParameters(float a, float b, float beta){
+    this->a_ = a;
+    this->b_ = b;
+    this->beta_ = beta; 
+}
+
+void Regler::setReglerParameters(float Kp, float Ki, float Ts){
+    this->Kp_ = Kp;
+    this->Ki_ = Ki;
+    this->Ts_ = Ts; 
+}
+
+void Regler::setReglerSetPoint(float Setpoint){
+    this->setPoint_ = Setpoint; 
+}
+void Regler::setErgoStufe(float ErgoStufe){
+    this->ErgoStufe_ = ErgoStufe;
+}
+    
+void Regler::setPedStufe(float PedStufe){
+    this->PedStufe_ = PedStufe;   
+}
+
+void Regler::setPedFactor(float PedFactor){
+    this->PedFactor_ = PedFactor;   
+}
+
 void Regler::ReglerCalculate(void){
     float Angle, RPM, Acc;                          // Value form Encoder
     float F_RPM, F_Acc;                             // Filtered Values
@@ -63,25 +104,33 @@
     float Omega_cAcc = 2*PI*F_ab/200;               // 200 Mal kleiner als die Abtastfrequenz
     float sfAcc = (Omega_cAcc*T_ab)/(1+Omega_cAcc*T_ab);//smoothing factor Lowpass Acceleration
     float R, a, b, beta, phi;                       // Ellipse paramter
+    float ReglerWert;                                // Ausgangswert des Reglers
+    // Set local value
+    a=this->a_;
+    b=this->b_;
+    beta=this->beta_;
     
-     
     //Read Value from Encoder
     Angle = encoder.readAngle();
     RPM = encoder.readRPM();
     Acc = encoder.readAcceleration();
+    
     // LowPass Filter RPM
     F_RPM = sfRPM * RPM + (1-sfRPM)*F_RPMOld;
             
-    // LowPass Filter ACC
+    // LowPass Filter Acceleration
     F_Acc = sfAcc * Acc + (1-sfAcc)*F_AccOld;
     
     // Ellipse
-    a = 1.0f; // % of Torque Max 0..1
-    b = 0.6f; // % of Torque Max 0..1
-    beta = 0.52f; // 30°
     phi = Angle;  
     R = sqrt(pow(a,2) * pow(sin(beta + phi),2) + pow(b,2) * pow(cos(beta + phi),2)); // Torque in function of the Ellipse parameters
     
+    // PI Regler
+    pid.setTunings(this->Kp_,this->Ki_,0);
+    pid.setSetPoint(this->setPoint_);
+    pid.setProcessValue(F_RPM);             // Neue Ist-Wert für den Regler
+    ReglerWert = pid.compute();             // Der Regler berechnet den neuen Wert
+