Revision 2:394782101261, committed 2019-05-16

Comitter:

kellep15

Date:

Thu May 16 15:34:29 2019 +0000

Parent:

1:92f175969d90

Commit message:

rEGLER TUT NICHT

Changed in this revision

--- a/PID_Cntrl.cpp	Fri May 10 14:25:24 2019 +0000
+++ b/PID_Cntrl.cpp	Thu May 16 15:34:29 2019 +0000
@@ -15,10 +15,15 @@
 #include "PID_Cntrl.h"
 using namespace std;
 
-PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax)
+PID_Cntrl::PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float yMin, float yMax)
 {
     // link member variables
-    // ???
+    this->Kp = Kp;
+    this->Ki = Ki;
+    this->Tf = Tf;
+    this->Ts = Ts;
+    this->yMin = yMin;
+    this->yMax = yMax;
     
     reset(0.0f);
 }
@@ -29,33 +34,46 @@
 {
 
     // implement controller reset
-    // ???
-
+    e_old = 0.0;
+    y_Dold = 0.0;
+    y_I = initValue; 
 }
 
-float PID_Cntrl::update(double e)
+float PID_Cntrl::update(float e)
 {
 
     // controller update function 
     
     // calculate uI
-    // ???
+    y_I = y_I + Ki*Ts*e; 
     
     // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
-    // ???
+    if(y_I > yMax)
+        y_I = yMax;
+    else if(y_I < yMin)
+        y_I = yMin; 
 
     // calculate uD
-    // ???
+    y_Dold = (1.0f - Ts/Tf)*y_Dold + Kd / Tf * (e-e_old);
+    
+    float y = Kp * e + y_I + y_Dold;
+    
+    // update signal storage
+    e_old = e; 
+    
     
     // calculate u
     // ???
     
     // saturate u, uMin <= u <= uMax
-    // ???
+    if(y > yMax)
+        y = yMax;
+    else if(y < yMin)
+        y = yMin; 
     
     // update signal storage
     // ???
 
-    return 0.0f;
+    return y;
 }
 

--- a/PID_Cntrl.h	Fri May 10 14:25:24 2019 +0000
+++ b/PID_Cntrl.h	Thu May 16 15:34:29 2019 +0000
@@ -9,21 +9,22 @@
     PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax);
 
     float operator()(float error) {
-        return update((double)error);
+        return update(error);
     }
 
     virtual     ~PID_Cntrl();
 
     void        reset(float initValue);
-    float       update(double error);
+    float       update(float error);
     
 private:
 
     // controller parameters (member variables)
-    // ???
+    float Kp, Ki, Kd, Tf, Tx, Ts, yMin, yMax;
+    
         
     // storage for signals (member variables)
-    // ???
+    float e_old, y_I, y_Dold;
 };
 
 #endif

--- a/main.cpp	Fri May 10 14:25:24 2019 +0000
+++ b/main.cpp	Thu May 16 15:34:29 2019 +0000
@@ -35,9 +35,11 @@
 void updateLoop(void);   // loop for State machine (via interrupt)
 float Ts = 0.0005f;                     // sample time of main loop
 uint16_t k = 0;
+float w = 1; 
 // initiate PIDT1 controller objects
-// ???
-// ???
+PID_Cntrl dt1(0.0, 0.0, 0.0116, 0.000625, Ts, -3.0, 3.0);
+PID_Cntrl pi1(3.59, 13.5, 0.0, 1.0, Ts, -3.0, 3.0);
+// 
 //******************************************************************************
 //---------- main loop -------------
 //******************************************************************************
@@ -61,13 +63,14 @@
     float i_des = 0.0f;         // default: set motor current to zero (will be overwritten)
     if(controller_active) {
         // controller update
-        // ???
+        i_des = -dt1(x);
+        //i_des = pi1(w-x)-dt1(x); 
     }
     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 +94,7 @@
         if(controller_active) {
             pc.printf("Controller actived\r\n");
             // reset controller here!!!
-            // ???
+            dt1.reset(0.0);
         } else
             pc.printf("Controller disabled\r\n");
     }