Revision 8:8ed679044a72, committed 2018-04-17

Comitter:

altb

Date:

Tue Apr 17 11:47:35 2018 +0000

Parent:

7:72982ede2ff6

Child:

9:dc0eb7dd0d92

Commit message:

RT2_P3_W2, 2nd session

Changed in this revision

--- a/GPA.h	Tue Apr 17 09:06:47 2018 +0000
+++ b/GPA.h	Tue Apr 17 11:47:35 2018 +0000
@@ -58,4 +58,7 @@
     void   calcGPAmeasPara(double fexcDes_i);
     void   printLine();
 
-};
\ No newline at end of file
+};
+
+
+#endif /* GPA_H_ */
\ No newline at end of file

--- a/IIR_filter.h	Tue Apr 17 09:06:47 2018 +0000
+++ b/IIR_filter.h	Tue Apr 17 11:47:35 2018 +0000
@@ -1,3 +1,6 @@
+#ifndef IIR_FILTER_H_
+#define IIR_FILTER_H_
+
 class IIR_filter{
      public:
      
@@ -22,4 +25,6 @@
         double *uk;
         double *yk;
         double K;
-};
\ No newline at end of file
+};
+
+#endif /* IIR_FILTER_H_ */
\ No newline at end of file

--- a/PI_Cntrl.cpp	Tue Apr 17 09:06:47 2018 +0000
+++ b/PI_Cntrl.cpp	Tue Apr 17 11:47:35 2018 +0000
@@ -1,31 +1,61 @@
-/*#include "PI_Cntrl.h"
+/*  
+    PI Controller class with anti windup reset in biquad transposed direct form 2
+    see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
+    everything is calculated in double
+    
+                  Tn*s + 1                      
+      G(s) = Kp -------------  with s ~ (1 - z^-1)/Ts
+                    Ts*s                     
+*/
+
+#include "PI_Cntrl.h"
 
 using namespace std;
 
-PI_Cntrl::PI_Cntrl(float Kp, float Tn){
-    this->Kp = Kp;
-    this->Tn = Tn_;
-    
-    //...
-    
-    
-    }
+PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts)
+{
+    setCoefficients(Kp, Tn, Ts);
+    uMax = 10000000000.0;
+    uMin = -uMax;
+    reset(0.0f);
+}
 
+PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax)
+{
+    setCoefficients(Kp, Tn, Ts);
+    this->uMax = (double)uMax;
+    uMin = -(double)uMax;
+    reset(0.0f);
+}
 
-PI_Cntrl::~PI_Cntrl() {} 
-    
-void PI_Cntrl::reset(float initValue) {
-//...
-
+PI_Cntrl::PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin)
+{
+    setCoefficients(Kp, Tn, Ts);
+    this->uMax = (double)uMax;
+    this->uMin = (double)uMin;
+    reset(0.0f);
 }
 
-float PI_Cntrl::doStep(float error){
-    
-    // ...
-    
-    // -------- Anti-Windup -------- 
-    // ...
-    // -------- Timeshift --------
-    
-    
-    }*/
\ No newline at end of file
+PI_Cntrl::~PI_Cntrl() {}
+
+void PI_Cntrl::reset(float initValue)
+{
+    s = (double)initValue;
+}
+
+void PI_Cntrl::setCoefficients(float Kp, float Tn, float Ts)
+{
+    b0 = (double)Kp*(1.0 + (double)Ts/(double)Tn);
+    b1 = -(double)Kp;
+    b2 = (double)Ts/(double)Tn;    
+}
+
+float PI_Cntrl::doStep(double e)
+{
+    double u = b0*e + s;          // unconstrained output
+    double uc = u;                // constrained output
+    if(u > uMax) uc = uMax;
+    else if(u < uMin) uc = uMin;
+    s = b1*e + u - b2*(u - uc);
+    return (float)uc;
+}
\ No newline at end of file

--- a/PI_Cntrl.h	Tue Apr 17 09:06:47 2018 +0000
+++ b/PI_Cntrl.h	Tue Apr 17 11:47:35 2018 +0000
@@ -2,17 +2,35 @@
 #define PI_CNTRL_H_
 
 
-class PI_Cntrl{
-     public:
+class PI_Cntrl
+{
+public:
 
-// ....    
+    PI_Cntrl(float Kp, float Tn, float Ts);
+    PI_Cntrl(float Kp, float Tn, float Ts, float uMax);
+    PI_Cntrl(float Kp, float Tn, float Ts, float uMax, float uMin);
 
+    float operator()(float error) {
+        return doStep((double)error);
+    }
+
+    virtual     ~PI_Cntrl();
 
-    private:
+    void        reset(float initValue);
+    float       doStep(double error);
+
+private:
+
+    double b0;
+    double b1;
+    double b2;
+    double s;
+    double uMax;
+    double uMin;
     
-    // ....
-    
-    };
+    void        setCoefficients(float Kp, float Tn, float Ts);
+
+};
     
     
 #endif      // PI_CNTRL_H_
\ No newline at end of file

--- a/main.cpp	Tue Apr 17 09:06:47 2018 +0000
+++ b/main.cpp	Tue Apr 17 11:47:35 2018 +0000
@@ -8,6 +8,7 @@
 #include "IIR_filter.h"
 #include "LinearCharacteristics.h"
 #include "PI_Cntrl.h"
+#include "GPA.h"
 /* Cuboid balance on one edge on Nucleo F446RE
 
  **** IMPORTANT: use ..\Labormodelle\RT-MOD054 - Würfel\Escon_Parameter_4nucleo.edc 
@@ -50,7 +51,7 @@
 //------------------------------------------
 // ... here define variables like gains etc.
 //------------------------------------------
-LinearCharacteristics i2u(1.5f,2.5f);
+LinearCharacteristics i2u(0.8f,-2.0f);
 
 //------------------------------------------
 Ticker  ControllerLoopTimer;            // interrupt for control loop
@@ -59,7 +60,10 @@
 //------------------------------------------
 // ... here define instantiate classes
 //------------------------------------------
-
+PI_Cntrl vel_cntrl(0.5f,.05f,Ts,0.4f);
+GPA gpa1(1.0f, 200.0f,      150,       4,      400, Ts, 10.0f, 0.3f);
+float excWobble = 0.0f;
+// GPA(t fMin, t fMax, NfexcDes, NperMin, NmeasMin, Ts, Aexc0, Aexc1)
 // ... define some linear characteristics -----------------------------------------
 
 // ----- User defined functions -----------
@@ -85,10 +89,18 @@
 void updateControllers(void){
     short counts = counter1;            // get counts from Encoder
     float vel = diff(counts);           // motor velocity 
-     // ... your code 
+     /* desTorque = pi_w(omega_desired - omega + excWobble);
+            outWobble = omega;
+            excWobble = Wobble(excWobble, outWobble); */
+    
+    float torq_des = vel_cntrl(excWobble + w_soll - vel);
+    excWobble = gpa1(torq_des,vel);
+    out.write(i2u(torq_des/0.217f));     // the controller! convert torque to Amps km = 0.217 Nm/A
+    
+    
   //  if(++k >= 249){
   //      k = 0;
-        pc.printf("i2u(8): %3.3f\r\n",i2u(8.0f));
+        //pc.printf("Des. velocity: %3.3f, Velocity: %3.3f\r\n",w_soll,vel);
     //}
 }
 //******************************************************************************