Diff: PiControlThread.cpp

Revision:

4:417e475239c7

Parent:

3:4def4ca68910

Child:

5:b29220d29022

Child:

6:e7ce340fe91e

--- a/PiControlThread.cpp	Sat Feb 03 00:48:47 2018 +0000
+++ b/PiControlThread.cpp	Fri Feb 09 18:37:11 2018 +0000
@@ -3,29 +3,17 @@
 #include "Drivers/motor_driver.h"
 #include "Drivers/DE0_driver.h"
 #include "PiControlThread.h"
+#include "PiController.h"
 
-// global speed variable;
 extern int setpoint;
-extern Serial pc;
-extern Mutex setpoint_mutex;
 
 uint16_t ID, dTime;
 int dPosition;
 int vel;
-
-int32_t e, u, xState;
-
-bool saturationFlag;
-
-float Ki; 
-float Kp;
+int32_t U;
 
 void PiControlThread(void const *);
 void PeriodicInterruptISR(void);
-int SaturatingSubtract(int , int );
-int SaturatingAdd(int , int );
-int SaturateValue(int , int );
-
 
 osThreadId PiControlId;
 
@@ -49,18 +37,16 @@
 
 void PiControlThreadInit()
 {
-    DE0_init();             // initialize FPGA
-    motorDriver_init();     // initialize motorDriver
+    DE0_init();                  // initialize FPGA
+    motorDriver_init();          // initialize motorDriver
+    PiController_init(2.2,0.01); // initialize the PI Controller gains and initialize variables
     
     PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
 
     // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
     // in seconds between interrupts, and start interrupt generation:
     PeriodicInt.attach(&PeriodicInterruptISR, 0.05);   // 50ms sampling rate
-    
-    Kp = 2.2;
-    Ki = 0.01;
-    
+     
 }
 
 
@@ -70,16 +56,6 @@
 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) 
     {
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
@@ -91,30 +67,15 @@
         // maximum velocity at dPostition = 560 is vel = 703
         vel = (float)((6135.92 * dPosition) / dTime) ;
         
-        setpoint_mutex.lock();
-        e = SaturatingSubtract(setpoint, dPosition);  // e is the velocity error
-        setpoint_mutex.unlock();
-            
-        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
-        uProportional = (float)(Kp*e/scale);
-        uIntegral = (float)(Ki*xState/scale);
+        U = PiController(setpoint,dPosition);
         
-        uS = SaturatingAdd(uProportional, uIntegral);
-        
-        u = SaturateValue(uS, U_LIMIT);      
-        if(u==uS) xState=xTemp;
-        
-        if (u >= 0)
+        if (U >= 0)
         {
-            motorDriver_forward(u);
+            motorDriver_forward(U);
         }
-        else if (u < 0)
+        else if (U < 0)
         {
-            motorDriver_reverse(u);
+            motorDriver_reverse(U);
         }   
            
     }
@@ -132,31 +93,3 @@
     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);
-}