ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

Drivers/PiController.cpp@7:73fd05fe556a, 2018-02-15 (annotated)

Committer:
asobhy
Date:
Thu Feb 15 01:15:26 2018 +0000
Revision:
7:73fd05fe556a
Child:
8:a0890fa79084
Added comments to several files and organized the code a bit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asobhy 7:73fd05fe556a 1 #include "mbed.h"
asobhy 7:73fd05fe556a 2 #include "PiController.h"
asobhy 7:73fd05fe556a 3
asobhy 7:73fd05fe556a 4 // global speed variable;
asobhy 7:73fd05fe556a 5 extern Mutex setpoint_mutex;
asobhy 7:73fd05fe556a 6 float Ki;
asobhy 7:73fd05fe556a 7 float Kp;
asobhy 7:73fd05fe556a 8 int32_t e, u, xState, scale;
asobhy 7:73fd05fe556a 9
asobhy 7:73fd05fe556a 10 /******************************************************************************/
asobhy 7:73fd05fe556a 11 int SaturatingSubtract(int x, int y)
asobhy 7:73fd05fe556a 12 {
asobhy 7:73fd05fe556a 13 int z;
asobhy 7:73fd05fe556a 14 z = x - y; // 32-bit overflow detection and saturating arithmetic
asobhy 7:73fd05fe556a 15 if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
asobhy 7:73fd05fe556a 16 else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
asobhy 7:73fd05fe556a 17 return z;
asobhy 7:73fd05fe556a 18 }
asobhy 7:73fd05fe556a 19
asobhy 7:73fd05fe556a 20 /******************************************************************************/
asobhy 7:73fd05fe556a 21 int SaturatingAdd(int x, int y)
asobhy 7:73fd05fe556a 22 {
asobhy 7:73fd05fe556a 23 int z;
asobhy 7:73fd05fe556a 24 z = x + y; // 32-bit overflow detection and saturating arithmetic
asobhy 7:73fd05fe556a 25 if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
asobhy 7:73fd05fe556a 26 else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
asobhy 7:73fd05fe556a 27 return z;
asobhy 7:73fd05fe556a 28 }
asobhy 7:73fd05fe556a 29
asobhy 7:73fd05fe556a 30 /******************************************************************************/
asobhy 7:73fd05fe556a 31 int SaturateValue(int x, int Limit)
asobhy 7:73fd05fe556a 32 {
asobhy 7:73fd05fe556a 33 if(x > Limit) return(Limit); // Impose maximum limit on x
asobhy 7:73fd05fe556a 34 else if(x < -Limit) return(-Limit);
asobhy 7:73fd05fe556a 35 else return(x);
asobhy 7:73fd05fe556a 36 }
asobhy 7:73fd05fe556a 37
asobhy 7:73fd05fe556a 38
asobhy 7:73fd05fe556a 39 /*****************************************************************************/
asobhy 7:73fd05fe556a 40 void PiController_init(float Kp_given, float Ki_given)
asobhy 7:73fd05fe556a 41 {
asobhy 7:73fd05fe556a 42 Kp = Kp_given;
asobhy 7:73fd05fe556a 43 Ki = Ki_given;
asobhy 7:73fd05fe556a 44
asobhy 7:73fd05fe556a 45 // initialization
asobhy 7:73fd05fe556a 46 scale = 40;
asobhy 7:73fd05fe556a 47 xState = 0;
asobhy 7:73fd05fe556a 48
asobhy 7:73fd05fe556a 49 }
asobhy 7:73fd05fe556a 50
asobhy 7:73fd05fe556a 51
asobhy 7:73fd05fe556a 52 /*******************************************************************************
asobhy 7:73fd05fe556a 53 * @brief PI Controller function
asobhy 7:73fd05fe556a 54 * @param setp is the setpoint we would like the system to get to
asobhy 7:73fd05fe556a 55 * @param dP is the current speed of the system
asobhy 7:73fd05fe556a 56 * @return u is the control signal out of the controller to make the system
asobhy 7:73fd05fe556a 57 * reach the desired setpoint
asobhy 7:73fd05fe556a 58 *******************************************************************************/
asobhy 7:73fd05fe556a 59 uint32_t PiController(int setp, int dP)
asobhy 7:73fd05fe556a 60 {
asobhy 7:73fd05fe556a 61
asobhy 7:73fd05fe556a 62 int32_t xTemp;
asobhy 7:73fd05fe556a 63 int32_t uProportional;
asobhy 7:73fd05fe556a 64 int32_t uIntegral;
asobhy 7:73fd05fe556a 65 int32_t uS;
asobhy 7:73fd05fe556a 66
asobhy 7:73fd05fe556a 67 setpoint_mutex.lock();
asobhy 7:73fd05fe556a 68 e = SaturatingSubtract(setp, dP); // e is the velocity error
asobhy 7:73fd05fe556a 69 setpoint_mutex.unlock();
asobhy 7:73fd05fe556a 70
asobhy 7:73fd05fe556a 71 xTemp = SaturatingAdd(xState, e);
asobhy 7:73fd05fe556a 72
asobhy 7:73fd05fe556a 73 // the maximum value that 'u' can get to is 20
asobhy 7:73fd05fe556a 74 // the maximum value that dPosition can get to 560
asobhy 7:73fd05fe556a 75 // scaling factor is 560/20 = 28
asobhy 7:73fd05fe556a 76 // scaling factor used is 40
asobhy 7:73fd05fe556a 77
asobhy 7:73fd05fe556a 78 uProportional = (float)(Kp*e/scale);
asobhy 7:73fd05fe556a 79 uIntegral = (float)(Ki*xState/scale);
asobhy 7:73fd05fe556a 80
asobhy 7:73fd05fe556a 81 uS = SaturatingAdd(uProportional, uIntegral);
asobhy 7:73fd05fe556a 82
asobhy 7:73fd05fe556a 83 u = SaturateValue(uS, U_LIMIT);
asobhy 7:73fd05fe556a 84 if(u==uS) xState=xTemp; // if limit has not been reached then update xState
asobhy 7:73fd05fe556a 85
asobhy 7:73fd05fe556a 86 return u;
asobhy 7:73fd05fe556a 87
asobhy 7:73fd05fe556a 88 }