Diff: Drivers/PiController.cpp

Revision:

9:fe56b888985c

Parent:

8:a0890fa79084

Child:

15:cf67f83d5409

--- a/Drivers/PiController.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ b/Drivers/PiController.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 // filename: PiController.cpp
 // file content description:
@@ -16,10 +16,11 @@
 #include "PiController.h"
 
 // global speed variable;
-extern Mutex setpoint_mutex;
+extern Mutex setpointR_mutex;
+extern Mutex setpointL_mutex;
 float Ki;
 float Kp;
-int32_t e, u, xState, scale;
+int32_t scale;
 
 /******************************************************************************/
 int SaturatingSubtract(int x, int y)
@@ -58,7 +59,7 @@
     
     // initialization
     scale = 40;
-    xState = 0;  
+    //xState = 0;  
         
 }
 
@@ -70,17 +71,20 @@
 * @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)
+uint32_t PiControllerR(int setp, int dP)
 {
-    
+    static int32_t e, u; 
+    static int32_t xState = 0;
     int32_t xTemp;
     int32_t uProportional;
     int32_t uIntegral;
     int32_t uS;
     
-    setpoint_mutex.lock();
+    setpointR_mutex.lock();
+    setpointL_mutex.lock();
     e = SaturatingSubtract(setp, dP);  // e is the velocity error
-    setpoint_mutex.unlock();
+    setpointL_mutex.unlock();
+    setpointR_mutex.unlock();
 
     xTemp = SaturatingAdd(xState, e);
 
@@ -100,3 +104,45 @@
     return u;
     
 }
+
+
+/*******************************************************************************
+* @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 PiControllerL(int setp, int dP)
+{
+    static int32_t e, u;
+    static int32_t xState = 0;
+    int32_t xTemp;
+    int32_t uProportional;
+    int32_t uIntegral;
+    int32_t uS;
+    
+    setpointR_mutex.lock();
+    setpointL_mutex.lock();
+    e = SaturatingSubtract(setp, dP);  // e is the velocity error
+    setpointL_mutex.unlock();
+    setpointR_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;
+    
+}