Diff: Drivers/PiController.cpp

Revision:

7:73fd05fe556a

Child:

8:a0890fa79084

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Drivers/PiController.cpp	Thu Feb 15 01:15:26 2018 +0000
@@ -0,0 +1,88 @@
+#include "mbed.h"
+#include "PiController.h"
+
+// global speed variable;
+extern Mutex setpoint_mutex;
+float Ki;
+float Kp;
+int32_t e, u, xState, scale;
+
+/******************************************************************************/
+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);
+}
+
+
+/*****************************************************************************/
+void PiController_init(float Kp_given, float Ki_given)
+{
+    Kp = Kp_given;
+    Ki = Ki_given;
+    
+    // initialization
+    scale = 40;
+    xState = 0;  
+        
+}
+
+
+/*******************************************************************************
+* @brief    PI Controller function
+* @param    setp is the setpoint we would like the system to get to
+* @param    dP is the current speed of the system 
+* @return   u is the control signal out of the controller to make the system 
+*           reach the desired setpoint
+*******************************************************************************/
+uint32_t PiController(int setp, int dP)
+{
+    
+    int32_t xTemp;
+    int32_t uProportional;
+    int32_t uIntegral;
+    int32_t uS;
+    
+    setpoint_mutex.lock();
+    e = SaturatingSubtract(setp, dP);  // 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
+    // scaling factor used is 40
+    
+    uProportional = (float)(Kp*e/scale);
+    uIntegral = (float)(Ki*xState/scale);
+
+    uS = SaturatingAdd(uProportional, uIntegral);
+
+    u = SaturateValue(uS, U_LIMIT);
+    if(u==uS) xState=xTemp; // if limit has not been reached then update xState
+    
+    return u;
+    
+}