Revision 2:91678e836872, committed 2019-05-24

Comitter:

altb2

Date:

Fri May 24 10:06:51 2019 +0000

Parent:

1:92f175969d90

Commit message:

Final G3

Changed in this revision

diff -r 92f175969d90 -r 91678e836872 PID_Cntrl.cpp
--- a/PID_Cntrl.cpp	Fri May 10 14:25:24 2019 +0000
+++ b/PID_Cntrl.cpp	Fri May 24 10:06:51 2019 +0000
@@ -15,10 +15,17 @@
 #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 Min, float Max)
 {
+    
     // link member variables
-    // ???
+    this->Kp = Kp;
+    this->Ki = Ki;
+    this->Kd = Kd;
+    this->Tf = Tf;
+    this->Ts = Ts;
+    this->Min = Min;
+    this->Max = Max;
     
     reset(0.0f);
 }
@@ -29,33 +36,39 @@
 {
 
     // implement controller reset
-    // ???
-
+    yI = initValue;
+    yD = 0.0;
+    e_old = 0.0;
 }
-
+//-------------------------------------------------
 float PID_Cntrl::update(double e)
 {
 
     // controller update function 
     
     // calculate uI
-    // ???
+    yI += Ki * Ts * e;
     
     // saturate uI, uMin <= uI <= uMax (anti-windup for the integrator part)
-    // ???
+    if(yI > Max) 
+        yI = Max;
+    else if(yI < Min)
+        yI = Min;
 
     // calculate uD
-    // ???
+    yD = (1 - Ts/Tf)*yD + Kd/Tf * (e - e_old);
     
-    // calculate u
-    // ???
-    
+    float y = Kp * e + yI + yD;
+        
     // saturate u, uMin <= u <= uMax
-    // ???
+    if(y > Max) 
+        y = Max;
+    else if(y < Min)
+        y = Min;
     
     // update signal storage
-    // ???
-
-    return 0.0f;
+    e_old = e;
+    
+    return y;
 }
 

diff -r 92f175969d90 -r 91678e836872 PID_Cntrl.h
--- a/PID_Cntrl.h	Fri May 10 14:25:24 2019 +0000
+++ b/PID_Cntrl.h	Fri May 24 10:06:51 2019 +0000
@@ -6,7 +6,7 @@
 {
 public:
 
-    PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float uMin, float uMax);
+    PID_Cntrl(float Kp, float Ki, float Kd, float Tf, float Ts, float Min, float Max);
 
     float operator()(float error) {
         return update((double)error);
@@ -20,10 +20,9 @@
 private:
 
     // controller parameters (member variables)
-    // ???
-        
+    float Kp, Ki, Kd, Tf, Ts, Min, Max;
     // storage for signals (member variables)
-    // ???
+    float yI, yD, e_old;
 };
 
 #endif

diff -r 92f175969d90 -r 91678e836872 main.cpp
--- a/main.cpp	Fri May 10 14:25:24 2019 +0000
+++ b/main.cpp	Fri May 24 10:06:51 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 = 0.50;
 // initiate PIDT1 controller objects
-// ???
-// ???
+PID_Cntrl dt1(0.0,0.0,0.0116,0.000625,Ts,-4.0,4.0);
+PID_Cntrl pi1(3.59,13.5,0.0,1.0,Ts,-4.0,4.0);
+
 //******************************************************************************
 //---------- main loop -------------
 //******************************************************************************
@@ -61,12 +63,13 @@
     float i_des = 0.0f;         // default: set motor current to zero (will be overwritten)
     if(controller_active) {
         // controller update
-        // ???
+        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,8 @@
         if(controller_active) {
             pc.printf("Controller actived\r\n");
             // reset controller here!!!
-            // ???
+            dt1.reset(0.0);
+            pi1.reset(0.0);
         } else
             pc.printf("Controller disabled\r\n");
     }