PiController.cpp

00001 /******************************************************************************/
00002 // ECE4333
00003 // LAB Partner 1:   Ahmed Sobhy - ID: 3594449
00004 // LAB Partner 2:   Brandon Kingman - ID: 3470444
00005 // Project:         Autonomous Robot Design
00006 // Instructor:      Prof. Chris Diduch
00007 /******************************************************************************/
00008 // filename: PiController.cpp
00009 // file content description:
00010 //      * PiController initialization function
00011 //      * Picontroller function
00012 //      * Value Saturation functions
00013 /******************************************************************************/
00014 
00015 #include "mbed.h"
00016 #include "PiController.h"
00017 #include "ui.h"
00018 
00019 // global speed variable;
00020 float Ki;
00021 float Kp;
00022 int32_t scale;
00023 
00024 /******************************************************************************/
00025 int SaturatingSubtract(int x, int y)
00026 {
00027     int z;
00028     z = x - y; // 32-bit overflow detection and saturating arithmetic
00029     if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
00030     else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
00031     return z;
00032 }
00033 
00034 /******************************************************************************/
00035 int SaturatingAdd(int x, int y)
00036 {
00037     int z;
00038     z = x + y; // 32-bit overflow detection and saturating arithmetic
00039     if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
00040     else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
00041     return z;
00042 }
00043 
00044 /******************************************************************************/
00045 int SaturateValue(int x, int Limit)
00046 {
00047     if(x > Limit) return(Limit); // Impose maximum limit on x
00048     else if(x < -Limit) return(-Limit);
00049     else return(x);
00050 }
00051 
00052 
00053 /*****************************************************************************/
00054 void PiController_init(float Kp_given, float Ki_given)
00055 {
00056     Kp = Kp_given;
00057     Ki = Ki_given;
00058     
00059     // initialization
00060     scale = 40; 
00061         
00062 }
00063 
00064 
00065 /*******************************************************************************
00066 * @brief    PI Controller function
00067 * @param    setp is the setpoint we would like the system to get to
00068 * @param    dP is the current speed of the system 
00069 * @return   u is the control signal out of the controller to make the system 
00070 *           reach the desired setpoint
00071 *******************************************************************************/
00072 uint32_t PiControllerR(int setp, int dP)
00073 {
00074     static int32_t e, u; 
00075     static int32_t xState = 0;
00076     int32_t xTemp;
00077     int32_t uProportional;
00078     int32_t uIntegral;
00079     int32_t uS;
00080     
00081     e = SaturatingSubtract(setp, dP);  // e is the velocity error
00082 
00083     xTemp = SaturatingAdd(xState, e);
00084 
00085     // the maximum value that 'u' can get to is 20
00086     // the maximum value that dPosition can get to 560
00087     // scaling factor is 560/20 = 28
00088     // scaling factor used is 40
00089     
00090     uProportional = (float)(Kp*e/scale);
00091     uIntegral = (float)(Ki*xState/scale);
00092 
00093     uS = SaturatingAdd(uProportional, uIntegral);
00094 
00095     u = SaturateValue(uS, U_LIMIT);
00096     if(u==uS) xState=xTemp; // if limit has not been reached then update xState
00097     
00098     return u;
00099     
00100 }
00101 
00102 
00103 /*******************************************************************************
00104 * @brief    PI Controller function
00105 * @param    setp is the setpoint we would like the system to get to
00106 * @param    dP is the current speed of the system 
00107 * @return   u is the control signal out of the controller to make the system 
00108 *           reach the desired setpoint
00109 *******************************************************************************/
00110 uint32_t PiControllerL(int setp, int dP)
00111 {
00112     static int32_t e, u;
00113     static int32_t xState = 0;
00114     int32_t xTemp;
00115     int32_t uProportional;
00116     int32_t uIntegral;
00117     int32_t uS;
00118     
00119     e = SaturatingSubtract(setp, dP);  // e is the velocity error
00120 
00121     xTemp = SaturatingAdd(xState, e);
00122 
00123     // the maximum value that 'u' can get to is 20
00124     // the maximum value that dPosition can get to 560
00125     // scaling factor is 560/20 = 28
00126     // scaling factor used is 40
00127     
00128     uProportional = (float)(Kp*e/scale);
00129     uIntegral = (float)(Ki*xState/scale);
00130 
00131     uS = SaturatingAdd(uProportional, uIntegral);
00132 
00133     u = SaturateValue(uS, U_LIMIT);
00134     if(u==uS) xState=xTemp; // if limit has not been reached then update xState
00135     
00136     return u;
00137     
00138 }