Revision 2:1ded9d10f322, committed 2019-05-14

Comitter:

voegetob

Date:

Tue May 14 15:29:24 2019 +0000

Parent:

1:92f175969d90

Commit message:

pr7

Changed in this revision

--- a/PID_Cntrl.cpp	Fri May 10 14:25:24 2019 +0000
+++ b/PID_Cntrl.cpp	Tue May 14 15:29:24 2019 +0000
@@ -18,7 +18,15 @@
 PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax)
 {
     // link member variables
-    // ???
+    this->Kp = Kp;
+    this->Ki = Ki;
+    this->Kd = Kd;
+    
+    this->Tf = Tf;
+    this->Ts = Ts;
+    
+    this->uMin = uMin;
+    this->uMax = uMax;
     
     reset(0.0f);
 }
@@ -29,7 +37,9 @@
 {
 
     // implement controller reset
-    // ???
+    e_old = 0.0f;
+    uI_old = initValue;
+    uD_old = 0.0f;
 
 }
 
@@ -39,23 +49,31 @@
     // controller update function 
     
     // calculate uI
-    // ???
+    float uI = Ki * Ts * e + uI_old;
     
     // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
-    // ???
+    if (uI > uMax)
+        uI = uMax;
+    else if (uI < uMin)
+        uI = uMin;
 
     // calculate uD
-    // ???
+    float uD = (Kd * (e - e_old) + Tf * uD_old) / (Tf + Ts);
     
     // calculate u
-    // ???
+    float u = (Kp * e) + uI + uD;
     
     // saturate u, uMin <= u <= uMax
-    // ???
+    if (u > uMax)
+        u = uMax;
+    else if (u < uMin)
+        u = uMin;
     
     // update signal storage
-    // ???
+    e_old = e;
+    uI_old = uI;
+    uD_old = uD;
 
-    return 0.0f;
+    return u;
 }
 

--- a/PID_Cntrl.h	Fri May 10 14:25:24 2019 +0000
+++ b/PID_Cntrl.h	Tue May 14 15:29:24 2019 +0000
@@ -20,10 +20,10 @@
 private:
 
     // controller parameters (member variables)
-    // ???
+    float Kp, Ki, Kd, Tf, Ts, uMin, uMax;
         
     // storage for signals (member variables)
-    // ???
+    float e_old, uI_old, uD_old;
 };
 
 #endif

--- a/main.cpp	Fri May 10 14:25:24 2019 +0000
+++ b/main.cpp	Tue May 14 15:29:24 2019 +0000
@@ -36,8 +36,9 @@
 float Ts = 0.0005f;                     // sample time of main loop
 uint16_t k = 0;
 // initiate PIDT1 controller objects
-// ???
-// ???
+PID_Cntrl dt1( 0.0f, 0.0f, 0.0186f, 0.000667f, Ts, -3.0f, 3.0f); 
+PID_Cntrl pi( 8.13f, 15.0f, 0.0f, 0.0f, Ts, -3.0f, 3.0f); // Ki 38.8
+float w = 0.5f;
 //******************************************************************************
 //---------- main loop -------------
 //******************************************************************************
@@ -61,13 +62,13 @@
     float i_des = 0.0f;         // default: set motor current to zero (will be overwritten)
     if(controller_active) {
         // controller update
-        // ???
+        i_des = pi(w - x) - dt1(x);
     }
-    out.write(i2u(i_des));
+    out.write(i2u(i_des)); 
     if(++k>1000) {
         pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des);
         k = 0;
-
+        w = -w;
     }
 } // END OF updateLoop(void)
 
@@ -91,7 +92,8 @@
         if(controller_active) {
             pc.printf("Controller actived\r\n");
             // reset controller here!!!
-            // ???
+            dt1.reset(0.0f);
+            pi.reset(0.0f);
         } else
             pc.printf("Controller disabled\r\n");
     }