Diff: PiControlThread.cpp

Revision:

1:3e9684e81312

Parent:

0:a355e511bc5d

Child:

2:ca2a7430739b

--- a/PiControlThread.cpp	Thu Feb 01 03:58:00 2018 +0000
+++ b/PiControlThread.cpp	Sat Feb 03 00:05:08 2018 +0000
@@ -2,19 +2,30 @@
 #include "ui.h"
 #include "Drivers/motor_driver.h"
 #include "Drivers/DE0_driver.h"
+#include "PiControlThread.h"
 
 // global speed variable;
 extern int setpoint;
 extern Serial pc;
 extern Mutex setpoint_mutex;
 
-uint16_t ID, dPosition, dTime;
-int e, u, xState;
+uint16_t ID, dTime;
+int dPosition;
+int vel;
 
-float Ki, Kp;
+int32_t e, u, xState;
+
+bool saturationFlag;
+
+float Ki; 
+float Kp;
 
 void PiControlThread(void const *);
 void PeriodicInterruptISR(void);
+int SaturatingSubtract(int , int );
+int SaturatingAdd(int , int );
+int SaturateValue(int , int );
+
 
 osThreadId PiControlId;
 
@@ -45,7 +56,11 @@
 
     // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
     // in seconds between interrupts, and start interrupt generation:
-    PeriodicInt.attach(&PeriodicInterruptISR, 0.05); 
+    PeriodicInt.attach(&PeriodicInterruptISR, 0.05);   // 50ms sampling rate
+    
+    Kp = 2;
+    Ki = 0.01;
+    
 }
 
 
@@ -53,23 +68,48 @@
 *               ******** Periodic Timer Interrupt Thread ********
 *******************************************************************************/
 void PiControlThread(void const *argument)
-{  
+{
+    // initialization
+    saturationFlag = false;
+    int scale = 40;
+      
     while (true) 
     {
-        
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
-        led3= !led3; // Alive status - led3 toggles each time PieriodicZInterruptsThread is signaled.
         
         // get incremental position and time from QEI
         DE0_read(&ID, &dPosition, &dTime);
+        SaturateValue(dPosition, 560);
+        
+        // maximum velocity at dPostition = 560 is vel = 703
+        vel = (float)((6135.92 * dPosition) / dTime) ;
         
         setpoint_mutex.lock();
-        e = setpoint-dPosition;  // e is the velocity error
+        e = SaturatingSubtract(setpoint, vel);  // e is the velocity error
         setpoint_mutex.unlock();
         
-        xState = xState + e; // x is the Euler approximation to the integral of e.
-        u = Kp*e + Ki*xState; // u is the control signal
-              
+        // 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.    
+        }
+        
+        // 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      
+        
+        // if u is saturated set the flag to true
+        if( (u>= U_LIMIT) || (u<= -U_LIMIT) )  saturationFlag = true;
+        else saturationFlag = false;
+        
+        // saturate u at values greater than 20 and smaller than -20
+        u = SaturateValue(u, U_LIMIT);
         
         if (u >= 0)
         {
@@ -78,12 +118,10 @@
         else if (u < 0)
         {
             motorDriver_reverse(u);
-        }
-        else
-        {
-            pc.printf("\r\nerror!!!");
-        }
+        }   
+        
     }
+    
 }
 
 /*******************************************************************************
@@ -96,3 +134,32 @@
     // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
     osSignalSet(PiControlId,0x1); 
 }
+
+
+/*****************************************************************************/
+int SaturatingSubtract(int x, int y)
+{
+    int z;
+    z = x - y; // 32-bit overflow detection and saturating arithmetic
+    if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
+    else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
+    return z;
+}
+
+/*****************************************************************************/
+int SaturatingAdd(int x, int y)
+{
+    int z;
+    z = x + y; // 32-bit overflow detection and saturating arithmetic
+    if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
+    else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
+    return z;
+}
+
+/*****************************************************************************/
+int SaturateValue(int x, int Limit)
+{
+    if(x > Limit) return(Limit); // Impose maximum limit on x
+    else if(x < -Limit) return(-Limit);
+    else return(x);
+}