├── LICENSE ├── README.md ├── examples ├── PID_AdaptiveTunings │ └── PID_AdaptiveTunings.ino ├── PID_Basic │ └── PID_Basic.ino └── PID_RelayOutput │ └── PID_RelayOutput.ino ├── keywords.txt ├── library.properties └── src ├── pid.cpp ├── pid.h └── pid.h.gch /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Particle 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PID control library for Particle 2 | 3 | _Adapted from Brett Beauregard's [Arduino PID Library](https://github.com/br3ttb/Arduino-PID-Library)_ 4 | 5 | For a thorough explanation of the library's workings, see http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ 6 | -------------------------------------------------------------------------------- /examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino: -------------------------------------------------------------------------------- 1 | /******************************************************** 2 | * PID Adaptive Tuning Example 3 | * One of the benefits of the PID library is that you can 4 | * change the tuning parameters at any time. this can be 5 | * helpful if we want the controller to be agressive at some 6 | * times, and conservative at others. in the example below 7 | * we set the controller to use Conservative Tuning Parameters 8 | * when we're near setpoint and more agressive Tuning 9 | * Parameters when we're farther away. 10 | ********************************************************/ 11 | 12 | #include "pid.h" 13 | 14 | //Define Variables we'll be connecting to 15 | double Setpoint, Input, Output; 16 | 17 | //Define the aggressive and conservative Tuning Parameters 18 | double aggKp=4, aggKi=0.2, aggKd=1; 19 | double consKp=1, consKi=0.05, consKd=0.25; 20 | 21 | //Specify the links and initial tuning parameters 22 | PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, PID::DIRECT); 23 | 24 | void setup() 25 | { 26 | //initialize the variables we're linked to 27 | Input = analogRead(0); 28 | Setpoint = 100; 29 | 30 | //turn the PID on 31 | myPID.SetMode(PID::AUTOMATIC); 32 | } 33 | 34 | void loop() 35 | { 36 | Input = analogRead(0); 37 | 38 | double gap = abs(Setpoint-Input); //distance away from setpoint 39 | if(gap<10) 40 | { //we're close to setpoint, use conservative tuning parameters 41 | myPID.SetTunings(consKp, consKi, consKd); 42 | } 43 | else 44 | { 45 | //we're far from setpoint, use aggressive tuning parameters 46 | myPID.SetTunings(aggKp, aggKi, aggKd); 47 | } 48 | 49 | myPID.Compute(); 50 | analogWrite(3,Output); 51 | } 52 | 53 | 54 | -------------------------------------------------------------------------------- /examples/PID_Basic/PID_Basic.ino: -------------------------------------------------------------------------------- 1 | /******************************************************** 2 | * PID Basic Example 3 | * Reading analog input 0 to control analog PWM output 3 4 | ********************************************************/ 5 | 6 | #include "pid.h" 7 | 8 | //Define Variables we'll be connecting to 9 | double Setpoint, Input, Output; 10 | 11 | //Specify the links and initial tuning parameters 12 | PID myPID(&Input, &Output, &Setpoint,2,5,1, PID::DIRECT); 13 | 14 | void setup() 15 | { 16 | //initialize the variables we're linked to 17 | Input = analogRead(0); 18 | Setpoint = 100; 19 | 20 | //turn the PID on 21 | myPID.SetMode(PID::AUTOMATIC); 22 | } 23 | 24 | void loop() 25 | { 26 | Input = analogRead(0); 27 | myPID.Compute(); 28 | analogWrite(3,Output); 29 | } 30 | 31 | 32 | -------------------------------------------------------------------------------- /examples/PID_RelayOutput/PID_RelayOutput.ino: -------------------------------------------------------------------------------- 1 | /******************************************************** 2 | * PID RelayOutput Example 3 | * Same as basic example, except that this time, the output 4 | * is going to a digital pin which (we presume) is controlling 5 | * a relay. the pid is designed to Output an analog value, 6 | * but the relay can only be On/Off. 7 | * 8 | * to connect them together we use "time proportioning 9 | * control" it's essentially a really slow version of PWM. 10 | * first we decide on a window size (5000mS say.) we then 11 | * set the pid to adjust its output between 0 and that window 12 | * size. lastly, we add some logic that translates the PID 13 | * output into "Relay On Time" with the remainder of the 14 | * window being "Relay Off Time" 15 | ********************************************************/ 16 | 17 | #include "pid.h" 18 | #define RelayPin 6 19 | 20 | //Define Variables we'll be connecting to 21 | double Setpoint, Input, Output; 22 | 23 | //Specify the links and initial tuning parameters 24 | PID myPID(&Input, &Output, &Setpoint,2,5,1, PID::DIRECT); 25 | 26 | int WindowSize = 5000; 27 | unsigned long windowStartTime; 28 | void setup() 29 | { 30 | windowStartTime = millis(); 31 | 32 | //initialize the variables we're linked to 33 | Setpoint = 100; 34 | 35 | //tell the PID to range between 0 and the full window size 36 | myPID.SetOutputLimits(0, WindowSize); 37 | 38 | //turn the PID on 39 | myPID.SetMode(PID::AUTOMATIC); 40 | } 41 | 42 | void loop() 43 | { 44 | Input = analogRead(0); 45 | myPID.Compute(); 46 | 47 | /************************************************ 48 | * turn the output pin on/off based on pid output 49 | ************************************************/ 50 | if(millis() - windowStartTime>WindowSize) 51 | { //time to shift the Relay Window 52 | windowStartTime += WindowSize; 53 | } 54 | if(Output < millis() - windowStartTime) digitalWrite(RelayPin,HIGH); 55 | else digitalWrite(RelayPin,LOW); 56 | 57 | } 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For PID Library 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | PID KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | 15 | SetMode KEYWORD2 16 | Compute KEYWORD2 17 | SetOutputLimits KEYWORD2 18 | SetTunings KEYWORD2 19 | SetControllerDirection KEYWORD2 20 | SetSampleTime KEYWORD2 21 | GetKp KEYWORD2 22 | GetKi KEYWORD2 23 | GetKd KEYWORD2 24 | GetMode KEYWORD2 25 | GetDirection KEYWORD2 26 | 27 | ####################################### 28 | # Constants (LITERAL1) 29 | ####################################### 30 | 31 | AUTOMATIC LITERAL1 32 | MANUAL LITERAL1 33 | DIRECT LITERAL1 34 | REVERSE LITERAL1 35 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=pid 2 | version=2.0.1 3 | license=GPLv3 4 | author=Dan Rice 5 | maintainer=Particle 6 | sentence=PID control library 7 | paragraph=A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D). 8 | category=Signal Input/Output 9 | url=https://github.com/particle-iot/Particle-PID 10 | repository=https://github.com/particle-iot/Particle-PID.git 11 | -------------------------------------------------------------------------------- /src/pid.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * Arduino PID Library - Version 1.2.1 3 | * by Brett Beauregard brettbeauregard.com 4 | * 5 | * Adapted for Particle by Dan Rice 6 | * 7 | * This Library is licensed under a GPLv3 License 8 | **********************************************************************************************/ 9 | 10 | #if ARDUINO >= 100 11 | #include "Arduino.h" 12 | #elif defined(SPARK) 13 | #include "application.h" 14 | #else 15 | #include "WProgram.h" 16 | #endif 17 | 18 | #include "pid.h" 19 | 20 | /*Constructor (...)********************************************************* 21 | * The parameters specified here are those for for which we can't set up 22 | * reliable defaults, so we need to have the user set them. 23 | ***************************************************************************/ 24 | PID::PID(double* Input, double* Output, double* Setpoint, 25 | double Kp, double Ki, double Kd, action_t POn, direction_t ControllerDirection) 26 | { 27 | myOutput = Output; 28 | myInput = Input; 29 | mySetpoint = Setpoint; 30 | inAuto = false; 31 | 32 | PID::SetOutputLimits(0, 255); //default output limit corresponds to 33 | //the arduino pwm limits 34 | 35 | SampleTime = 100; //default Controller Sample Time is 0.1 seconds 36 | 37 | PID::SetControllerDirection(ControllerDirection); 38 | PID::SetTunings(Kp, Ki, Kd); 39 | PID::SetAction(POn); 40 | 41 | lastTime = millis()-SampleTime; 42 | } 43 | 44 | /*Constructor (...)********************************************************* 45 | * To allow backwards compatability for v1.1, or for people that just want 46 | * to use Proportional on Error without explicitly saying so 47 | ***************************************************************************/ 48 | 49 | PID::PID(double* Input, double* Output, double* Setpoint, 50 | double Kp, double Ki, double Kd, direction_t ControllerDirection) 51 | :PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) 52 | { 53 | 54 | } 55 | 56 | 57 | /* Compute() ********************************************************************** 58 | * This, as they say, is where the magic happens. this function should be called 59 | * every time "void loop()" executes. the function will decide for itself whether a new 60 | * pid Output needs to be computed. returns true when the output is computed, 61 | * false when nothing has been done. 62 | **********************************************************************************/ 63 | bool PID::Compute() 64 | { 65 | if(!inAuto) return false; 66 | unsigned long now = millis(); 67 | unsigned long timeChange = (now - lastTime); 68 | if(timeChange>=SampleTime) 69 | { 70 | /*Compute all the working error variables*/ 71 | double input = *myInput; 72 | double error = *mySetpoint - input; 73 | double dInput = (input - lastInput); 74 | outputSum+= (ki * error); 75 | 76 | /*Add Proportional on Measurement, if P_ON_M is specified*/ 77 | if(!pOnE) outputSum-= kp * dInput; 78 | 79 | if(outputSum > outMax) outputSum= outMax; 80 | else if(outputSum < outMin) outputSum= outMin; 81 | 82 | /*Add Proportional on Error, if P_ON_E is specified*/ 83 | double output; 84 | if(pOnE) output = kp * error; 85 | else output = 0; 86 | 87 | /*Compute Rest of PID Output*/ 88 | output += outputSum - kd * dInput; 89 | 90 | if(output > outMax) output = outMax; 91 | else if(output < outMin) output = outMin; 92 | *myOutput = output; 93 | 94 | /*Remember some variables for next time*/ 95 | lastInput = input; 96 | lastTime = now; 97 | return true; 98 | } 99 | else return false; 100 | } 101 | 102 | /* SetTunings(...)************************************************************* 103 | * This function allows the controller's dynamic performance to be adjusted. 104 | * it's called automatically from the constructor, but tunings can also 105 | * be adjusted on the fly during normal operation 106 | ******************************************************************************/ 107 | void PID::SetTunings(double Kp, double Ki, double Kd) 108 | { 109 | if (Kp<0 || Ki<0 || Kd<0) return; 110 | 111 | dispKp = Kp; dispKi = Ki; dispKd = Kd; 112 | 113 | double SampleTimeInSec = ((double)SampleTime)/1000; 114 | kp = Kp; 115 | ki = Ki * SampleTimeInSec; 116 | kd = Kd / SampleTimeInSec; 117 | 118 | if(controllerDirection == REVERSE) 119 | { 120 | kp = (0 - kp); 121 | ki = (0 - ki); 122 | kd = (0 - kd); 123 | } 124 | } 125 | 126 | /* SetAction(...)************************************************************* 127 | * Set PID Action P on Error or P on Measurement 128 | ******************************************************************************/ 129 | void PID::SetAction(action_t POn) 130 | { 131 | pOn = POn; 132 | pOnE = POn == P_ON_E; 133 | } 134 | 135 | /* SetSampleTime(...) ********************************************************* 136 | * sets the period, in Milliseconds, at which the calculation is performed 137 | ******************************************************************************/ 138 | void PID::SetSampleTime(int NewSampleTime) 139 | { 140 | if (NewSampleTime > 0) 141 | { 142 | double ratio = (double)NewSampleTime 143 | / (double)SampleTime; 144 | ki *= ratio; 145 | kd /= ratio; 146 | SampleTime = (unsigned long)NewSampleTime; 147 | } 148 | } 149 | 150 | /* SetOutputLimits(...)**************************************************** 151 | * This function will be used far more often than SetInputLimits. while 152 | * the input to the controller will generally be in the 0-1023 range (which is 153 | * the default already,) the output will be a little different. maybe they'll 154 | * be doing a time window and will need 0-8000 or something. or maybe they'll 155 | * want to clamp it from 0-125. who knows. at any rate, that can all be done 156 | * here. 157 | **************************************************************************/ 158 | void PID::SetOutputLimits(double Min, double Max) 159 | { 160 | if(Min >= Max) return; 161 | outMin = Min; 162 | outMax = Max; 163 | 164 | if(inAuto) 165 | { 166 | if(*myOutput > outMax) *myOutput = outMax; 167 | else if(*myOutput < outMin) *myOutput = outMin; 168 | 169 | if(outputSum > outMax) outputSum= outMax; 170 | else if(outputSum < outMin) outputSum= outMin; 171 | } 172 | } 173 | 174 | /* SetMode(...)**************************************************************** 175 | * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) 176 | * when the transition from manual to auto occurs, the controller is 177 | * automatically initialized 178 | ******************************************************************************/ 179 | void PID::SetMode(mode_t Mode) 180 | { 181 | bool newAuto = (Mode == AUTOMATIC); 182 | if(newAuto && !inAuto) 183 | { /*we just went from manual to auto*/ 184 | PID::Initialize(); 185 | } 186 | inAuto = newAuto; 187 | } 188 | 189 | /* Initialize()**************************************************************** 190 | * does all the things that need to happen to ensure a bumpless transfer 191 | * from manual to automatic mode. 192 | ******************************************************************************/ 193 | void PID::Initialize() 194 | { 195 | outputSum = *myOutput; 196 | lastInput = *myInput; 197 | if(outputSum > outMax) outputSum = outMax; 198 | else if(outputSum < outMin) outputSum = outMin; 199 | } 200 | 201 | /* SetControllerDirection(...)************************************************* 202 | * The PID will either be connected to a DIRECT acting process (+Output leads 203 | * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to 204 | * know which one, because otherwise we may increase the output when we should 205 | * be decreasing. This is called from the constructor. 206 | ******************************************************************************/ 207 | void PID::SetControllerDirection(direction_t Direction) 208 | { 209 | if(inAuto && Direction != controllerDirection) 210 | { 211 | kp = (0 - kp); 212 | ki = (0 - ki); 213 | kd = (0 - kd); 214 | } 215 | controllerDirection = Direction; 216 | } 217 | 218 | /* Status Funcions************************************************************* 219 | * Just because you set the Kp=-1 doesn't mean it actually happened. these 220 | * functions query the internal state of the PID. they're here for display 221 | * purposes. this are the functions the PID Front-end uses for example 222 | ******************************************************************************/ 223 | double PID::GetKp(){ return dispKp; } 224 | double PID::GetKi(){ return dispKi; } 225 | double PID::GetKd(){ return dispKd; } 226 | int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL; } 227 | int PID::GetDirection(){ return controllerDirection; } 228 | int PID::GetAction(){ return pOn; } 229 | -------------------------------------------------------------------------------- /src/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_h 2 | #define PID_h 3 | #define LIBRARY_VERSION 1.2.1 4 | 5 | class PID 6 | { 7 | public: 8 | 9 | //Parameter types for some of the functions below 10 | enum mode_t { AUTOMATIC = 1, MANUAL = 0 }; 11 | enum direction_t { DIRECT = 0, REVERSE = 1 }; 12 | enum action_t { P_ON_M = 0, P_ON_E = 1 }; 13 | 14 | //commonly used functions ************************************************************************** 15 | PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and 16 | double, double, double, // Setpoint. Initial tuning parameters are also set here. 17 | action_t, direction_t); // (overload for specifying proportional mode) 18 | 19 | 20 | PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and 21 | double, double, double, direction_t); // Setpoint. Initial tuning parameters are also set here 22 | 23 | void SetMode(mode_t); // * sets PID to either Manual (0) or Auto (non-0) 24 | 25 | bool Compute(); // * performs the PID calculation. it should be 26 | // called every time loop() cycles. ON/OFF and 27 | // calculation frequency can be set using SetMode 28 | // SetSampleTime respectively 29 | 30 | void SetOutputLimits(double, double); // * clamps the output to a specific range. 0-255 by default, but 31 | // it's likely the user will want to change this depending on 32 | // the application 33 | 34 | 35 | 36 | //available but not commonly used functions ******************************************************** 37 | void SetTunings(double, double, // * While most users will set the tunings once in the 38 | double); // constructor, this function gives the user the option 39 | // of changing tunings during runtime for Adaptive control 40 | void SetAction(action_t); 41 | 42 | void SetControllerDirection(direction_t); // * Sets the Direction, or "Action" of the controller. DIRECT 43 | // means the output will increase when error is positive. REVERSE 44 | // means the opposite. it's very unlikely that this will be needed 45 | // once it is set in the constructor. 46 | void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which 47 | // the PID calculation is performed. default is 100 48 | 49 | 50 | 51 | //Display functions **************************************************************** 52 | double GetKp(); // These functions query the pid for interal values. 53 | double GetKi(); // they were created mainly for the pid front-end, 54 | double GetKd(); // where it's important to know what is actually 55 | int GetMode(); // inside the PID. 56 | int GetDirection(); 57 | int GetAction(); 58 | 59 | private: 60 | void Initialize(); 61 | 62 | double dispKp; // * we'll hold on to the tuning parameters in user-entered 63 | double dispKi; // format for display purposes 64 | double dispKd; 65 | 66 | double kp; // * (P)roportional Tuning Parameter 67 | double ki; // * (I)ntegral Tuning Parameter 68 | double kd; // * (D)erivative Tuning Parameter 69 | 70 | int controllerDirection; 71 | int pOn; 72 | 73 | double *myInput; // * Pointers to the Input, Output, and Setpoint variables 74 | double *myOutput; // This creates a hard link between the variables and the 75 | double *mySetpoint; // PID, freeing the user from having to constantly tell us 76 | // what these values are. with pointers we'll just know. 77 | 78 | unsigned long lastTime; 79 | double outputSum, lastInput; 80 | 81 | unsigned long SampleTime; 82 | double outMin, outMax; 83 | bool inAuto, pOnE; 84 | }; 85 | #endif 86 | -------------------------------------------------------------------------------- /src/pid.h.gch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/particle-iot/Particle-PID/64930d3920d0f3bc86240dbaff93c46d1affbb86/src/pid.h.gch --------------------------------------------------------------------------------