Jesus Fausto / PID

PID Library Brought in from the PID Arduino Library

Dependents:   Basic_PID wheelchaircontrol wheelchaircontrolRosCom wheelchaircontrol ... more

PID.cpp@4:1b6ecfeb7ba9, 2019-04-19 (annotated)

Committer:
jvfausto
Date:
Fri Apr 19 23:03:35 2019 +0000
Revision:
4:1b6ecfeb7ba9
Parent:
2:60801ab3cbf9 
Hello

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jvfausto 0:58f3a6c65ad5 1 #include "PID.h"
jvfausto 0:58f3a6c65ad5 2 #include "mbed.h"
jvfausto 0:58f3a6c65ad5 3
jvfausto 2:60801ab3cbf9 4 PID::PID(volatile double* Input, volatile double* Output, volatile double* Setpoint, double Kp, double Ki, double Kd, int POn, int ControllerDirection){
jvfausto 1:37c3ab46d475 5 PIDtimer.start();
jvfausto 1:37c3ab46d475 6 PIDtimer.reset();
jvfausto 0:58f3a6c65ad5 7 myOutput = Output;
jvfausto 0:58f3a6c65ad5 8 myInput = Input;
jvfausto 0:58f3a6c65ad5 9 mySetpoint = Setpoint;
jvfausto 1:37c3ab46d475 10 inAuto = true;
jvfausto 0:58f3a6c65ad5 11 PID::SetOutputLimits(0, 1); //default output limit corresponds to
jvfausto 0:58f3a6c65ad5 12 //the arduino pwm limits
jvfausto 0:58f3a6c65ad5 13 SampleTime = .1; //default Controller Sample Time is 0.1 seconds
jvfausto 0:58f3a6c65ad5 14
jvfausto 0:58f3a6c65ad5 15 PID::SetControllerDirection(ControllerDirection);
jvfausto 0:58f3a6c65ad5 16 PID::SetTunings(Kp, Ki, Kd, POn);
jvfausto 0:58f3a6c65ad5 17
jvfausto 0:58f3a6c65ad5 18 lastTime = PIDtimer.read()-SampleTime;
jvfausto 1:37c3ab46d475 19 // printf("the values are %f, %f, %f, %f, %f, %f \r\n", *myOutput, *myInput, *mySetpoint, kp, ki, kd);
jvfausto 0:58f3a6c65ad5 20 }
jvfausto 0:58f3a6c65ad5 21
jvfausto 0:58f3a6c65ad5 22 /* Compute() **********************************************************************
jvfausto 0:58f3a6c65ad5 23 * This, as they say, is where the magic happens. this function should be called
jvfausto 0:58f3a6c65ad5 24 * every time "void loop()" executes. the function will decide for itself whether a new
jvfausto 0:58f3a6c65ad5 25 * pid Output needs to be computed. returns true when the output is computed,
jvfausto 0:58f3a6c65ad5 26 * false when nothing has been done.
jvfausto 0:58f3a6c65ad5 27 **********************************************************************************/
jvfausto 0:58f3a6c65ad5 28 bool PID::Compute()
jvfausto 0:58f3a6c65ad5 29 {
jvfausto 0:58f3a6c65ad5 30 if(!inAuto) return false;
jvfausto 0:58f3a6c65ad5 31 double now = PIDtimer.read();
jvfausto 0:58f3a6c65ad5 32 double timeChange = (now - lastTime);
jvfausto 0:58f3a6c65ad5 33 if(timeChange>=SampleTime)
jvfausto 0:58f3a6c65ad5 34 {
jvfausto 0:58f3a6c65ad5 35 /*Compute all the working error variables*/
jvfausto 0:58f3a6c65ad5 36 double input = *myInput;
jvfausto 0:58f3a6c65ad5 37 double error = *mySetpoint - input;
jvfausto 0:58f3a6c65ad5 38 double dInput = (input - lastInput);
jvfausto 0:58f3a6c65ad5 39 outputSum+= (ki * error);
jvfausto 1:37c3ab46d475 40
jvfausto 0:58f3a6c65ad5 41 /*Add Proportional on Measurement, if P_ON_M is specified*/
jvfausto 1:37c3ab46d475 42 if(!pOnE) outputSum -= kp * dInput;
jvfausto 0:58f3a6c65ad5 43 if(outputSum > outMax) outputSum= outMax;
jvfausto 0:58f3a6c65ad5 44 else if(outputSum < outMin) outputSum= outMin;
jvfausto 0:58f3a6c65ad5 45
jvfausto 0:58f3a6c65ad5 46 /*Add Proportional on Error, if P_ON_E is specified*/
jvfausto 0:58f3a6c65ad5 47 double output;
jvfausto 0:58f3a6c65ad5 48 if(pOnE) output = kp * error;
jvfausto 0:58f3a6c65ad5 49 else output = 0;
jvfausto 0:58f3a6c65ad5 50 /*Compute Rest of PID Output*/
jvfausto 0:58f3a6c65ad5 51 output += outputSum - kd * dInput;
jvfausto 0:58f3a6c65ad5 52
jvfausto 0:58f3a6c65ad5 53 if(output > outMax) output = outMax;
jvfausto 0:58f3a6c65ad5 54 else if(output < outMin) output = outMin;
jvfausto 0:58f3a6c65ad5 55 *myOutput = output;
jvfausto 0:58f3a6c65ad5 56
jvfausto 0:58f3a6c65ad5 57 /*Remember some variables for next time*/
jvfausto 0:58f3a6c65ad5 58 lastInput = input;
jvfausto 0:58f3a6c65ad5 59 lastTime = now;
jvfausto 4:1b6ecfeb7ba9 60 //printf("the values are %f, %f, %f, %f \r\n", *myOutput, *myInput, *mySetpoint, lastTime);
jvfausto 1:37c3ab46d475 61
jvfausto 0:58f3a6c65ad5 62 return true;
jvfausto 0:58f3a6c65ad5 63 }
jvfausto 1:37c3ab46d475 64 else
jvfausto 1:37c3ab46d475 65 {
jvfausto 1:37c3ab46d475 66 return false;
jvfausto 1:37c3ab46d475 67 }
jvfausto 0:58f3a6c65ad5 68 }
jvfausto 0:58f3a6c65ad5 69
jvfausto 0:58f3a6c65ad5 70 /* SetTunings(...)*************************************************************
jvfausto 0:58f3a6c65ad5 71 * This function allows the controller's dynamic performance to be adjusted.
jvfausto 0:58f3a6c65ad5 72 * it's called automatically from the constructor, but tunings can also
jvfausto 0:58f3a6c65ad5 73 * be adjusted on the fly during normal operation
jvfausto 0:58f3a6c65ad5 74 ******************************************************************************/
jvfausto 0:58f3a6c65ad5 75 void PID::SetTunings(double Kp, double Ki, double Kd, int POn)
jvfausto 0:58f3a6c65ad5 76 {
jvfausto 0:58f3a6c65ad5 77 if (Kp<0 || Ki<0 || Kd<0) return;
jvfausto 0:58f3a6c65ad5 78
jvfausto 0:58f3a6c65ad5 79 pOn = POn;
jvfausto 0:58f3a6c65ad5 80 pOnE = POn == P_ON_E;
jvfausto 0:58f3a6c65ad5 81
jvfausto 0:58f3a6c65ad5 82 dispKp = Kp; dispKi = Ki; dispKd = Kd;
jvfausto 0:58f3a6c65ad5 83
jvfausto 0:58f3a6c65ad5 84 double SampleTimeInSec = ((double)SampleTime)/1000;
jvfausto 0:58f3a6c65ad5 85 kp = Kp;
jvfausto 0:58f3a6c65ad5 86 ki = Ki * SampleTimeInSec;
jvfausto 0:58f3a6c65ad5 87 kd = Kd / SampleTimeInSec;
jvfausto 0:58f3a6c65ad5 88
jvfausto 0:58f3a6c65ad5 89 if(controllerDirection ==REVERSE)
jvfausto 0:58f3a6c65ad5 90 {
jvfausto 0:58f3a6c65ad5 91 kp = (0 - kp);
jvfausto 0:58f3a6c65ad5 92 ki = (0 - ki);
jvfausto 0:58f3a6c65ad5 93 kd = (0 - kd);
jvfausto 0:58f3a6c65ad5 94 }
jvfausto 0:58f3a6c65ad5 95 }
jvfausto 0:58f3a6c65ad5 96
jvfausto 0:58f3a6c65ad5 97 /* SetTunings(...)*************************************************************
jvfausto 0:58f3a6c65ad5 98 * Set Tunings using the last-rembered POn setting
jvfausto 0:58f3a6c65ad5 99 ******************************************************************************/
jvfausto 0:58f3a6c65ad5 100 void PID::SetTunings(double Kp, double Ki, double Kd){
jvfausto 0:58f3a6c65ad5 101 SetTunings(Kp, Ki, Kd, pOn);
jvfausto 0:58f3a6c65ad5 102 }
jvfausto 0:58f3a6c65ad5 103
jvfausto 0:58f3a6c65ad5 104 /* SetSampleTime(...) *********************************************************
jvfausto 0:58f3a6c65ad5 105 * sets the period, in Milliseconds, at which the calculation is performed
jvfausto 0:58f3a6c65ad5 106 ******************************************************************************/
jvfausto 0:58f3a6c65ad5 107 void PID::SetSampleTime(int NewSampleTime)
jvfausto 0:58f3a6c65ad5 108 {
jvfausto 0:58f3a6c65ad5 109 if (NewSampleTime > 0)
jvfausto 0:58f3a6c65ad5 110 {
jvfausto 0:58f3a6c65ad5 111 double ratio = (double)NewSampleTime
jvfausto 0:58f3a6c65ad5 112 / (double)SampleTime;
jvfausto 0:58f3a6c65ad5 113 ki *= ratio;
jvfausto 0:58f3a6c65ad5 114 kd /= ratio;
jvfausto 0:58f3a6c65ad5 115 SampleTime = (unsigned long)NewSampleTime;
jvfausto 0:58f3a6c65ad5 116 }
jvfausto 0:58f3a6c65ad5 117 }
jvfausto 0:58f3a6c65ad5 118
jvfausto 0:58f3a6c65ad5 119 /* SetOutputLimits(...)****************************************************
jvfausto 0:58f3a6c65ad5 120 * This function will be used far more often than SetInputLimits. while
jvfausto 0:58f3a6c65ad5 121 * the input to the controller will generally be in the 0-1023 range (which is
jvfausto 0:58f3a6c65ad5 122 * the default already,) the output will be a little different. maybe they'll
jvfausto 0:58f3a6c65ad5 123 * be doing a time window and will need 0-8000 or something. or maybe they'll
jvfausto 0:58f3a6c65ad5 124 * want to clamp it from 0-125. who knows. at any rate, that can all be done
jvfausto 0:58f3a6c65ad5 125 * here.
jvfausto 0:58f3a6c65ad5 126 **************************************************************************/
jvfausto 0:58f3a6c65ad5 127 void PID::SetOutputLimits(double Min, double Max)
jvfausto 0:58f3a6c65ad5 128 {
jvfausto 0:58f3a6c65ad5 129 if(Min >= Max) return;
jvfausto 0:58f3a6c65ad5 130 outMin = Min;
jvfausto 0:58f3a6c65ad5 131 outMax = Max;
jvfausto 0:58f3a6c65ad5 132
jvfausto 0:58f3a6c65ad5 133 if(inAuto)
jvfausto 0:58f3a6c65ad5 134 {
jvfausto 0:58f3a6c65ad5 135 if(*myOutput > outMax) *myOutput = outMax;
jvfausto 0:58f3a6c65ad5 136 else if(*myOutput < outMin) *myOutput = outMin;
jvfausto 0:58f3a6c65ad5 137
jvfausto 0:58f3a6c65ad5 138 if(outputSum > outMax) outputSum= outMax;
jvfausto 0:58f3a6c65ad5 139 else if(outputSum < outMin) outputSum= outMin;
jvfausto 0:58f3a6c65ad5 140 }
jvfausto 0:58f3a6c65ad5 141 }
jvfausto 0:58f3a6c65ad5 142
jvfausto 0:58f3a6c65ad5 143 /* SetMode(...)****************************************************************
jvfausto 0:58f3a6c65ad5 144 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
jvfausto 0:58f3a6c65ad5 145 * when the transition from manual to auto occurs, the controller is
jvfausto 0:58f3a6c65ad5 146 * automatically initialized
jvfausto 0:58f3a6c65ad5 147 ******************************************************************************/
jvfausto 0:58f3a6c65ad5 148 void PID::SetMode(int Mode)
jvfausto 0:58f3a6c65ad5 149 {
jvfausto 0:58f3a6c65ad5 150 bool newAuto = (Mode == AUTOMATIC);
jvfausto 0:58f3a6c65ad5 151 if(newAuto && !inAuto)
jvfausto 0:58f3a6c65ad5 152 { /*we just went from manual to auto*/
jvfausto 0:58f3a6c65ad5 153 PID::Initialize();
jvfausto 0:58f3a6c65ad5 154 }
jvfausto 0:58f3a6c65ad5 155 inAuto = newAuto;
jvfausto 0:58f3a6c65ad5 156 }
jvfausto 0:58f3a6c65ad5 157
jvfausto 0:58f3a6c65ad5 158 /* Initialize()****************************************************************
jvfausto 0:58f3a6c65ad5 159 * does all the things that need to happen to ensure a bumpless transfer
jvfausto 0:58f3a6c65ad5 160 * from manual to automatic mode.
jvfausto 0:58f3a6c65ad5 161 ******************************************************************************/
jvfausto 0:58f3a6c65ad5 162 void PID::Initialize()
jvfausto 0:58f3a6c65ad5 163 {
jvfausto 0:58f3a6c65ad5 164 outputSum = *myOutput;
jvfausto 0:58f3a6c65ad5 165 lastInput = *myInput;
jvfausto 0:58f3a6c65ad5 166 if(outputSum > outMax) outputSum = outMax;
jvfausto 0:58f3a6c65ad5 167 else if(outputSum < outMin) outputSum = outMin;
jvfausto 0:58f3a6c65ad5 168 }
jvfausto 0:58f3a6c65ad5 169
jvfausto 0:58f3a6c65ad5 170 /* SetControllerDirection(...)*************************************************
jvfausto 0:58f3a6c65ad5 171 * The PID will either be connected to a DIRECT acting process (+Output leads
jvfausto 0:58f3a6c65ad5 172 * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
jvfausto 0:58f3a6c65ad5 173 * know which one, because otherwise we may increase the output when we should
jvfausto 0:58f3a6c65ad5 174 * be decreasing. This is called from the constructor.
jvfausto 0:58f3a6c65ad5 175 ******************************************************************************/
jvfausto 0:58f3a6c65ad5 176 void PID::SetControllerDirection(int Direction)
jvfausto 0:58f3a6c65ad5 177 {
jvfausto 0:58f3a6c65ad5 178 if(inAuto && Direction !=controllerDirection)
jvfausto 0:58f3a6c65ad5 179 {
jvfausto 0:58f3a6c65ad5 180 kp = (0 - kp);
jvfausto 0:58f3a6c65ad5 181 ki = (0 - ki);
jvfausto 0:58f3a6c65ad5 182 kd = (0 - kd);
jvfausto 0:58f3a6c65ad5 183 }
jvfausto 0:58f3a6c65ad5 184 controllerDirection = Direction;
jvfausto 0:58f3a6c65ad5 185 }
jvfausto 0:58f3a6c65ad5 186
jvfausto 0:58f3a6c65ad5 187 /* Status Funcions*************************************************************
jvfausto 0:58f3a6c65ad5 188 * Just because you set the Kp=-1 doesn't mean it actually happened. these
jvfausto 0:58f3a6c65ad5 189 * functions query the internal state of the PID. they're here for display
jvfausto 0:58f3a6c65ad5 190 * purposes. this are the functions the PID Front-end uses for example
jvfausto 0:58f3a6c65ad5 191 ******************************************************************************/
jvfausto 0:58f3a6c65ad5 192 double PID::GetKp(){ return dispKp; }
jvfausto 0:58f3a6c65ad5 193 double PID::GetKi(){ return dispKi;}
jvfausto 0:58f3a6c65ad5 194 double PID::GetKd(){ return dispKd;}
jvfausto 0:58f3a6c65ad5 195 int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
jvfausto 0:58f3a6c65ad5 196 int PID::GetDirection(){ return controllerDirection;}