Diff: PiControlThread.cpp

Revision:

3:4def4ca68910

Parent:

2:ca2a7430739b

Child:

4:417e475239c7

--- a/PiControlThread.cpp	Sat Feb 03 00:37:57 2018 +0000
+++ b/PiControlThread.cpp	Sat Feb 03 00:48:47 2018 +0000
@@ -69,9 +69,16 @@
 *******************************************************************************/
 void PiControlThread(void const *argument)
 {
+    
     // initialization
     saturationFlag = false;
     int scale = 40;
+    xState = 0;  
+    
+    int32_t xTemp;
+    int32_t uProportional;
+    int32_t uIntegral;
+    int32_t uS;
       
     while (true) 
     {
@@ -87,29 +94,19 @@
         setpoint_mutex.lock();
         e = SaturatingSubtract(setpoint, dPosition);  // e is the velocity error
         setpoint_mutex.unlock();
-        
-        // disable integration if u is saturated
-        if(saturationFlag)
-        {
-            xState = xState;
-        }
-        else
-        {
-            xState = SaturatingAdd(xState, e); // x is the Euler approximation to the integral of e.    
-        }
+            
+        xTemp = SaturatingAdd(xState, e);
         
         // the maximum value that 'u' can get to is 20
         // the maximum value that dPosition can get to 560
         // scaling factor is 560/20 = 28
-        
-        u = (float)(Kp*e/scale) + (float)(Ki*xState/scale); // u is the control signal      
+        uProportional = (float)(Kp*e/scale);
+        uIntegral = (float)(Ki*xState/scale);
         
-        // if u is saturated set the flag to true
-        if( (u>= U_LIMIT) || (u<= -U_LIMIT) )  saturationFlag = true;
-        else saturationFlag = false;
+        uS = SaturatingAdd(uProportional, uIntegral);
         
-        // saturate u at values greater than 20 and smaller than -20
-        u = SaturateValue(u, U_LIMIT);
+        u = SaturateValue(uS, U_LIMIT);      
+        if(u==uS) xState=xTemp;
         
         if (u >= 0)
         {
@@ -119,7 +116,7 @@
         {
             motorDriver_reverse(u);
         }   
-        
+           
     }
     
 }