PID Library Brought in from the PID Arduino Library
Dependents: Basic_PID wheelchaircontrol wheelchaircontrolRosCom wheelchaircontrol ... more
PID.cpp
00001 #include "PID.h" 00002 #include "mbed.h" 00003 00004 PID::PID(volatile double* Input, volatile double* Output, volatile double* Setpoint, double Kp, double Ki, double Kd, int POn, int ControllerDirection){ 00005 PIDtimer.start(); 00006 PIDtimer.reset(); 00007 myOutput = Output; 00008 myInput = Input; 00009 mySetpoint = Setpoint; 00010 inAuto = true; 00011 PID::SetOutputLimits(0, 1); //default output limit corresponds to 00012 //the arduino pwm limits 00013 SampleTime = .1; //default Controller Sample Time is 0.1 seconds 00014 00015 PID::SetControllerDirection(ControllerDirection); 00016 PID::SetTunings(Kp, Ki, Kd, POn); 00017 00018 lastTime = PIDtimer.read()-SampleTime; 00019 // printf("the values are %f, %f, %f, %f, %f, %f \r\n", *myOutput, *myInput, *mySetpoint, kp, ki, kd); 00020 } 00021 00022 /* Compute() ********************************************************************** 00023 * This, as they say, is where the magic happens. this function should be called 00024 * every time "void loop()" executes. the function will decide for itself whether a new 00025 * pid Output needs to be computed. returns true when the output is computed, 00026 * false when nothing has been done. 00027 **********************************************************************************/ 00028 bool PID::Compute() 00029 { 00030 if(!inAuto) return false; 00031 double now = PIDtimer.read(); 00032 double timeChange = (now - lastTime); 00033 if(timeChange>=SampleTime) 00034 { 00035 /*Compute all the working error variables*/ 00036 double input = *myInput; 00037 double error = *mySetpoint - input; 00038 double dInput = (input - lastInput); 00039 outputSum+= (ki * error); 00040 00041 /*Add Proportional on Measurement, if P_ON_M is specified*/ 00042 if(!pOnE) outputSum -= kp * dInput; 00043 if(outputSum > outMax) outputSum= outMax; 00044 else if(outputSum < outMin) outputSum= outMin; 00045 00046 /*Add Proportional on Error, if P_ON_E is specified*/ 00047 double output; 00048 if(pOnE) output = kp * error; 00049 else output = 0; 00050 /*Compute Rest of PID Output*/ 00051 output += outputSum - kd * dInput; 00052 00053 if(output > outMax) output = outMax; 00054 else if(output < outMin) output = outMin; 00055 *myOutput = output; 00056 00057 /*Remember some variables for next time*/ 00058 lastInput = input; 00059 lastTime = now; 00060 //printf("the values are %f, %f, %f, %f \r\n", *myOutput, *myInput, *mySetpoint, lastTime); 00061 00062 return true; 00063 } 00064 else 00065 { 00066 return false; 00067 } 00068 } 00069 00070 /* SetTunings(...)************************************************************* 00071 * This function allows the controller's dynamic performance to be adjusted. 00072 * it's called automatically from the constructor, but tunings can also 00073 * be adjusted on the fly during normal operation 00074 ******************************************************************************/ 00075 void PID::SetTunings(double Kp, double Ki, double Kd, int POn) 00076 { 00077 if (Kp<0 || Ki<0 || Kd<0) return; 00078 00079 pOn = POn; 00080 pOnE = POn == P_ON_E; 00081 00082 dispKp = Kp; dispKi = Ki; dispKd = Kd; 00083 00084 double SampleTimeInSec = ((double)SampleTime)/1000; 00085 kp = Kp; 00086 ki = Ki * SampleTimeInSec; 00087 kd = Kd / SampleTimeInSec; 00088 00089 if(controllerDirection ==REVERSE) 00090 { 00091 kp = (0 - kp); 00092 ki = (0 - ki); 00093 kd = (0 - kd); 00094 } 00095 } 00096 00097 /* SetTunings(...)************************************************************* 00098 * Set Tunings using the last-rembered POn setting 00099 ******************************************************************************/ 00100 void PID::SetTunings(double Kp, double Ki, double Kd){ 00101 SetTunings(Kp, Ki, Kd, pOn); 00102 } 00103 00104 /* SetSampleTime(...) ********************************************************* 00105 * sets the period, in Milliseconds, at which the calculation is performed 00106 ******************************************************************************/ 00107 void PID::SetSampleTime(int NewSampleTime) 00108 { 00109 if (NewSampleTime > 0) 00110 { 00111 double ratio = (double)NewSampleTime 00112 / (double)SampleTime; 00113 ki *= ratio; 00114 kd /= ratio; 00115 SampleTime = (unsigned long)NewSampleTime; 00116 } 00117 } 00118 00119 /* SetOutputLimits(...)**************************************************** 00120 * This function will be used far more often than SetInputLimits. while 00121 * the input to the controller will generally be in the 0-1023 range (which is 00122 * the default already,) the output will be a little different. maybe they'll 00123 * be doing a time window and will need 0-8000 or something. or maybe they'll 00124 * want to clamp it from 0-125. who knows. at any rate, that can all be done 00125 * here. 00126 **************************************************************************/ 00127 void PID::SetOutputLimits(double Min, double Max) 00128 { 00129 if(Min >= Max) return; 00130 outMin = Min; 00131 outMax = Max; 00132 00133 if(inAuto) 00134 { 00135 if(*myOutput > outMax) *myOutput = outMax; 00136 else if(*myOutput < outMin) *myOutput = outMin; 00137 00138 if(outputSum > outMax) outputSum= outMax; 00139 else if(outputSum < outMin) outputSum= outMin; 00140 } 00141 } 00142 00143 /* SetMode(...)**************************************************************** 00144 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) 00145 * when the transition from manual to auto occurs, the controller is 00146 * automatically initialized 00147 ******************************************************************************/ 00148 void PID::SetMode(int Mode) 00149 { 00150 bool newAuto = (Mode == AUTOMATIC); 00151 if(newAuto && !inAuto) 00152 { /*we just went from manual to auto*/ 00153 PID::Initialize(); 00154 } 00155 inAuto = newAuto; 00156 } 00157 00158 /* Initialize()**************************************************************** 00159 * does all the things that need to happen to ensure a bumpless transfer 00160 * from manual to automatic mode. 00161 ******************************************************************************/ 00162 void PID::Initialize() 00163 { 00164 outputSum = *myOutput; 00165 lastInput = *myInput; 00166 if(outputSum > outMax) outputSum = outMax; 00167 else if(outputSum < outMin) outputSum = outMin; 00168 } 00169 00170 /* SetControllerDirection(...)************************************************* 00171 * The PID will either be connected to a DIRECT acting process (+Output leads 00172 * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to 00173 * know which one, because otherwise we may increase the output when we should 00174 * be decreasing. This is called from the constructor. 00175 ******************************************************************************/ 00176 void PID::SetControllerDirection(int Direction) 00177 { 00178 if(inAuto && Direction !=controllerDirection) 00179 { 00180 kp = (0 - kp); 00181 ki = (0 - ki); 00182 kd = (0 - kd); 00183 } 00184 controllerDirection = Direction; 00185 } 00186 00187 /* Status Funcions************************************************************* 00188 * Just because you set the Kp=-1 doesn't mean it actually happened. these 00189 * functions query the internal state of the PID. they're here for display 00190 * purposes. this are the functions the PID Front-end uses for example 00191 ******************************************************************************/ 00192 double PID::GetKp(){ return dispKp; } 00193 double PID::GetKi(){ return dispKi;} 00194 double PID::GetKd(){ return dispKd;} 00195 int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} 00196 int PID::GetDirection(){ return controllerDirection;}
Generated on Tue Jul 12 2022 21:32:38 by 1.7.2