Michael Ball / PID

This is a copy of the Reference Standard PID controller ala controlguru.com

Fork of PID by FRDM-K64F Code Share

PID.cpp@3:316f974b7f98, 2016-02-07 (annotated)

Committer:
unix_guru
Date:
Sun Feb 07 19:06:37 2016 +0000
Revision:
3:316f974b7f98
Parent:
2:55bf0f813bb4 
Moved from Double to Floating point PID library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arnaudsuire 0:d58c1b8d63d9 1 /**
unix_guru 3:316f974b7f98 2 * Arduino PID Library - Version 1.1.1
unix_guru 3:316f974b7f98 3 * @author Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
unix_guru 2:55bf0f813bb4 4 *
unix_guru 2:55bf0f813bb4 5 * @section LICENSE
unix_guru 2:55bf0f813bb4 6 *
unix_guru 2:55bf0f813bb4 7 * Permission is hereby granted, free of charge, to any person obtaining a copy
unix_guru 2:55bf0f813bb4 8 * of this software and associated documentation files (the "Software"), to deal
unix_guru 2:55bf0f813bb4 9 * in the Software without restriction, including without limitation the rights
unix_guru 2:55bf0f813bb4 10 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
unix_guru 2:55bf0f813bb4 11 * copies of the Software, and to permit persons to whom the Software is
unix_guru 2:55bf0f813bb4 12 * furnished to do so, subject to the following conditions:
unix_guru 2:55bf0f813bb4 13 *
unix_guru 2:55bf0f813bb4 14 * The above copyright notice and this permission notice shall be included in
unix_guru 2:55bf0f813bb4 15 * all copies or substantial portions of the Software.
unix_guru 2:55bf0f813bb4 16 *
unix_guru 2:55bf0f813bb4 17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
unix_guru 2:55bf0f813bb4 18 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
unix_guru 2:55bf0f813bb4 19 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
unix_guru 2:55bf0f813bb4 20 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
unix_guru 2:55bf0f813bb4 21 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
unix_guru 2:55bf0f813bb4 22 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
unix_guru 2:55bf0f813bb4 23 * THE SOFTWARE.
unix_guru 2:55bf0f813bb4 24 *
unix_guru 2:55bf0f813bb4 25 * @section DESCRIPTION
unix_guru 2:55bf0f813bb4 26 *
unix_guru 2:55bf0f813bb4 27 * A PID controller is a widely used feedback controller commonly found in
unix_guru 2:55bf0f813bb4 28 * industry.
unix_guru 2:55bf0f813bb4 29 *
unix_guru 2:55bf0f813bb4 30 * This library is a port of Brett Beauregard's Arduino PID library:
unix_guru 2:55bf0f813bb4 31 *
unix_guru 3:316f974b7f98 32 * https://github.com/br3ttb/Arduino-PID-Library
unix_guru 2:55bf0f813bb4 33 *
unix_guru 2:55bf0f813bb4 34 * The wikipedia article on PID controllers is a good place to start on
unix_guru 2:55bf0f813bb4 35 * understanding how they work:
unix_guru 2:55bf0f813bb4 36 *
unix_guru 2:55bf0f813bb4 37 * http://en.wikipedia.org/wiki/PID_controller
unix_guru 2:55bf0f813bb4 38 *
unix_guru 2:55bf0f813bb4 39 * For a clear and elegant explanation of how to implement and tune a
unix_guru 2:55bf0f813bb4 40 * controller, the controlguru website by Douglas J. Cooper (who also happened
unix_guru 2:55bf0f813bb4 41 * to be Brett's controls professor) is an excellent reference:
unix_guru 2:55bf0f813bb4 42 *
unix_guru 2:55bf0f813bb4 43 * http://www.controlguru.com/
unix_guru 2:55bf0f813bb4 44 */
unix_guru 2:55bf0f813bb4 45
unix_guru 2:55bf0f813bb4 46 /**
unix_guru 2:55bf0f813bb4 47 * Includes
unix_guru 2:55bf0f813bb4 48 */
arnaudsuire 0:d58c1b8d63d9 49 #include "PID.h"
unix_guru 3:316f974b7f98 50 #include "millis/millis.h"
unix_guru 2:55bf0f813bb4 51
unix_guru 3:316f974b7f98 52 extern Serial pc;
unix_guru 2:55bf0f813bb4 53
unix_guru 3:316f974b7f98 54
unix_guru 3:316f974b7f98 55 /*Constructor (...)*********************************************************
unix_guru 3:316f974b7f98 56 * The parameters specified here are those for for which we can't set up
unix_guru 3:316f974b7f98 57 * reliable defaults, so we need to have the user set them.
unix_guru 3:316f974b7f98 58 ***************************************************************************/
unix_guru 3:316f974b7f98 59 PID::PID(float* Input, float* Output, float* Setpoint,
unix_guru 3:316f974b7f98 60 float Kp, float Ki, float Kd, int ControllerDirection)
unix_guru 3:316f974b7f98 61 {
unix_guru 3:316f974b7f98 62
unix_guru 3:316f974b7f98 63 myOutput = Output;
unix_guru 3:316f974b7f98 64 myInput = Input;
unix_guru 3:316f974b7f98 65 mySetpoint = Setpoint;
unix_guru 3:316f974b7f98 66 inAuto = false;
unix_guru 3:316f974b7f98 67
unix_guru 3:316f974b7f98 68 PID::SetOutputLimits(0, 1000); // default output limit corresponds to
unix_guru 3:316f974b7f98 69 // the arduino pwm limits
unix_guru 3:316f974b7f98 70 SampleTime = 100; // default Controller Sample Time is 0.1 seconds
unix_guru 3:316f974b7f98 71
unix_guru 3:316f974b7f98 72 PID::SetControllerDirection(ControllerDirection);
unix_guru 3:316f974b7f98 73 PID::SetTunings(Kp, Ki, Kd);
unix_guru 3:316f974b7f98 74
unix_guru 3:316f974b7f98 75 lastTime = millis()-SampleTime;
unix_guru 1:117e0c36eb22 76 }
unix_guru 2:55bf0f813bb4 77
unix_guru 2:55bf0f813bb4 78
unix_guru 3:316f974b7f98 79 /* Compute() **********************************************************************
unix_guru 3:316f974b7f98 80 * This, as they say, is where the magic happens. this function should be called
unix_guru 3:316f974b7f98 81 * every time "void loop()" executes. the function will decide for itself whether a new
unix_guru 3:316f974b7f98 82 * pid Output needs to be computed. returns true when the output is computed,
unix_guru 3:316f974b7f98 83 * false when nothing has been done.
unix_guru 3:316f974b7f98 84 **********************************************************************************/
unix_guru 3:316f974b7f98 85 bool PID::Compute()
unix_guru 3:316f974b7f98 86 {
unix_guru 3:316f974b7f98 87 if(!inAuto) return false;
unix_guru 3:316f974b7f98 88 unsigned long now = millis();
unix_guru 3:316f974b7f98 89 unsigned long timeChange = (now - lastTime);
unix_guru 3:316f974b7f98 90 if(timeChange>=SampleTime)
unix_guru 3:316f974b7f98 91 {
unix_guru 3:316f974b7f98 92 /*Compute all the working error variables*/
unix_guru 3:316f974b7f98 93 float input = *myInput;
unix_guru 3:316f974b7f98 94 float error = *mySetpoint - input;
unix_guru 3:316f974b7f98 95 ITerm+= (ki * error);
unix_guru 3:316f974b7f98 96 if(ITerm > outMax) ITerm= outMax;
unix_guru 3:316f974b7f98 97 else if(ITerm < outMin) ITerm= outMin;
unix_guru 3:316f974b7f98 98 float dInput = (input - lastInput);
unix_guru 3:316f974b7f98 99 // pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint);
unix_guru 2:55bf0f813bb4 100
unix_guru 3:316f974b7f98 101 /*Compute PID Output*/
unix_guru 3:316f974b7f98 102 float output = kp * error + ITerm- kd * dInput;
unix_guru 3:316f974b7f98 103 // pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint);
unix_guru 3:316f974b7f98 104
unix_guru 3:316f974b7f98 105 if(output > outMax) output = outMax;
unix_guru 3:316f974b7f98 106 else if(output < outMin) output = outMin;
unix_guru 3:316f974b7f98 107 *myOutput = output;
unix_guru 2:55bf0f813bb4 108
unix_guru 3:316f974b7f98 109 /*Remember some variables for next time*/
unix_guru 3:316f974b7f98 110 lastInput = input;
unix_guru 3:316f974b7f98 111 lastTime = now;
unix_guru 3:316f974b7f98 112 return true;
unix_guru 3:316f974b7f98 113 }
unix_guru 3:316f974b7f98 114 else return false;
arnaudsuire 0:d58c1b8d63d9 115 }
unix_guru 3:316f974b7f98 116
unix_guru 3:316f974b7f98 117
unix_guru 3:316f974b7f98 118 /* SetTunings(...)*************************************************************
unix_guru 3:316f974b7f98 119 * This function allows the controller's dynamic performance to be adjusted.
unix_guru 3:316f974b7f98 120 * it's called automatically from the constructor, but tunings can also
unix_guru 3:316f974b7f98 121 * be adjusted on the fly during normal operation
unix_guru 3:316f974b7f98 122 ******************************************************************************/
unix_guru 3:316f974b7f98 123 void PID::SetTunings(float Kp, float Ki, float Kd)
unix_guru 3:316f974b7f98 124 {
unix_guru 3:316f974b7f98 125 if (Kp<0 || Ki<0 || Kd<0) return;
unix_guru 2:55bf0f813bb4 126
unix_guru 3:316f974b7f98 127 dispKp = Kp; dispKi = Ki; dispKd = Kd;
unix_guru 3:316f974b7f98 128
unix_guru 3:316f974b7f98 129 float SampleTimeInSec = ((float)SampleTime)/1000;
unix_guru 3:316f974b7f98 130 kp = Kp;
unix_guru 3:316f974b7f98 131 ki = Ki * SampleTimeInSec;
unix_guru 3:316f974b7f98 132 kd = Kd / SampleTimeInSec;
unix_guru 2:55bf0f813bb4 133
unix_guru 3:316f974b7f98 134 if(controllerDirection ==REVERSE)
unix_guru 3:316f974b7f98 135 {
unix_guru 3:316f974b7f98 136 kp = (0 - kp);
unix_guru 3:316f974b7f98 137 ki = (0 - ki);
unix_guru 3:316f974b7f98 138 kd = (0 - kd);
unix_guru 3:316f974b7f98 139 }
unix_guru 3:316f974b7f98 140 }
unix_guru 3:316f974b7f98 141
unix_guru 3:316f974b7f98 142 /* SetSampleTime(...) *********************************************************
unix_guru 3:316f974b7f98 143 * sets the period, in Milliseconds, at which the calculation is performed
unix_guru 3:316f974b7f98 144 ******************************************************************************/
unix_guru 3:316f974b7f98 145 void PID::SetSampleTime(int NewSampleTime)
unix_guru 3:316f974b7f98 146 {
unix_guru 3:316f974b7f98 147 if (NewSampleTime > 0)
unix_guru 3:316f974b7f98 148 {
unix_guru 3:316f974b7f98 149 float ratio = (float)NewSampleTime
unix_guru 3:316f974b7f98 150 / (float)SampleTime;
unix_guru 3:316f974b7f98 151 ki *= ratio;
unix_guru 3:316f974b7f98 152 kd /= ratio;
unix_guru 3:316f974b7f98 153 SampleTime = (unsigned long)NewSampleTime;
unix_guru 3:316f974b7f98 154 }
arnaudsuire 0:d58c1b8d63d9 155 }
unix_guru 2:55bf0f813bb4 156
unix_guru 3:316f974b7f98 157 /* SetOutputLimits(...)****************************************************
unix_guru 3:316f974b7f98 158 * This function will be used far more often than SetInputLimits. while
unix_guru 3:316f974b7f98 159 * the input to the controller will generally be in the 0-1023 range (which is
unix_guru 3:316f974b7f98 160 * the default already,) the output will be a little different. maybe they'll
unix_guru 3:316f974b7f98 161 * be doing a time window and will need 0-8000 or something. or maybe they'll
unix_guru 3:316f974b7f98 162 * want to clamp it from 0-125. who knows. at any rate, that can all be done
unix_guru 3:316f974b7f98 163 * here.
unix_guru 3:316f974b7f98 164 **************************************************************************/
unix_guru 3:316f974b7f98 165 void PID::SetOutputLimits(float Min, float Max)
unix_guru 3:316f974b7f98 166 {
unix_guru 3:316f974b7f98 167 if(Min >= Max) return;
unix_guru 3:316f974b7f98 168 outMin = Min;
unix_guru 3:316f974b7f98 169 outMax = Max;
unix_guru 2:55bf0f813bb4 170
unix_guru 3:316f974b7f98 171 if(inAuto)
unix_guru 3:316f974b7f98 172 {
unix_guru 3:316f974b7f98 173 if(*myOutput > outMax) *myOutput = outMax;
unix_guru 3:316f974b7f98 174 else if(*myOutput < outMin) *myOutput = outMin;
unix_guru 3:316f974b7f98 175
unix_guru 3:316f974b7f98 176 if(ITerm > outMax) ITerm= outMax;
unix_guru 3:316f974b7f98 177 else if(ITerm < outMin) ITerm= outMin;
unix_guru 3:316f974b7f98 178 }
unix_guru 3:316f974b7f98 179 }
unix_guru 3:316f974b7f98 180
unix_guru 3:316f974b7f98 181 /* SetMode(...)****************************************************************
unix_guru 3:316f974b7f98 182 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
unix_guru 3:316f974b7f98 183 * when the transition from manual to auto occurs, the controller is
unix_guru 3:316f974b7f98 184 * automatically initialized
unix_guru 3:316f974b7f98 185 ******************************************************************************/
unix_guru 3:316f974b7f98 186 void PID::SetMode(int Mode)
unix_guru 3:316f974b7f98 187 {
unix_guru 3:316f974b7f98 188 bool newAuto = (Mode == AUTOMATIC);
unix_guru 3:316f974b7f98 189 if(newAuto == !inAuto)
unix_guru 3:316f974b7f98 190 { /*we just went from manual to auto*/
unix_guru 3:316f974b7f98 191 PID::Initialize();
arnaudsuire 0:d58c1b8d63d9 192 }
unix_guru 3:316f974b7f98 193 inAuto = newAuto;
arnaudsuire 0:d58c1b8d63d9 194 }
unix_guru 2:55bf0f813bb4 195
unix_guru 3:316f974b7f98 196 /* Initialize()****************************************************************
unix_guru 3:316f974b7f98 197 * does all the things that need to happen to ensure a bumpless transfer
unix_guru 3:316f974b7f98 198 * from manual to automatic mode.
unix_guru 3:316f974b7f98 199 ******************************************************************************/
unix_guru 3:316f974b7f98 200 void PID::Initialize()
unix_guru 3:316f974b7f98 201 {
unix_guru 3:316f974b7f98 202 ITerm = *myOutput;
unix_guru 3:316f974b7f98 203 lastInput = *myInput;
unix_guru 3:316f974b7f98 204 if(ITerm > outMax) ITerm = outMax;
unix_guru 3:316f974b7f98 205 else if(ITerm < outMin) ITerm = outMin;
arnaudsuire 0:d58c1b8d63d9 206 }
unix_guru 3:316f974b7f98 207
unix_guru 3:316f974b7f98 208 /* SetControllerDirection(...)*************************************************
unix_guru 3:316f974b7f98 209 * The PID will either be connected to a DIRECT acting process (+Output leads
unix_guru 3:316f974b7f98 210 * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
unix_guru 3:316f974b7f98 211 * know which one, because otherwise we may increase the output when we should
unix_guru 3:316f974b7f98 212 * be decreasing. This is called from the constructor.
unix_guru 3:316f974b7f98 213 ******************************************************************************/
unix_guru 3:316f974b7f98 214 void PID::SetControllerDirection(int Direction)
unix_guru 3:316f974b7f98 215 {
unix_guru 3:316f974b7f98 216 if(inAuto && Direction !=controllerDirection)
unix_guru 3:316f974b7f98 217 {
unix_guru 3:316f974b7f98 218 kp = (0 - kp);
unix_guru 3:316f974b7f98 219 ki = (0 - ki);
unix_guru 3:316f974b7f98 220 kd = (0 - kd);
unix_guru 3:316f974b7f98 221 }
unix_guru 3:316f974b7f98 222 controllerDirection = Direction;
unix_guru 2:55bf0f813bb4 223 }
arnaudsuire 0:d58c1b8d63d9 224
unix_guru 3:316f974b7f98 225 /* Status Funcions*************************************************************
unix_guru 3:316f974b7f98 226 * Just because you set the Kp=-1 doesn't mean it actually happened. these
unix_guru 3:316f974b7f98 227 * functions query the internal state of the PID. they're here for display
unix_guru 3:316f974b7f98 228 * purposes. this are the functions the PID Front-end uses for example
unix_guru 3:316f974b7f98 229 ******************************************************************************/
unix_guru 3:316f974b7f98 230 float PID::GetKp(){ return dispKp; }
unix_guru 3:316f974b7f98 231 float PID::GetKi(){ return dispKi;}
unix_guru 3:316f974b7f98 232 float PID::GetKd(){ return dispKd;}
unix_guru 3:316f974b7f98 233 int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
unix_guru 3:316f974b7f98 234 int PID::GetDirection(){ return controllerDirection;}
arnaudsuire 0:d58c1b8d63d9 235