ObjectFollower
Dependencies: TPixy-Interface
Fork of PlayBack by
Diff: Drivers/PiController.cpp
- Revision:
- 9:fe56b888985c
- Parent:
- 8:a0890fa79084
- Child:
- 15:cf67f83d5409
diff -r a0890fa79084 -r fe56b888985c Drivers/PiController.cpp --- 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; + +}