*************************************************************** * Arduino PID Library - Version 1.1.1 * by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com * * This Library is licensed under a GPLv3 License ***************************************************************
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?
User | Revision | Line number | New 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 |