Gert Lauritsen / PID

*************************************************************** * Arduino PID Library - Version 1.1.1 * by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com * * This Library is licensed under a GPLv3 License ***************************************************************

Dependents:   TestPID

PID.cpp@0:f6efcf8146b1, 2017-06-02 (annotated)

Committer:
gert_lauritsen
Date:
Fri Jun 02 19:37:02 2017 +0000
Revision:
0:f6efcf8146b1
Testprogram for PID lib

Who changed what in which revision?

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