From 27cdb74a9c9a039813618d11dca31157e332e195 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 6 Jan 2021 22:18:03 -0500 Subject: [PATCH 001/101] New Library, improved performance --- PID_v1.cpp | 224 -------------- PID_v1.h | 90 ------ QuickPID.cpp | 279 ++++++++++++++++++ QuickPID.h | 100 +++++++ README.md | 60 ++++ README.txt | 11 - .../PID_AdaptiveTunings.ino | 56 ---- examples/PID_Basic/PID_Basic.ino | 35 --- examples/PID_PonM/PID_PonM.ino | 36 --- examples/PID_RelayOutput/PID_RelayOutput.ino | 64 ---- .../QuickPID_Self_Test/QuickPID_Self_Test.ino | 69 +++++ keywords.txt | 24 +- library.json | 12 +- library.properties | 14 +- 14 files changed, 534 insertions(+), 540 deletions(-) delete mode 100644 PID_v1.cpp delete mode 100644 PID_v1.h create mode 100644 QuickPID.cpp create mode 100644 QuickPID.h create mode 100644 README.md delete mode 100644 README.txt delete mode 100644 examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino delete mode 100644 examples/PID_Basic/PID_Basic.ino delete mode 100644 examples/PID_PonM/PID_PonM.ino delete mode 100644 examples/PID_RelayOutput/PID_RelayOutput.ino create mode 100644 examples/QuickPID_Self_Test/QuickPID_Self_Test.ino diff --git a/PID_v1.cpp b/PID_v1.cpp deleted file mode 100644 index cb6637c..0000000 --- a/PID_v1.cpp +++ /dev/null @@ -1,224 +0,0 @@ -/********************************************************************************************** - * Arduino PID Library - Version 1.2.1 - * by Brett Beauregard brettbeauregard.com - * - * This Library is licensed under the MIT License - **********************************************************************************************/ - -#if ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - -#include - -/*Constructor (...)********************************************************* - * The parameters specified here are those for for which we can't set up - * reliable defaults, so we need to have the user set them. - ***************************************************************************/ -PID::PID(double* Input, double* Output, double* Setpoint, - double Kp, double Ki, double Kd, int POn, int ControllerDirection) -{ - myOutput = Output; - myInput = Input; - mySetpoint = Setpoint; - inAuto = false; - - PID::SetOutputLimits(0, 255); //default output limit corresponds to - //the arduino pwm limits - - SampleTime = 100; //default Controller Sample Time is 0.1 seconds - - PID::SetControllerDirection(ControllerDirection); - PID::SetTunings(Kp, Ki, Kd, POn); - - lastTime = millis()-SampleTime; -} - -/*Constructor (...)********************************************************* - * To allow backwards compatability for v1.1, or for people that just want - * to use Proportional on Error without explicitly saying so - ***************************************************************************/ - -PID::PID(double* Input, double* Output, double* Setpoint, - double Kp, double Ki, double Kd, int ControllerDirection) - :PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) -{ - -} - - -/* Compute() ********************************************************************** - * This, as they say, is where the magic happens. this function should be called - * every time "void loop()" executes. the function will decide for itself whether a new - * pid Output needs to be computed. returns true when the output is computed, - * false when nothing has been done. - **********************************************************************************/ -bool PID::Compute() -{ - if(!inAuto) return false; - unsigned long now = millis(); - unsigned long timeChange = (now - lastTime); - if(timeChange>=SampleTime) - { - /*Compute all the working error variables*/ - double input = *myInput; - double error = *mySetpoint - input; - double dInput = (input - lastInput); - outputSum+= (ki * error); - - /*Add Proportional on Measurement, if P_ON_M is specified*/ - if(!pOnE) outputSum-= kp * dInput; - - if(outputSum > outMax) outputSum= outMax; - else if(outputSum < outMin) outputSum= outMin; - - /*Add Proportional on Error, if P_ON_E is specified*/ - double output; - if(pOnE) output = kp * error; - else output = 0; - - /*Compute Rest of PID Output*/ - output += outputSum - kd * dInput; - - if(output > outMax) output = outMax; - else if(output < outMin) output = outMin; - *myOutput = output; - - /*Remember some variables for next time*/ - lastInput = input; - lastTime = now; - return true; - } - else return false; -} - -/* SetTunings(...)************************************************************* - * This function allows the controller's dynamic performance to be adjusted. - * it's called automatically from the constructor, but tunings can also - * be adjusted on the fly during normal operation - ******************************************************************************/ -void PID::SetTunings(double Kp, double Ki, double Kd, int POn) -{ - if (Kp<0 || Ki<0 || Kd<0) return; - - pOn = POn; - pOnE = POn == P_ON_E; - - dispKp = Kp; dispKi = Ki; dispKd = Kd; - - double SampleTimeInSec = ((double)SampleTime)/1000; - kp = Kp; - ki = Ki * SampleTimeInSec; - kd = Kd / SampleTimeInSec; - - if(controllerDirection ==REVERSE) - { - kp = (0 - kp); - ki = (0 - ki); - kd = (0 - kd); - } -} - -/* SetTunings(...)************************************************************* - * Set Tunings using the last-rembered POn setting - ******************************************************************************/ -void PID::SetTunings(double Kp, double Ki, double Kd){ - SetTunings(Kp, Ki, Kd, pOn); -} - -/* SetSampleTime(...) ********************************************************* - * sets the period, in Milliseconds, at which the calculation is performed - ******************************************************************************/ -void PID::SetSampleTime(int NewSampleTime) -{ - if (NewSampleTime > 0) - { - double ratio = (double)NewSampleTime - / (double)SampleTime; - ki *= ratio; - kd /= ratio; - SampleTime = (unsigned long)NewSampleTime; - } -} - -/* SetOutputLimits(...)**************************************************** - * This function will be used far more often than SetInputLimits. while - * the input to the controller will generally be in the 0-1023 range (which is - * the default already,) the output will be a little different. maybe they'll - * be doing a time window and will need 0-8000 or something. or maybe they'll - * want to clamp it from 0-125. who knows. at any rate, that can all be done - * here. - **************************************************************************/ -void PID::SetOutputLimits(double Min, double Max) -{ - if(Min >= Max) return; - outMin = Min; - outMax = Max; - - if(inAuto) - { - if(*myOutput > outMax) *myOutput = outMax; - else if(*myOutput < outMin) *myOutput = outMin; - - if(outputSum > outMax) outputSum= outMax; - else if(outputSum < outMin) outputSum= outMin; - } -} - -/* SetMode(...)**************************************************************** - * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) - * when the transition from manual to auto occurs, the controller is - * automatically initialized - ******************************************************************************/ -void PID::SetMode(int Mode) -{ - bool newAuto = (Mode == AUTOMATIC); - if(newAuto && !inAuto) - { /*we just went from manual to auto*/ - PID::Initialize(); - } - inAuto = newAuto; -} - -/* Initialize()**************************************************************** - * does all the things that need to happen to ensure a bumpless transfer - * from manual to automatic mode. - ******************************************************************************/ -void PID::Initialize() -{ - outputSum = *myOutput; - lastInput = *myInput; - if(outputSum > outMax) outputSum = outMax; - else if(outputSum < outMin) outputSum = outMin; -} - -/* SetControllerDirection(...)************************************************* - * The PID will either be connected to a DIRECT acting process (+Output leads - * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to - * know which one, because otherwise we may increase the output when we should - * be decreasing. This is called from the constructor. - ******************************************************************************/ -void PID::SetControllerDirection(int Direction) -{ - if(inAuto && Direction !=controllerDirection) - { - kp = (0 - kp); - ki = (0 - ki); - kd = (0 - kd); - } - controllerDirection = Direction; -} - -/* Status Funcions************************************************************* - * Just because you set the Kp=-1 doesn't mean it actually happened. these - * functions query the internal state of the PID. they're here for display - * purposes. this are the functions the PID Front-end uses for example - ******************************************************************************/ -double PID::GetKp(){ return dispKp; } -double PID::GetKi(){ return dispKi;} -double PID::GetKd(){ return dispKd;} -int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} -int PID::GetDirection(){ return controllerDirection;} - diff --git a/PID_v1.h b/PID_v1.h deleted file mode 100644 index 9cba046..0000000 --- a/PID_v1.h +++ /dev/null @@ -1,90 +0,0 @@ -#ifndef PID_v1_h -#define PID_v1_h -#define LIBRARY_VERSION 1.2.1 - -class PID -{ - - - public: - - //Constants used in some of the functions below - #define AUTOMATIC 1 - #define MANUAL 0 - #define DIRECT 0 - #define REVERSE 1 - #define P_ON_M 0 - #define P_ON_E 1 - - //commonly used functions ************************************************************************** - PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and - double, double, double, int, int);// Setpoint. Initial tuning parameters are also set here. - // (overload for specifying proportional mode) - - PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and - double, double, double, int); // Setpoint. Initial tuning parameters are also set here - - void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) - - bool Compute(); // * performs the PID calculation. it should be - // called every time loop() cycles. ON/OFF and - // calculation frequency can be set using SetMode - // SetSampleTime respectively - - void SetOutputLimits(double, double); // * clamps the output to a specific range. 0-255 by default, but - // it's likely the user will want to change this depending on - // the application - - - - //available but not commonly used functions ******************************************************** - void SetTunings(double, double, // * While most users will set the tunings once in the - double); // constructor, this function gives the user the option - // of changing tunings during runtime for Adaptive control - void SetTunings(double, double, // * overload for specifying proportional mode - double, int); - - void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT - // means the output will increase when error is positive. REVERSE - // means the opposite. it's very unlikely that this will be needed - // once it is set in the constructor. - void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which - // the PID calculation is performed. default is 100 - - - - //Display functions **************************************************************** - double GetKp(); // These functions query the pid for interal values. - double GetKi(); // they were created mainly for the pid front-end, - double GetKd(); // where it's important to know what is actually - int GetMode(); // inside the PID. - int GetDirection(); // - - private: - void Initialize(); - - double dispKp; // * we'll hold on to the tuning parameters in user-entered - double dispKi; // format for display purposes - double dispKd; // - - double kp; // * (P)roportional Tuning Parameter - double ki; // * (I)ntegral Tuning Parameter - double kd; // * (D)erivative Tuning Parameter - - int controllerDirection; - int pOn; - - double *myInput; // * Pointers to the Input, Output, and Setpoint variables - double *myOutput; // This creates a hard link between the variables and the - double *mySetpoint; // PID, freeing the user from having to constantly tell us - // what these values are. with pointers we'll just know. - - unsigned long lastTime; - double outputSum, lastInput; - - unsigned long SampleTime; - double outMin, outMax; - bool inAuto, pOnE; -}; -#endif - diff --git a/QuickPID.cpp b/QuickPID.cpp new file mode 100644 index 0000000..0b808ef --- /dev/null +++ b/QuickPID.cpp @@ -0,0 +1,279 @@ +/********************************************************************************** + Arduino QuickPID Library for Arduino - Version 2.0.0 + by dlloydev https://github.com/Dlloydev/QuickPID + Based on the Arduino PID Library by Brett Beauregard + + This Library is licensed under the MIT License + **********************************************************************************/ + +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#include "QuickPID.h" + + +/* Constructor ******************************************************************** + The parameters specified here are those for for which we can't set up + reliable defaults, so we need to have the user set them. + **********************************************************************************/ +QuickPID::QuickPID(int16_t* Input, uint8_t* Output, int16_t* Setpoint, + float Kp, float Ki, float Kd, bool POn, bool ControllerDirection) +{ + myOutput = Output; + myInput = Input; + mySetpoint = Setpoint; + inAuto = false; + + QuickPID::SetOutputLimits(0, 255); // default is same as the arduino PWM limit + SampleTimeUs = 100000; // default is 0.1 seconds + + QuickPID::SetControllerDirection(ControllerDirection); + QuickPID::SetTunings(Kp, Ki, Kd, POn); + + lastTime = micros() - SampleTimeUs; +} + +/* Constructor ******************************************************************** + To allow backwards compatability for v1.1, or for people that just want + to use Proportional on Error without explicitly saying so. + **********************************************************************************/ + +QuickPID::QuickPID(int16_t* Input, uint8_t* Output, int16_t* Setpoint, + float Kp, float Ki, float Kd, bool ControllerDirection) + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) +{ + +} + +/* Compute() ********************************************************************** + This, as they say, is where the magic happens. This function should be called + every time "void loop()" executes. The function will decide whether a new + PID Output needs to be computed. Returns true when the output is computed, + false when nothing has been done. + **********************************************************************************/ +bool QuickPID::Compute() +{ + if (!inAuto) return false; + uint32_t now = micros(); + uint32_t timeChange = (now - lastTime); + if (timeChange >= SampleTimeUs) + { + /*Compute all the working error variables*/ + int16_t input = *myInput; + error = (*mySetpoint - input); + int32_t dInput = (int32_t)input - (int32_t)lastInput; + + /* outputSum+= (ki * error); */ + //outputSum += (ki * error); + if (ki < 31) outputSum += FX_INT(FX_MUL(FL_FX(ki), INT_FX(error))); + else outputSum += FX_INT(FX_MUL(FL__FX(ki), INT_FX(error))); + + /*Add Proportional on Measurement, if P_ON_M is specified*/ + /* outputSum -= kp * dInput; */ + if (!pOnE) { + //outputSum -= kp * dInput; + if (kp < 31) outputSum -= FX_INT(FX_MUL(FL_FX(kp), INT_FX(dInput))); + else outputSum -= FX_INT(FX_MUL(FL__FX(kp), INT_FX(dInput))); + } + + if (outputSum > outMax) outputSum = outMax; + else if (outputSum < outMin) outputSum = outMin; + + /*Add Proportional on Error, if P_ON_E is specified*/ + /* if (pOnE) output = (kp * (float)error); */ + uint16_t output; + if (pOnE) { + //output = (kp * (float)error); + if (kp < 31) output = FX_INT(FX_MUL(FL_FX(kp), INT_FX(error))); + else output = FX_INT(FX_MUL(FL__FX(kp), INT_FX(error))); + } + else output = 0; + + /*Compute Rest of PID output*/ + /* output += outputSum - (kd * dInput); */ + //output += outputSum - (kd * dInput); + output += outputSum - FX_INT(FX_MUL(FL_FX(kd), INT_FX(dInput))); + + if (output > outMax) output = outMax; + else if (output < outMin) output = outMin; + *myOutput = output; + + /*Remember some variables for next time*/ + lastInput = input; + lastTime = now; + return true; + } + else return false; +} + +/* SetTunings(....)************************************************************ + This function allows the controller's dynamic performance to be adjusted. + it's called automatically from the constructor, but tunings can also + be adjusted on the fly during normal operation + ******************************************************************************/ +void QuickPID::SetTunings(float Kp, float Ki, float Kd, bool POn) +{ + if (Kp < 0 || Ki < 0 || Kd < 0) return; + + pOn = POn; + pOnE = POn == P_ON_E; + dispKp = Kp; dispKi = Ki; dispKd = Kd; + + float SampleTimeSec = ((float)SampleTimeUs) / 1000000; + kp = Kp; + ki = Ki * SampleTimeSec; + kd = Kd / SampleTimeSec; + + if (controllerDirection == REVERSE) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } +} + +/* SetTunings(...)************************************************************* + Set Tunings using the last-rembered POn setting + ******************************************************************************/ +void QuickPID::SetTunings(float Kp, float Ki, float Kd) { + SetTunings(Kp, Ki, Kd, pOn); +} + +/* SetSampleTime(...) ********************************************************* + Sets the period, in microseconds, at which the calculation is performed + ******************************************************************************/ +void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) +{ + if (NewSampleTimeUs > 0) + { + float ratio = (float)NewSampleTimeUs / (float)SampleTimeUs; + //float ratio = FX_FL(FX_DIV(FL_FX(NewSampleTimeUs), FL_FX(SampleTimeUs))); + + ki *= ratio; + //ki = FX_FL(FX_MUL(FL_FX(ki), FL_FX(ratio))); + + kd /= ratio; + //kd = FX_FL(FX_DIV(FL_FX(kd), FL_FX(ratio))); + //---------------------------------------------------------------------------------------------------------------------------- + SampleTimeUs = (uint32_t)NewSampleTimeUs; + } +} + +/* SetOutputLimits(...)**************************************************** + This function will be used far more often than SetInputLimits. While + the input to the controller will generally be in the 0-1023 range, which is + the default already, the required output limits might be unique, like using + a time window of 0-8000 needing to clamp it from 0-125. + **************************************************************************/ +void QuickPID::SetOutputLimits(uint8_t Min, uint8_t Max) +{ + if (Min >= Max) return; + outMin = Min; + outMax = Max; + + if (inAuto) + { + if (*myOutput > outMax) *myOutput = outMax; + else if (*myOutput < outMin) *myOutput = outMin; + + if (outputSum > outMax) outputSum = outMax; + else if (outputSum < outMin) outputSum = outMin; + } +} + +/* SetMode(...)**************************************************************** + Allows the controller Mode to be set to manual (0) or Automatic (non-zero) + when the transition from manual to auto occurs, the controller is + automatically initialized + ******************************************************************************/ +void QuickPID::SetMode(bool Mode) +{ + bool newAuto = (Mode == AUTOMATIC); + if (newAuto && !inAuto) + { /*we just went from manual to auto*/ + QuickPID::Initialize(); + } + inAuto = newAuto; +} + +/* Initialize()**************************************************************** + Does all the things that need to happen to ensure a bumpless transfer + from manual to automatic mode. + ******************************************************************************/ +void QuickPID::Initialize() +{ + outputSum = *myOutput; + lastInput = *myInput; + if (outputSum > outMax) outputSum = outMax; + else if (outputSum < outMin) outputSum = outMin; +} + +/* SetControllerDirection(...)************************************************* + The PID will either be connected to a DIRECT acting process (+Output leads + to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to + know which one, because otherwise we may increase the output when we should + be decreasing. This is called from the constructor. + ******************************************************************************/ +void QuickPID::SetControllerDirection(bool Direction) +{ + if (inAuto && Direction != controllerDirection) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } + controllerDirection = Direction; +} + +/* Status Functions************************************************************ + Just because you set the Kp=-1 doesn't mean it actually happened. These + functions query the internal state of the PID. They're here for display + purposes. These are the functions the PID Front-end uses for example. + ******************************************************************************/ +float QuickPID::GetKp() { + return dispKp; +} +float QuickPID::GetKi() { + return dispKi; +} +float QuickPID::GetKd() { + return dispKd; +} +int16_t QuickPID::GetError() { + return error; +} +bool QuickPID::GetMode() { + return inAuto ? AUTOMATIC : MANUAL; +} +bool QuickPID::GetDirection() { + return controllerDirection; +} + +// Utility functions ******************************************************************************** + +int QuickPID::analogReadFast(int ADCpin) { +#if defined(__AVR_ATmega328P__) + byte ADCregOriginal = ADCSRA; + ADCSRA = (ADCSRA & B11111000) | 6; //64 prescaler + int adc = analogRead(ADCpin); + ADCSRA = ADCregOriginal; + return adc; +#elif defined(__AVR_ATtiny_Zero_One__) || defined(__AVR_ATmega_Zero__) + byte ADCregOriginal = ADC0_CTRLC; + ADC0_CTRLC = 0x55; //reduced cap, Vdd ref, 64 prescaler + int adc = analogRead(ADCpin); + ADC0_CTRLC = ADCregOriginal; + return adc; +#elif defined(__AVR_DA__) + byte ADCregOriginal = ADC0.CTRLC; + ADC0.CTRLC = ADC_PRESC_DIV64_gc; //64 prescaler + int adc = analogRead(ADCpin); + ADC0.CTRLC = ADCregOriginal; + return adc; +#else + return analogRead(ADCpin); +# endif +} diff --git a/QuickPID.h b/QuickPID.h new file mode 100644 index 0000000..c2ef101 --- /dev/null +++ b/QuickPID.h @@ -0,0 +1,100 @@ +#ifndef QuickPID_h +#define QuickPID_h + +class QuickPID +{ + + public: + + //Constants used in some of the functions below +#define AUTOMATIC 1 +#define MANUAL 0 +#define DIRECT 0 +#define REVERSE 1 +#define P_ON_M 0 +#define P_ON_E 1 + + // s23.8 fixed point defines +#define FL__FX(a) (int64_t)(a*256.0) // float to fixed point +#define FL_FX(a) (int32_t)(a*256.0) // float to fixed point +#define FX_FL(a) (float)(a/256.0) // fixed point to float +#define INT_FX(a) (a<<8) // integer to fixed point +#define FX_INT(a) (int32_t)(a>>8) // fixed point to integer +#define FX_ADD(a,b) (a+b) // fixed point add +#define FX_SUB(a,b) (a-b) // fixed point subtract +#define FX_MUL(a,b) ((a*b)>>8) // fixed point multiply +#define FX_DIV(a,b) ((a/b)<<8) // fixed point divide + + + // commonly used functions ************************************************************************************ + + // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. + QuickPID(int16_t*, uint8_t*, int16_t*, float, float, float, bool, bool); + + // Overload constructor with proportional mode. Links the PID to Input, Output, Setpoint and Tuning Parameters. + QuickPID(int16_t*, uint8_t*, int16_t*, float, float, float, bool); + + // Sets PID to either Manual (0) or Auto (non-0). + void SetMode(bool Mode); + + // Performs the PID calculation. It should be called every time loop() cycles. ON/OFF and calculation frequency + // can be set using SetMode and SetSampleTime respectively. + bool Compute(); + + // Clamps the output to a specific range. 0-255 by default, but it's likely the user will want to change this + // depending on the application. + void SetOutputLimits(uint8_t, uint8_t); + + // available but not commonly used functions ****************************************************************** + + // While most users will set the tunings once in the constructor, this function gives the user the option of + // changing tunings during runtime for Adaptive control. + void SetTunings(float, float, float); + + // Overload for specifying proportional mode. + void SetTunings(float, float, float, bool); + + // Sets the Direction, or "Action" of control. DIRECT means the output will increase when error is positive. + // REVERSE means the opposite. It's very unlikely that this will be needed once it is set in the constructor. + void SetControllerDirection(bool); + + // Sets the sample time in milliseconds with which each PID calculation is performed. Default is 100. + void SetSampleTimeUs(uint32_t); + + //Display functions ****************************************************************************************** + float GetKp(); // These functions query the pid for interal values. They were created mainly for + float GetKi(); // the pid front-end, where it's important to know what is actually inside the PID. + float GetKd(); + int16_t GetError(); + bool GetMode(); + bool GetDirection(); + + // Utility functions ****************************************************************************************** + int analogReadFast(int); + + private: + void Initialize(); + + float dispKp; // We'll hold on to the tuning parameters in user-entered format for display purposes. + float dispKi; + float dispKd; + + float kp; // (P)roportional Tuning Parameter + float ki; // (I)ntegral Tuning Parameter + float kd; // (D)erivative Tuning Parameter + + bool controllerDirection; + bool pOn; + + int16_t *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a + uint8_t *myOutput; // hard link between the variables and the PID, freeing the user from having + int16_t *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. + + uint32_t SampleTimeUs, lastTime; + int16_t lastInput, outputSum; + uint8_t outMin, outMax; + bool inAuto, pOnE; + int32_t error; +}; + +#endif diff --git a/README.md b/README.md new file mode 100644 index 0000000..f9d311b --- /dev/null +++ b/README.md @@ -0,0 +1,60 @@ +# QuickPID + +### API + +The API follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: + +- Library named as **QuickPID** and can run concurrently with Arduino **PID** +- Fixed-point calculations are used in `Compute()` to improve performance +- `analogReadFast()` added to provide further performance improvement +- `GetError()`added for diagnostics + +### Performance + +PID performance varies depending on how many coefficients are used. When a coefficient is zero, less calculation is done. The controller was benchmarked using an Arduino UNO. QuickPID was benchmarked using analogReadFast() code. + +| P_ON_M Compute() | Kp | Ki | Kd | Step Time (µS) | +| ---------------------------------------------------------- | ---- | ---- | ---- | -------------- | +| QuickPID | 2.0 | 5.0 | 0.0 | 88 | +| Arduino PID | 2.0 | 5.0 | 0.0 | 104 | +| **P_ON_M `analogRead(), `Compute(),** **analogWrite()** | | | | | +| QuickPID | 2.0 | 5.0 | 0.2 | 164 | +| Arduino PID | 2.0 | 5.0 | 0.2 | 224 | + +### Execution Frequency + +A future version will provide further performance improvements by pre-calculating (scaling) the terms and providing direct timer with ISR support. + +#### Variables + + +| Variable | Arduino PID | QuickPID | Data Type | Resolution | Bits Used | Min | Max | +| ------------ | ----------- | ---------------- | ------------ | ---------- | --------- | ----- | ---------- | +| Setpoint | double | int16_t | Binary | 1 | 10 | 0 | 1023 | +| Input | double | int16_t | Binary | 1 | 10 | 0 | 1023 | +| Output | double | uint8_t | Binary | 1 | 8 | 0 | 255 | +| Kp | double | int32_t, int64_t | s23.8, s55.8 | 0.00390625 | 10.8 | <-1m | >1m | +| Ki | double | int32_t, int64_t | s23.8, s55.8 | 0.00390625 | 10.8 | <-1m | >1m | +| Kd | double | int32_t | s23.8 | 0.00390625 | 5.8 | -32 | 31.984375 | +| ratio | double | int32_t | s23.8 | 0.00390625 | 5.8 | -32 | 31.984375 | +| SampleTimeUs | double | uint32_t | Binary | 1 | 32 | 0 | 4294967295 | +| outputSum | double | int16_t | Binary | 1 | 10 | 0 | 1023 | +| error | double | int32_t | Binary | 1 | s10 | -1023 | 1023 | +| dInput | double | int32_t | Binary | 1 | s10 | -1023 | 1023 | + +### Original README + +``` +*************************************************************** +* Arduino PID Library - Version 1.2.1 +* by Brett Beauregard brettbeauregard.com +* +* This Library is licensed under the MIT License +*************************************************************** +``` + + - For an ultra-detailed explanation of why the code is the way it is, please visit: + http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ + + - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary + diff --git a/README.txt b/README.txt deleted file mode 100644 index 3f2fb63..0000000 --- a/README.txt +++ /dev/null @@ -1,11 +0,0 @@ -*************************************************************** -* Arduino PID Library - Version 1.2.1 -* by Brett Beauregard brettbeauregard.com -* -* This Library is licensed under the MIT License -*************************************************************** - - - For an ultra-detailed explanation of why the code is the way it is, please visit: - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ - - - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary diff --git a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino deleted file mode 100644 index 94f3faf..0000000 --- a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino +++ /dev/null @@ -1,56 +0,0 @@ -/******************************************************** - * PID Adaptive Tuning Example - * One of the benefits of the PID library is that you can - * change the tuning parameters at any time. this can be - * helpful if we want the controller to be agressive at some - * times, and conservative at others. in the example below - * we set the controller to use Conservative Tuning Parameters - * when we're near setpoint and more agressive Tuning - * Parameters when we're farther away. - ********************************************************/ - -#include - -#define PIN_INPUT 0 -#define PIN_OUTPUT 3 - -//Define Variables we'll be connecting to -double Setpoint, Input, Output; - -//Define the aggressive and conservative Tuning Parameters -double aggKp=4, aggKi=0.2, aggKd=1; -double consKp=1, consKi=0.05, consKd=0.25; - -//Specify the links and initial tuning parameters -PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT); - -void setup() -{ - //initialize the variables we're linked to - Input = analogRead(PIN_INPUT); - Setpoint = 100; - - //turn the PID on - myPID.SetMode(AUTOMATIC); -} - -void loop() -{ - Input = analogRead(PIN_INPUT); - - double gap = abs(Setpoint-Input); //distance away from setpoint - if (gap < 10) - { //we're close to setpoint, use conservative tuning parameters - myPID.SetTunings(consKp, consKi, consKd); - } - else - { - //we're far from setpoint, use aggressive tuning parameters - myPID.SetTunings(aggKp, aggKi, aggKd); - } - - myPID.Compute(); - analogWrite(PIN_OUTPUT, Output); -} - - diff --git a/examples/PID_Basic/PID_Basic.ino b/examples/PID_Basic/PID_Basic.ino deleted file mode 100644 index 8028b58..0000000 --- a/examples/PID_Basic/PID_Basic.ino +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************** - * PID Basic Example - * Reading analog input 0 to control analog PWM output 3 - ********************************************************/ - -#include - -#define PIN_INPUT 0 -#define PIN_OUTPUT 3 - -//Define Variables we'll be connecting to -double Setpoint, Input, Output; - -//Specify the links and initial tuning parameters -double Kp=2, Ki=5, Kd=1; -PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); - -void setup() -{ - //initialize the variables we're linked to - Input = analogRead(PIN_INPUT); - Setpoint = 100; - - //turn the PID on - myPID.SetMode(AUTOMATIC); -} - -void loop() -{ - Input = analogRead(PIN_INPUT); - myPID.Compute(); - analogWrite(PIN_OUTPUT, Output); -} - - diff --git a/examples/PID_PonM/PID_PonM.ino b/examples/PID_PonM/PID_PonM.ino deleted file mode 100644 index 121c161..0000000 --- a/examples/PID_PonM/PID_PonM.ino +++ /dev/null @@ -1,36 +0,0 @@ -/******************************************************** - * PID Proportional on measurement Example - * Setting the PID to use Proportional on measurement will - * make the output move more smoothly when the setpoint - * is changed. In addition, it can eliminate overshoot - * in certain processes like sous-vides. - ********************************************************/ - -#include - -//Define Variables we'll be connecting to -double Setpoint, Input, Output; - -//Specify the links and initial tuning parameters -PID myPID(&Input, &Output, &Setpoint,2,5,1,P_ON_M, DIRECT); //P_ON_M specifies that Proportional on Measurement be used - //P_ON_E (Proportional on Error) is the default behavior - -void setup() -{ - //initialize the variables we're linked to - Input = analogRead(0); - Setpoint = 100; - - //turn the PID on - myPID.SetMode(AUTOMATIC); -} - -void loop() -{ - Input = analogRead(0); - myPID.Compute(); - analogWrite(3,Output); -} - - - diff --git a/examples/PID_RelayOutput/PID_RelayOutput.ino b/examples/PID_RelayOutput/PID_RelayOutput.ino deleted file mode 100644 index 17fbe1a..0000000 --- a/examples/PID_RelayOutput/PID_RelayOutput.ino +++ /dev/null @@ -1,64 +0,0 @@ -/******************************************************** - * PID RelayOutput Example - * Same as basic example, except that this time, the output - * is going to a digital pin which (we presume) is controlling - * a relay. the pid is designed to Output an analog value, - * but the relay can only be On/Off. - * - * to connect them together we use "time proportioning - * control" it's essentially a really slow version of PWM. - * first we decide on a window size (5000mS say.) we then - * set the pid to adjust its output between 0 and that window - * size. lastly, we add some logic that translates the PID - * output into "Relay On Time" with the remainder of the - * window being "Relay Off Time" - ********************************************************/ - -#include - -#define PIN_INPUT 0 -#define RELAY_PIN 6 - -//Define Variables we'll be connecting to -double Setpoint, Input, Output; - -//Specify the links and initial tuning parameters -double Kp=2, Ki=5, Kd=1; -PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); - -int WindowSize = 5000; -unsigned long windowStartTime; - -void setup() -{ - windowStartTime = millis(); - - //initialize the variables we're linked to - Setpoint = 100; - - //tell the PID to range between 0 and the full window size - myPID.SetOutputLimits(0, WindowSize); - - //turn the PID on - myPID.SetMode(AUTOMATIC); -} - -void loop() -{ - Input = analogRead(PIN_INPUT); - myPID.Compute(); - - /************************************************ - * turn the output pin on/off based on pid output - ************************************************/ - if (millis() - windowStartTime > WindowSize) - { //time to shift the Relay Window - windowStartTime += WindowSize; - } - if (Output < millis() - windowStartTime) digitalWrite(RELAY_PIN, HIGH); - else digitalWrite(RELAY_PIN, LOW); - -} - - - diff --git a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino new file mode 100644 index 0000000..8e8b42b --- /dev/null +++ b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino @@ -0,0 +1,69 @@ +/************************************************************ + PID RC Filter Self Test Example: + One 47µF capacitor connected from GND to a 27K resistor + terminated at pwm pin 3. Junction point of the RC filter + is connected to A0. Use Serial Plotter to view results. + ************************************************************/ + +#include "QuickPID.h" + +#define PIN_INPUT A0 +#define PIN_OUTPUT 3 + +//Define Variables +int16_t Setpoint = 700; +int16_t Input; +uint8_t Output; + +uint32_t before, after; +uint16_t cnt = 0; + +//Specify the initial tuning parameters +float Kp = 2.0, Ki = 5.0, Kd = 0.0; + +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); + +void setup() +{ + myQuickPID.SetTunings(Kp, Ki, Kd, P_ON_M); + Serial.begin(115200); + analogWrite(PIN_OUTPUT, 0); + delay(1000); // discharge capacitor + Input = myQuickPID.analogReadFast(PIN_INPUT); + myQuickPID.SetMode(AUTOMATIC); +} + +void loop() +{ + //Serial.println("Min:0,Max:1000"); // set scale + + // Stretch the plot x2 + for (int i = 0; i <= 1; i++) { + Serial.print("Setpoint:"); + Serial.print(Setpoint); + Serial.print(","); + Serial.print("Input:"); + Serial.print(Input); + Serial.print(","); + Serial.print("Output:"); + Serial.print(Output); + Serial.print(","); + Serial.print("Runtime:"); + Serial.print(after - before); + Serial.println(","); + } + + Input = myQuickPID.analogReadFast(PIN_INPUT); + before = micros(); + myQuickPID.Compute(); + after = micros(); + analogWrite(PIN_OUTPUT, Output); + + delay(110); + cnt++; + if (cnt == 100) { + analogWrite(PIN_OUTPUT, 0); + delay(1000); // discharge capacitor + cnt = 0; + } +} diff --git a/keywords.txt b/keywords.txt index 330d7fc..6616654 100644 --- a/keywords.txt +++ b/keywords.txt @@ -1,32 +1,34 @@ -####################################### -# Syntax Coloring Map For PID Library -####################################### +########################################## +# Syntax Coloring Map For QuickPID Library +########################################## -####################################### +########################################## # Datatypes (KEYWORD1) -####################################### +########################################## -PID KEYWORD1 +QuickPID KEYWORD1 -####################################### +########################################## # Methods and Functions (KEYWORD2) -####################################### +########################################## SetMode KEYWORD2 Compute KEYWORD2 SetOutputLimits KEYWORD2 SetTunings KEYWORD2 SetControllerDirection KEYWORD2 -SetSampleTime KEYWORD2 +SetSampleTimeUs KEYWORD2 GetKp KEYWORD2 GetKi KEYWORD2 GetKd KEYWORD2 GetMode KEYWORD2 +GetError KEYWORD2 GetDirection KEYWORD2 +analogReadFast KEYWORD2 -####################################### +########################################## # Constants (LITERAL1) -####################################### +########################################## AUTOMATIC LITERAL1 MANUAL LITERAL1 diff --git a/library.json b/library.json index cf2510c..4d76c6e 100644 --- a/library.json +++ b/library.json @@ -1,19 +1,19 @@ { - "name": "PID", + "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "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).", - "url": "http://playground.arduino.cc/Code/PIDLibrary", - "include": "PID_v1", + "description": "A fast, fixed point PID controller that seeks to keep an 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).", + "url": "https://github.com/Dlloydev/QuickPID", + "include": "QuickPID", "authors": [ { - "name": "Brett Beauregard" + "name": "dlloydev" } ], "repository": { "type": "git", - "url": "https://github.com/br3ttb/Arduino-PID-Library.git" + "url": "https://github.com/Dlloydev/QuickPID" }, "frameworks": "arduino" } diff --git a/library.properties b/library.properties index 4b3fadf..ef3f50e 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ -name=PID -version=1.2.1 -author=Brett Beauregard -maintainer=Brett Beauregard -sentence=PID controller -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). +name=QuickPID +version=2.0.0 +author=dlloydev +maintainer=dlloydev +sentence=QuickPID controller +paragraph=A fast, fixed point PID controller that seeks to keep an 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). category=Signal Input/Output -url=http://playground.arduino.cc/Code/PIDLibrary +url=https://github.com/Dlloydev/QuickPID architectures=* From 268bddb09e1b0ca77cc827ae366eacc73dbce2e8 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 6 Jan 2021 22:25:28 -0500 Subject: [PATCH 002/101] Updare Readme --- QuickPID.cpp | 2 +- README.md | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 0b808ef..f647272 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - Arduino QuickPID Library for Arduino - Version 2.0.0 + QuickPID Library for Arduino - Version 2.0.0 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard diff --git a/README.md b/README.md index f9d311b..8bcfa07 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,5 @@ # QuickPID -### API - The API follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: - Library named as **QuickPID** and can run concurrently with Arduino **PID** @@ -53,8 +51,7 @@ A future version will provide further performance improvements by pre-calculatin *************************************************************** ``` - - For an ultra-detailed explanation of why the code is the way it is, please visit: + - For an ultra-detailed explanation of why the code is the way it is, please visit: http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary - From 9c798fdae2a4d9415c6f63cdea651a8fbe182b7f Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Thu, 7 Jan 2021 01:31:29 -0500 Subject: [PATCH 003/101] Minor formatting --- QuickPID.cpp | 12 ++++-------- QuickPID.h | 4 ++-- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index f647272..4863f83 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -27,7 +27,7 @@ QuickPID::QuickPID(int16_t* Input, uint8_t* Output, int16_t* Setpoint, mySetpoint = Setpoint; inAuto = false; - QuickPID::SetOutputLimits(0, 255); // default is same as the arduino PWM limit + QuickPID::SetOutputLimits(0, 255); // default is same as the arduino PWM limit SampleTimeUs = 100000; // default is 0.1 seconds QuickPID::SetControllerDirection(ControllerDirection); @@ -150,24 +150,20 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) if (NewSampleTimeUs > 0) { float ratio = (float)NewSampleTimeUs / (float)SampleTimeUs; - //float ratio = FX_FL(FX_DIV(FL_FX(NewSampleTimeUs), FL_FX(SampleTimeUs))); ki *= ratio; - //ki = FX_FL(FX_MUL(FL_FX(ki), FL_FX(ratio))); - kd /= ratio; - //kd = FX_FL(FX_DIV(FL_FX(kd), FL_FX(ratio))); - //---------------------------------------------------------------------------------------------------------------------------- + SampleTimeUs = (uint32_t)NewSampleTimeUs; } } -/* SetOutputLimits(...)**************************************************** +/* SetOutputLimits(...)******************************************************** This function will be used far more often than SetInputLimits. While the input to the controller will generally be in the 0-1023 range, which is the default already, the required output limits might be unique, like using a time window of 0-8000 needing to clamp it from 0-125. - **************************************************************************/ + ******************************************************************************/ void QuickPID::SetOutputLimits(uint8_t Min, uint8_t Max) { if (Min >= Max) return; diff --git a/QuickPID.h b/QuickPID.h index c2ef101..ee76a2e 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -62,8 +62,8 @@ class QuickPID void SetSampleTimeUs(uint32_t); //Display functions ****************************************************************************************** - float GetKp(); // These functions query the pid for interal values. They were created mainly for - float GetKi(); // the pid front-end, where it's important to know what is actually inside the PID. + float GetKp(); // These functions query the pid for interal values. They were created mainly for + float GetKi(); // the pid front-end, where it's important to know what is actually inside the PID. float GetKd(); int16_t GetError(); bool GetMode(); From f3227e0108686d2959ce1e038587ccd30dcfccc3 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Thu, 7 Jan 2021 01:50:26 -0500 Subject: [PATCH 004/101] Minor cleanup --- QuickPID.cpp | 6 +----- README.md | 16 ++++++++-------- 2 files changed, 9 insertions(+), 13 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 4863f83..1616faa 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -67,14 +67,12 @@ bool QuickPID::Compute() int32_t dInput = (int32_t)input - (int32_t)lastInput; /* outputSum+= (ki * error); */ - //outputSum += (ki * error); if (ki < 31) outputSum += FX_INT(FX_MUL(FL_FX(ki), INT_FX(error))); else outputSum += FX_INT(FX_MUL(FL__FX(ki), INT_FX(error))); /*Add Proportional on Measurement, if P_ON_M is specified*/ /* outputSum -= kp * dInput; */ if (!pOnE) { - //outputSum -= kp * dInput; if (kp < 31) outputSum -= FX_INT(FX_MUL(FL_FX(kp), INT_FX(dInput))); else outputSum -= FX_INT(FX_MUL(FL__FX(kp), INT_FX(dInput))); } @@ -86,7 +84,6 @@ bool QuickPID::Compute() /* if (pOnE) output = (kp * (float)error); */ uint16_t output; if (pOnE) { - //output = (kp * (float)error); if (kp < 31) output = FX_INT(FX_MUL(FL_FX(kp), INT_FX(error))); else output = FX_INT(FX_MUL(FL__FX(kp), INT_FX(error))); } @@ -94,7 +91,6 @@ bool QuickPID::Compute() /*Compute Rest of PID output*/ /* output += outputSum - (kd * dInput); */ - //output += outputSum - (kd * dInput); output += outputSum - FX_INT(FX_MUL(FL_FX(kd), INT_FX(dInput))); if (output > outMax) output = outMax; @@ -248,7 +244,7 @@ bool QuickPID::GetDirection() { return controllerDirection; } -// Utility functions ******************************************************************************** +// Utility functions ********************************************************** int QuickPID::analogReadFast(int ADCpin) { #if defined(__AVR_ATmega328P__) diff --git a/README.md b/README.md index 8bcfa07..80011f8 100644 --- a/README.md +++ b/README.md @@ -11,19 +11,19 @@ The API follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) PID performance varies depending on how many coefficients are used. When a coefficient is zero, less calculation is done. The controller was benchmarked using an Arduino UNO. QuickPID was benchmarked using analogReadFast() code. -| P_ON_M Compute() | Kp | Ki | Kd | Step Time (µS) | -| ---------------------------------------------------------- | ---- | ---- | ---- | -------------- | -| QuickPID | 2.0 | 5.0 | 0.0 | 88 | -| Arduino PID | 2.0 | 5.0 | 0.0 | 104 | -| **P_ON_M `analogRead(), `Compute(),** **analogWrite()** | | | | | -| QuickPID | 2.0 | 5.0 | 0.2 | 164 | -| Arduino PID | 2.0 | 5.0 | 0.2 | 224 | +| P_ON_M Compute | Kp | Ki | Kd | Step Time (µS) | +| :-------------------------------------------- | ---- | ---- | ---- | -------------- | +| QuickPID | 2.0 | 5.0 | 0.0 | 88 | +| Arduino PID | 2.0 | 5.0 | 0.0 | 104 | +| **P_ON_M analogRead, Compute, analogWrite** | | | | | +| QuickPID | 2.0 | 5.0 | 0.2 | 164 | +| Arduino PID | 2.0 | 5.0 | 0.2 | 224 | ### Execution Frequency A future version will provide further performance improvements by pre-calculating (scaling) the terms and providing direct timer with ISR support. -#### Variables +### Variables | Variable | Arduino PID | QuickPID | Data Type | Resolution | Bits Used | Min | Max | From d43d07f68dafa22e4dc311be6bd1e2934d1d2195 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 8 Jan 2021 22:58:20 -0500 Subject: [PATCH 005/101] New version 2.01, faster PID algorithms --- QuickPID.cpp | 51 +++++++---------- QuickPID.h | 7 +-- README.md | 55 +++++++++---------- .../QuickPID_Self_Test/QuickPID_Self_Test.ino | 8 +-- library.properties | 2 +- 5 files changed, 52 insertions(+), 71 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 1616faa..803c2d7 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -14,7 +14,6 @@ #include "QuickPID.h" - /* Constructor ******************************************************************** The parameters specified here are those for for which we can't set up reliable defaults, so we need to have the user set them. @@ -63,39 +62,26 @@ bool QuickPID::Compute() { /*Compute all the working error variables*/ int16_t input = *myInput; - error = (*mySetpoint - input); - int32_t dInput = (int32_t)input - (int32_t)lastInput; - - /* outputSum+= (ki * error); */ - if (ki < 31) outputSum += FX_INT(FX_MUL(FL_FX(ki), INT_FX(error))); - else outputSum += FX_INT(FX_MUL(FL__FX(ki), INT_FX(error))); + int16_t dInput = input - lastInput; + error = *mySetpoint - input; - /*Add Proportional on Measurement, if P_ON_M is specified*/ - /* outputSum -= kp * dInput; */ + /*Working error, Proportional on Measurement and Remaining PID output*/ if (!pOnE) { - if (kp < 31) outputSum -= FX_INT(FX_MUL(FL_FX(kp), INT_FX(dInput))); - else outputSum -= FX_INT(FX_MUL(FL__FX(kp), INT_FX(dInput))); + if (ki < 31) outputSum += FX_INT(FX_MUL(FL_FX(ki), INT_FX(error))); + else outputSum += (ki * error); + if (kpd < 31) outputSum -= FX_INT(FX_MUL(FL_FX(kpd), INT_FX(dInput))); + else outputSum -= (kpd * dInput); } - - if (outputSum > outMax) outputSum = outMax; - else if (outputSum < outMin) outputSum = outMin; - - /*Add Proportional on Error, if P_ON_E is specified*/ - /* if (pOnE) output = (kp * (float)error); */ - uint16_t output; + /*Working error, Proportional on Error and remaining PID output*/ if (pOnE) { - if (kp < 31) output = FX_INT(FX_MUL(FL_FX(kp), INT_FX(error))); - else output = FX_INT(FX_MUL(FL__FX(kp), INT_FX(error))); + if (kpi < 31) outputSum += FX_INT(FX_MUL(FL_FX(kpi), INT_FX(error))); + else outputSum += (kpi * error); + if (kd < 31) outputSum -= FX_INT(FX_MUL(FL_FX(kd), INT_FX(dInput))); + else outputSum -= (kd * dInput); } - else output = 0; - - /*Compute Rest of PID output*/ - /* output += outputSum - (kd * dInput); */ - output += outputSum - FX_INT(FX_MUL(FL_FX(kd), INT_FX(dInput))); - - if (output > outMax) output = outMax; - else if (output < outMin) output = outMin; - *myOutput = output; + if (outputSum > outMax) outputSum = outMax; + if (outputSum < outMin) outputSum = outMin; + *myOutput = outputSum; /*Remember some variables for next time*/ lastInput = input; @@ -118,10 +104,12 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, bool POn) pOnE = POn == P_ON_E; dispKp = Kp; dispKi = Ki; dispKd = Kd; - float SampleTimeSec = ((float)SampleTimeUs) / 1000000; + float SampleTimeSec = (float)SampleTimeUs / 1000000; kp = Kp; ki = Ki * SampleTimeSec; kd = Kd / SampleTimeSec; + kpd = kp + kd; + kpi = kp + ki; if (controllerDirection == REVERSE) { @@ -146,11 +134,10 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) if (NewSampleTimeUs > 0) { float ratio = (float)NewSampleTimeUs / (float)SampleTimeUs; - ki *= ratio; kd /= ratio; - SampleTimeUs = (uint32_t)NewSampleTimeUs; + SampleTimeUs = NewSampleTimeUs; } } diff --git a/QuickPID.h b/QuickPID.h index ee76a2e..d70de25 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -14,14 +14,11 @@ class QuickPID #define P_ON_M 0 #define P_ON_E 1 - // s23.8 fixed point defines -#define FL__FX(a) (int64_t)(a*256.0) // float to fixed point + // fixed point defines #define FL_FX(a) (int32_t)(a*256.0) // float to fixed point #define FX_FL(a) (float)(a/256.0) // fixed point to float #define INT_FX(a) (a<<8) // integer to fixed point #define FX_INT(a) (int32_t)(a>>8) // fixed point to integer -#define FX_ADD(a,b) (a+b) // fixed point add -#define FX_SUB(a,b) (a-b) // fixed point subtract #define FX_MUL(a,b) ((a*b)>>8) // fixed point multiply #define FX_DIV(a,b) ((a/b)<<8) // fixed point divide @@ -82,6 +79,8 @@ class QuickPID float kp; // (P)roportional Tuning Parameter float ki; // (I)ntegral Tuning Parameter float kd; // (D)erivative Tuning Parameter + float kpd = kp + kd; + float kpi = kp + ki; bool controllerDirection; bool pOn; diff --git a/README.md b/README.md index 80011f8..b09dcd6 100644 --- a/README.md +++ b/README.md @@ -1,44 +1,39 @@ # QuickPID -The API follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: +This API (version 2.01) follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: - Library named as **QuickPID** and can run concurrently with Arduino **PID** -- Fixed-point calculations are used in `Compute()` to improve performance -- `analogReadFast()` added to provide further performance improvement -- `GetError()`added for diagnostics +- Reorganized and more efficient PID algorithms +- Fast fixed-point calculations for smaller coefficients, floating point calculations for larger coefficients +- Faster analog read function +- `GetError()`function added for diagnostics ### Performance -PID performance varies depending on how many coefficients are used. When a coefficient is zero, less calculation is done. The controller was benchmarked using an Arduino UNO. QuickPID was benchmarked using analogReadFast() code. - -| P_ON_M Compute | Kp | Ki | Kd | Step Time (µS) | -| :-------------------------------------------- | ---- | ---- | ---- | -------------- | -| QuickPID | 2.0 | 5.0 | 0.0 | 88 | -| Arduino PID | 2.0 | 5.0 | 0.0 | 104 | -| **P_ON_M analogRead, Compute, analogWrite** | | | | | -| QuickPID | 2.0 | 5.0 | 0.2 | 164 | -| Arduino PID | 2.0 | 5.0 | 0.2 | 224 | - -### Execution Frequency - -A future version will provide further performance improvements by pre-calculating (scaling) the terms and providing direct timer with ISR support. +| Compute | Kp | Ki | Kd | Step Time (µS) | +| :----------------------------------- | ---- | ---- | ---- | -------------- | +| QuickPID | 2.0 | 15.0 | 0.05 | 72 | +| Arduino PID | 2.0 | 15.0 | 0.05 | 104 | +| **analogRead, Compute, analogWrite** | | | | | +| QuickPID | 2.0 | 5.0 | 0.2 | 132 | +| Arduino PID | 2.0 | 5.0 | 0.2 | 224 | ### Variables -| Variable | Arduino PID | QuickPID | Data Type | Resolution | Bits Used | Min | Max | -| ------------ | ----------- | ---------------- | ------------ | ---------- | --------- | ----- | ---------- | -| Setpoint | double | int16_t | Binary | 1 | 10 | 0 | 1023 | -| Input | double | int16_t | Binary | 1 | 10 | 0 | 1023 | -| Output | double | uint8_t | Binary | 1 | 8 | 0 | 255 | -| Kp | double | int32_t, int64_t | s23.8, s55.8 | 0.00390625 | 10.8 | <-1m | >1m | -| Ki | double | int32_t, int64_t | s23.8, s55.8 | 0.00390625 | 10.8 | <-1m | >1m | -| Kd | double | int32_t | s23.8 | 0.00390625 | 5.8 | -32 | 31.984375 | -| ratio | double | int32_t | s23.8 | 0.00390625 | 5.8 | -32 | 31.984375 | -| SampleTimeUs | double | uint32_t | Binary | 1 | 32 | 0 | 4294967295 | -| outputSum | double | int16_t | Binary | 1 | 10 | 0 | 1023 | -| error | double | int32_t | Binary | 1 | s10 | -1023 | 1023 | -| dInput | double | int32_t | Binary | 1 | s10 | -1023 | 1023 | +| Variable | Arduino PID | QuickPID | Data Type | Resolution | Bits Used | Min | Max | +| ------------ | ----------- | -------------- | ------------ | -------------------- | --------- | ----------- | ----------- | +| Setpoint | double | int16_t | Binary | 1 | 10 | 0 | 1023 | +| Input | double | int16_t | Binary | 1 | 10 | 0 | 1023 | +| Output | double | uint8_t | Binary | 1 | 8 | 0 | 255 | +| Kp | double | int32_t, float | s23.8, float | 6-7 digits precision | 4 bytes | | | +| Ki | double | int32_t, float | s23.8, float | 6-7 digits precision | 4 bytes | | | +| Kd | double | int32_t, float | s23.8, float | 6-7 digits precision | 4 bytes | | | +| ratio | double | float | float | 6-7 digits precision | 4 bytes | | | +| SampleTimeUs | double | uint32_t | Binary | 1 | 32 | 0 | 4294967295 | +| outputSum | double | int16_t | Binary | 1 | 8 | 0 (limit) | 255 (limit) | +| error | double | int32_t | Binary | 1 | 32 | -2147483648 | 2147483647 | +| dInput | double | int32_t | Binary | 1 | 32 | -2147483648 | 2147483647 | ### Original README diff --git a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino index 8e8b42b..e080a42 100644 --- a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino +++ b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino @@ -19,7 +19,7 @@ uint32_t before, after; uint16_t cnt = 0; //Specify the initial tuning parameters -float Kp = 2.0, Ki = 5.0, Kd = 0.0; +float Kp = 2.0, Ki = 15.0, Kd = 0.05; QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); @@ -53,13 +53,13 @@ void loop() Serial.println(","); } - Input = myQuickPID.analogReadFast(PIN_INPUT); before = micros(); + Input = myQuickPID.analogReadFast(PIN_INPUT); myQuickPID.Compute(); - after = micros(); analogWrite(PIN_OUTPUT, Output); + after = micros(); - delay(110); + delay(50); cnt++; if (cnt == 100) { analogWrite(PIN_OUTPUT, 0); diff --git a/library.properties b/library.properties index ef3f50e..c463237 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.0.0 +version=2.0.1 author=dlloydev maintainer=dlloydev sentence=QuickPID controller From f1987c5f9f3b026da8b56f5ff7d52da0c3537895 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 9 Jan 2021 01:48:54 -0500 Subject: [PATCH 006/101] Update QuickPID.cpp --- QuickPID.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 803c2d7..ec52c56 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.0.0 + QuickPID Library for Arduino - Version 2.0.1 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard From f134ad8024d6d19775157c3f9ca3f15d279c5d05 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 9 Jan 2021 12:30:50 -0500 Subject: [PATCH 007/101] Version 2.02 --- QuickPID.cpp | 14 ++++----- QuickPID.h | 8 ----- README.md | 30 +++++++++---------- .../QuickPID_Self_Test/QuickPID_Self_Test.ino | 6 ++-- library.properties | 2 +- 5 files changed, 24 insertions(+), 36 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index ec52c56..6163a6f 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.0.1 + QuickPID Library for Arduino - Version 2.0.2 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard @@ -67,17 +67,13 @@ bool QuickPID::Compute() /*Working error, Proportional on Measurement and Remaining PID output*/ if (!pOnE) { - if (ki < 31) outputSum += FX_INT(FX_MUL(FL_FX(ki), INT_FX(error))); - else outputSum += (ki * error); - if (kpd < 31) outputSum -= FX_INT(FX_MUL(FL_FX(kpd), INT_FX(dInput))); - else outputSum -= (kpd * dInput); + outputSum += (ki * error); + outputSum -= (kpd * dInput); } /*Working error, Proportional on Error and remaining PID output*/ if (pOnE) { - if (kpi < 31) outputSum += FX_INT(FX_MUL(FL_FX(kpi), INT_FX(error))); - else outputSum += (kpi * error); - if (kd < 31) outputSum -= FX_INT(FX_MUL(FL_FX(kd), INT_FX(dInput))); - else outputSum -= (kd * dInput); + outputSum += (kpi * error); + outputSum -= (kd * dInput); } if (outputSum > outMax) outputSum = outMax; if (outputSum < outMin) outputSum = outMin; diff --git a/QuickPID.h b/QuickPID.h index d70de25..b25738c 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -14,14 +14,6 @@ class QuickPID #define P_ON_M 0 #define P_ON_E 1 - // fixed point defines -#define FL_FX(a) (int32_t)(a*256.0) // float to fixed point -#define FX_FL(a) (float)(a/256.0) // fixed point to float -#define INT_FX(a) (a<<8) // integer to fixed point -#define FX_INT(a) (int32_t)(a>>8) // fixed point to integer -#define FX_MUL(a,b) ((a*b)>>8) // fixed point multiply -#define FX_DIV(a,b) ((a/b)<<8) // fixed point divide - // commonly used functions ************************************************************************************ diff --git a/README.md b/README.md index b09dcd6..c37d0ea 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,10 @@ # QuickPID -This API (version 2.01) follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: +This API (version 2.02) follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: - Library named as **QuickPID** and can run concurrently with Arduino **PID** - Reorganized and more efficient PID algorithms -- Fast fixed-point calculations for smaller coefficients, floating point calculations for larger coefficients +- micros() timing resolution - Faster analog read function - `GetError()`function added for diagnostics @@ -21,19 +21,19 @@ This API (version 2.01) follows the [ArduinoPID](https://github.com/br3ttb/Ardui ### Variables -| Variable | Arduino PID | QuickPID | Data Type | Resolution | Bits Used | Min | Max | -| ------------ | ----------- | -------------- | ------------ | -------------------- | --------- | ----------- | ----------- | -| Setpoint | double | int16_t | Binary | 1 | 10 | 0 | 1023 | -| Input | double | int16_t | Binary | 1 | 10 | 0 | 1023 | -| Output | double | uint8_t | Binary | 1 | 8 | 0 | 255 | -| Kp | double | int32_t, float | s23.8, float | 6-7 digits precision | 4 bytes | | | -| Ki | double | int32_t, float | s23.8, float | 6-7 digits precision | 4 bytes | | | -| Kd | double | int32_t, float | s23.8, float | 6-7 digits precision | 4 bytes | | | -| ratio | double | float | float | 6-7 digits precision | 4 bytes | | | -| SampleTimeUs | double | uint32_t | Binary | 1 | 32 | 0 | 4294967295 | -| outputSum | double | int16_t | Binary | 1 | 8 | 0 (limit) | 255 (limit) | -| error | double | int32_t | Binary | 1 | 32 | -2147483648 | 2147483647 | -| dInput | double | int32_t | Binary | 1 | 32 | -2147483648 | 2147483647 | +| Variable | Arduino PID | QuickPID | Data Type | Min | Max | +| :----------- | :---------- | :------- | :-------- | :--------- | :--------- | +| Setpoint | double | int16_t | Binary | 0 | 1023 | +| Input | double | int16_t | Binary | 0 | 1023 | +| Output | double | uint8_t | Binary | 0 | 255 | +| Kp | double | float | float | -3.402E+38 | 3.402E+38 | +| Ki | double | float | float | -3.402E+38 | 3.402E+38 | +| Kd | double | float | float | -3.402E+38 | 3.402E+38 | +| ratio | double | float | float | -3.402E+38 | 3.402E+38 | +| SampleTimeUs | double | uint32_t | Binary | 0 | 4294967295 | +| outputSum | double | int16_t | Binary | 0 | 255 | +| error | double | int32_t | Binary | -1023 | 1023 | +| dInput | double | int32_t | Binary | -1023 | 1023 | ### Original README diff --git a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino index e080a42..bcf6814 100644 --- a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino +++ b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino @@ -53,13 +53,13 @@ void loop() Serial.println(","); } - before = micros(); Input = myQuickPID.analogReadFast(PIN_INPUT); + before = micros(); myQuickPID.Compute(); - analogWrite(PIN_OUTPUT, Output); after = micros(); + analogWrite(PIN_OUTPUT, Output); - delay(50); + delay(20); cnt++; if (cnt == 100) { analogWrite(PIN_OUTPUT, 0); diff --git a/library.properties b/library.properties index c463237..e863c7d 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.0.1 +version=2.0.2 author=dlloydev maintainer=dlloydev sentence=QuickPID controller From 7e48e779ba1d17b662c3f6a2071727d3ef7f4322 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 10 Jan 2021 01:00:15 -0500 Subject: [PATCH 008/101] Update variables and readme --- QuickPID.cpp | 6 ++-- QuickPID.h | 11 ++++--- README.md | 30 +++++++++++-------- .../QuickPID_Self_Test/QuickPID_Self_Test.ino | 10 +++---- 4 files changed, 30 insertions(+), 27 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 6163a6f..d73907b 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -18,7 +18,7 @@ The parameters specified here are those for for which we can't set up reliable defaults, so we need to have the user set them. **********************************************************************************/ -QuickPID::QuickPID(int16_t* Input, uint8_t* Output, int16_t* Setpoint, +QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, float Kp, float Ki, float Kd, bool POn, bool ControllerDirection) { myOutput = Output; @@ -40,7 +40,7 @@ QuickPID::QuickPID(int16_t* Input, uint8_t* Output, int16_t* Setpoint, to use Proportional on Error without explicitly saying so. **********************************************************************************/ -QuickPID::QuickPID(int16_t* Input, uint8_t* Output, int16_t* Setpoint, +QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, float Kp, float Ki, float Kd, bool ControllerDirection) : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) { @@ -143,7 +143,7 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) the default already, the required output limits might be unique, like using a time window of 0-8000 needing to clamp it from 0-125. ******************************************************************************/ -void QuickPID::SetOutputLimits(uint8_t Min, uint8_t Max) +void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) { if (Min >= Max) return; outMin = Min; diff --git a/QuickPID.h b/QuickPID.h index b25738c..eaa2d95 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -18,10 +18,10 @@ class QuickPID // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(int16_t*, uint8_t*, int16_t*, float, float, float, bool, bool); + QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, bool, bool); // Overload constructor with proportional mode. Links the PID to Input, Output, Setpoint and Tuning Parameters. - QuickPID(int16_t*, uint8_t*, int16_t*, float, float, float, bool); + QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, bool); // Sets PID to either Manual (0) or Auto (non-0). void SetMode(bool Mode); @@ -32,7 +32,7 @@ class QuickPID // Clamps the output to a specific range. 0-255 by default, but it's likely the user will want to change this // depending on the application. - void SetOutputLimits(uint8_t, uint8_t); + void SetOutputLimits(int16_t, int16_t); // available but not commonly used functions ****************************************************************** @@ -78,14 +78,13 @@ class QuickPID bool pOn; int16_t *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a - uint8_t *myOutput; // hard link between the variables and the PID, freeing the user from having + int16_t *myOutput; // hard link between the variables and the PID, freeing the user from having int16_t *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. uint32_t SampleTimeUs, lastTime; + int16_t outMin, outMax, error; int16_t lastInput, outputSum; - uint8_t outMin, outMax; bool inAuto, pOnE; - int32_t error; }; #endif diff --git a/README.md b/README.md index c37d0ea..8839900 100644 --- a/README.md +++ b/README.md @@ -18,22 +18,26 @@ This API (version 2.02) follows the [ArduinoPID](https://github.com/br3ttb/Ardui | QuickPID | 2.0 | 5.0 | 0.2 | 132 | | Arduino PID | 2.0 | 5.0 | 0.2 | 224 | +#### Self Test Example (RC Filter): P_ON_M + +![pid_self_test_pom](https://user-images.githubusercontent.com/63488701/104115407-2cee5900-52dd-11eb-9b24-ff06d39fd2d6.gif) + ### Variables -| Variable | Arduino PID | QuickPID | Data Type | Min | Max | -| :----------- | :---------- | :------- | :-------- | :--------- | :--------- | -| Setpoint | double | int16_t | Binary | 0 | 1023 | -| Input | double | int16_t | Binary | 0 | 1023 | -| Output | double | uint8_t | Binary | 0 | 255 | -| Kp | double | float | float | -3.402E+38 | 3.402E+38 | -| Ki | double | float | float | -3.402E+38 | 3.402E+38 | -| Kd | double | float | float | -3.402E+38 | 3.402E+38 | -| ratio | double | float | float | -3.402E+38 | 3.402E+38 | -| SampleTimeUs | double | uint32_t | Binary | 0 | 4294967295 | -| outputSum | double | int16_t | Binary | 0 | 255 | -| error | double | int32_t | Binary | -1023 | 1023 | -| dInput | double | int32_t | Binary | -1023 | 1023 | +| Variable | Arduino PID | QuickPID | +| :----------- | :---------- | :------- | +| Setpoint | double | int16_t | +| Input | double | int16_t | +| Output | double | int16_t | +| Kp | double | float | +| Ki | double | float | +| Kd | double | float | +| ratio | double | float | +| SampleTimeUs | double | uint32_t | +| outputSum | double | int16_t | +| error | double | int16_t | +| dInput | double | int16_t | ### Original README diff --git a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino index bcf6814..8cece24 100644 --- a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino +++ b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino @@ -11,12 +11,12 @@ #define PIN_OUTPUT 3 //Define Variables -int16_t Setpoint = 700; -int16_t Input; -uint8_t Output; +int Setpoint = 700; +int Input; +int Output; -uint32_t before, after; -uint16_t cnt = 0; +unsigned long before, after; +int cnt = 0; //Specify the initial tuning parameters float Kp = 2.0, Ki = 15.0, Kd = 0.05; From c4ee21804ce7e66448f65698ccd53e9c53481bcc Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 11 Jan 2021 03:18:07 -0500 Subject: [PATCH 009/101] Improved performance and fixed point algorithm --- QuickPID.cpp | 18 +++++++----------- QuickPID.h | 3 +++ README.md | 7 ++++--- .../QuickPID_Self_Test/QuickPID_Self_Test.ino | 2 +- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index d73907b..e48a3f3 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -66,15 +66,11 @@ bool QuickPID::Compute() error = *mySetpoint - input; /*Working error, Proportional on Measurement and Remaining PID output*/ - if (!pOnE) { - outputSum += (ki * error); - outputSum -= (kpd * dInput); - } + if (!pOnE) outputSum += FX_MUL(FL_FX(ki) , error) - FX_MUL(FL_FX(kpd), dInput); + /*Working error, Proportional on Error and remaining PID output*/ - if (pOnE) { - outputSum += (kpi * error); - outputSum -= (kd * dInput); - } + if (pOnE) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kd), dInput); + if (outputSum > outMax) outputSum = outMax; if (outputSum < outMin) outputSum = outMin; *myOutput = outputSum; @@ -232,19 +228,19 @@ bool QuickPID::GetDirection() { int QuickPID::analogReadFast(int ADCpin) { #if defined(__AVR_ATmega328P__) byte ADCregOriginal = ADCSRA; - ADCSRA = (ADCSRA & B11111000) | 6; //64 prescaler + ADCSRA = (ADCSRA & B11111000) | 5; //32 prescaler int adc = analogRead(ADCpin); ADCSRA = ADCregOriginal; return adc; #elif defined(__AVR_ATtiny_Zero_One__) || defined(__AVR_ATmega_Zero__) byte ADCregOriginal = ADC0_CTRLC; - ADC0_CTRLC = 0x55; //reduced cap, Vdd ref, 64 prescaler + ADC0_CTRLC = 0x54; //reduced cap, Vdd ref, 32 prescaler int adc = analogRead(ADCpin); ADC0_CTRLC = ADCregOriginal; return adc; #elif defined(__AVR_DA__) byte ADCregOriginal = ADC0.CTRLC; - ADC0.CTRLC = ADC_PRESC_DIV64_gc; //64 prescaler + ADC0.CTRLC = ADC_PRESC_DIV32_gc; //32 prescaler int adc = analogRead(ADCpin); ADC0.CTRLC = ADCregOriginal; return adc; diff --git a/QuickPID.h b/QuickPID.h index eaa2d95..c11786a 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -14,6 +14,9 @@ class QuickPID #define P_ON_M 0 #define P_ON_E 1 +#define FL_FX(a) (int32_t)(a*256.0) // float to fixed point +#define FX_MUL(a,b) ((a*b)>>8) // fixed point multiply + // commonly used functions ************************************************************************************ diff --git a/README.md b/README.md index 8839900..d0fe5d1 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,8 @@ This API (version 2.02) follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: - Library named as **QuickPID** and can run concurrently with Arduino **PID** -- Reorganized and more efficient PID algorithms +- Quicker fixed point math in compute function +- Reorganized and more efficient PID algorithm - micros() timing resolution - Faster analog read function - `GetError()`function added for diagnostics @@ -12,10 +13,10 @@ This API (version 2.02) follows the [ArduinoPID](https://github.com/br3ttb/Ardui | Compute | Kp | Ki | Kd | Step Time (µS) | | :----------------------------------- | ---- | ---- | ---- | -------------- | -| QuickPID | 2.0 | 15.0 | 0.05 | 72 | +| QuickPID | 2.0 | 15.0 | 0.05 | 68 | | Arduino PID | 2.0 | 15.0 | 0.05 | 104 | | **analogRead, Compute, analogWrite** | | | | | -| QuickPID | 2.0 | 5.0 | 0.2 | 132 | +| QuickPID | 2.0 | 5.0 | 0.2 | 96 | | Arduino PID | 2.0 | 5.0 | 0.2 | 224 | #### Self Test Example (RC Filter): P_ON_M diff --git a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino index 8cece24..89a19e6 100644 --- a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino +++ b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino @@ -59,7 +59,7 @@ void loop() after = micros(); analogWrite(PIN_OUTPUT, Output); - delay(20); + delay(40); cnt++; if (cnt == 100) { analogWrite(PIN_OUTPUT, 0); From e2c5d6c5165a7ad208f1aae5b93002f8c178d360 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 11 Jan 2021 13:31:21 -0500 Subject: [PATCH 010/101] Update QuickPID.cpp Uses fixed point math for smaller coefficients, floating point math for larger coefficients. --- QuickPID.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index e48a3f3..977e1a2 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -66,10 +66,16 @@ bool QuickPID::Compute() error = *mySetpoint - input; /*Working error, Proportional on Measurement and Remaining PID output*/ - if (!pOnE) outputSum += FX_MUL(FL_FX(ki) , error) - FX_MUL(FL_FX(kpd), dInput); + if (!pOnE) { + if (ki < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(ki) , error) - FX_MUL(FL_FX(kpd), dInput); + else outputSum += (ki * error) - (kpd * dInput); + } /*Working error, Proportional on Error and remaining PID output*/ - if (pOnE) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kd), dInput); + if (pOnE) { + if (kpi < 31 && kd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kd), dInput); + else outputSum += (kpi * error) - (kd * dInput); + } if (outputSum > outMax) outputSum = outMax; if (outputSum < outMin) outputSum = outMin; From 061b9032182c51e03071576a6987c97998ed8d75 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 12 Jan 2021 21:03:01 -0500 Subject: [PATCH 011/101] Update Readme, add new function --- QuickPID.cpp | 11 +++-- QuickPID.h | 3 +- README.md | 22 +++++++--- .../QuickPID_Self_Test/QuickPID_Self_Test.ino | 41 ++++++++----------- keywords.txt | 1 + library.properties | 2 +- 6 files changed, 45 insertions(+), 35 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 977e1a2..b02a78d 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.0.2 + QuickPID Library for Arduino - Version 2.0.3 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard @@ -219,15 +219,18 @@ float QuickPID::GetKi() { float QuickPID::GetKd() { return dispKd; } -int16_t QuickPID::GetError() { - return error; -} bool QuickPID::GetMode() { return inAuto ? AUTOMATIC : MANUAL; } bool QuickPID::GetDirection() { return controllerDirection; } +int16_t QuickPID::GetError() { + return error; +} +bool QuickPID::GetpOnE() { + return pOnE; +} // Utility functions ********************************************************** diff --git a/QuickPID.h b/QuickPID.h index c11786a..67e44b3 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -57,9 +57,10 @@ class QuickPID float GetKp(); // These functions query the pid for interal values. They were created mainly for float GetKi(); // the pid front-end, where it's important to know what is actually inside the PID. float GetKd(); - int16_t GetError(); bool GetMode(); bool GetDirection(); + int16_t GetError(); + bool GetpOnE(); // Utility functions ****************************************************************************************** int analogReadFast(int); diff --git a/README.md b/README.md index d0fe5d1..e03b70c 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,14 @@ # QuickPID -This API (version 2.02) follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: +This API (version 2.03) follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: -- Library named as **QuickPID** and can run concurrently with Arduino **PID** -- Quicker fixed point math in compute function +- This library named as **QuickPID** and can run alongside with Arduino **PID** if needed +- Quicker fixed point math in compute function for small tuning values, floating point math used for large tuning values - Reorganized and more efficient PID algorithm - micros() timing resolution - Faster analog read function -- `GetError()`function added for diagnostics +- `GetError()`and `GetpOnE()`functions added for diagnostics and control benefits +- Runs a complete PID cycle (*read-compute-write*) faster than just an `analogRead()` command in Arduino ### Performance @@ -19,9 +20,18 @@ This API (version 2.02) follows the [ArduinoPID](https://github.com/br3ttb/Ardui | QuickPID | 2.0 | 5.0 | 0.2 | 96 | | Arduino PID | 2.0 | 5.0 | 0.2 | 224 | -#### Self Test Example (RC Filter): P_ON_M +#### Self Test Example (RC Filter): -![pid_self_test_pom](https://user-images.githubusercontent.com/63488701/104115407-2cee5900-52dd-11eb-9b24-ff06d39fd2d6.gif) +This example allows you to set an output voltage, then view the result of your tuning parameters. The mode of the P-Term automatically toggles from Proportional on Error to [Proportional on Measurement.](http://brettbeauregard.com/blog/2017/06/introducing-proportional-on-measurement/) + +![pid_self_test_pom](https://user-images.githubusercontent.com/63488701/104389509-a66a8f00-5509-11eb-927b-1190231a1ee9.gif) + +### Simplified PID Algorithm + +| Proportional Term Mode | Algorithm | +| --------------------------- | --------------------------------------------- | +| Proportional on Error | `outputSum += (kpi * error) - (kd * dInput);` | +| Proportional on Measurement | `outputSum += (ki * error) - (kpd * dInput);` | ### Variables diff --git a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino index 89a19e6..51ea18c 100644 --- a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino +++ b/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino @@ -25,33 +25,27 @@ QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); void setup() { - myQuickPID.SetTunings(Kp, Ki, Kd, P_ON_M); Serial.begin(115200); - analogWrite(PIN_OUTPUT, 0); - delay(1000); // discharge capacitor - Input = myQuickPID.analogReadFast(PIN_INPUT); + myQuickPID.SetTunings(Kp, Ki, Kd, P_ON_E); myQuickPID.SetMode(AUTOMATIC); + analogWrite(PIN_OUTPUT, 0); // discharge capacitor + delay(1000); } void loop() { - //Serial.println("Min:0,Max:1000"); // set scale - - // Stretch the plot x2 - for (int i = 0; i <= 1; i++) { - Serial.print("Setpoint:"); - Serial.print(Setpoint); - Serial.print(","); - Serial.print("Input:"); - Serial.print(Input); - Serial.print(","); - Serial.print("Output:"); - Serial.print(Output); - Serial.print(","); - Serial.print("Runtime:"); - Serial.print(after - before); - Serial.println(","); - } + Serial.print("Setpoint:"); + Serial.print(Setpoint); + Serial.print(","); + Serial.print("Input:"); + Serial.print(Input); + Serial.print(","); + Serial.print("Output:"); + Serial.print(Output); + Serial.print(","); + Serial.print("Runtime:"); + Serial.print(after - before); + Serial.println(","); Input = myQuickPID.analogReadFast(PIN_INPUT); before = micros(); @@ -59,11 +53,12 @@ void loop() after = micros(); analogWrite(PIN_OUTPUT, Output); - delay(40); + delay(10); cnt++; - if (cnt == 100) { + if (cnt == 250) { analogWrite(PIN_OUTPUT, 0); delay(1000); // discharge capacitor cnt = 0; + myQuickPID.SetTunings(Kp, Ki, Kd, !(myQuickPID.GetpOnE())); //toggle P-Term mode } } diff --git a/keywords.txt b/keywords.txt index 6616654..d856f2c 100644 --- a/keywords.txt +++ b/keywords.txt @@ -23,6 +23,7 @@ GetKi KEYWORD2 GetKd KEYWORD2 GetMode KEYWORD2 GetError KEYWORD2 +GetpOnE KEYWORD2 GetDirection KEYWORD2 analogReadFast KEYWORD2 diff --git a/library.properties b/library.properties index e863c7d..2b8ce1c 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.0.2 +version=2.0.3 author=dlloydev maintainer=dlloydev sentence=QuickPID controller From 2e98e965b4c35c33cc7a2cb45083018a47b6a341 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 16 Jan 2021 11:43:16 -0500 Subject: [PATCH 012/101] Add setpoint weighting feature --- QuickPID.cpp | 28 ++++++------------- QuickPID.h | 17 +++++------ README.md | 8 +++--- .../QuickPID_RC_Filter.ino} | 14 +++++----- keywords.txt | 3 -- library.json | 2 +- library.properties | 10 +++---- 7 files changed, 32 insertions(+), 50 deletions(-) rename examples/{QuickPID_Self_Test/QuickPID_Self_Test.ino => QuickPID_RC_Filter/QuickPID_RC_Filter.ino} (74%) diff --git a/QuickPID.cpp b/QuickPID.cpp index b02a78d..9ffa3df 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -19,7 +19,7 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - float Kp, float Ki, float Kd, bool POn, bool ControllerDirection) + float Kp, float Ki, float Kd, float POn, bool ControllerDirection) { myOutput = Output; myInput = Input; @@ -42,7 +42,7 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, float Kp, float Ki, float Kd, bool ControllerDirection) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn, ControllerDirection) { } @@ -65,17 +65,9 @@ bool QuickPID::Compute() int16_t dInput = input - lastInput; error = *mySetpoint - input; - /*Working error, Proportional on Measurement and Remaining PID output*/ - if (!pOnE) { - if (ki < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(ki) , error) - FX_MUL(FL_FX(kpd), dInput); - else outputSum += (ki * error) - (kpd * dInput); - } - - /*Working error, Proportional on Error and remaining PID output*/ - if (pOnE) { - if (kpi < 31 && kd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kd), dInput); - else outputSum += (kpi * error) - (kd * dInput); - } + /*Working error, Proportional distribution and Remaining PID output*/ + if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); + else outputSum += (kpi * error) - (kpd * dInput); if (outputSum > outMax) outputSum = outMax; if (outputSum < outMin) outputSum = outMin; @@ -94,20 +86,19 @@ bool QuickPID::Compute() it's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation ******************************************************************************/ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, bool POn) +void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn) { if (Kp < 0 || Ki < 0 || Kd < 0) return; pOn = POn; - pOnE = POn == P_ON_E; dispKp = Kp; dispKi = Ki; dispKd = Kd; float SampleTimeSec = (float)SampleTimeUs / 1000000; kp = Kp; ki = Ki * SampleTimeSec; kd = Kd / SampleTimeSec; - kpd = kp + kd; - kpi = kp + ki; + kpi = kp * pOn + ki; + kpd = kp * (1 - pOn) + kd; if (controllerDirection == REVERSE) { @@ -228,9 +219,6 @@ bool QuickPID::GetDirection() { int16_t QuickPID::GetError() { return error; } -bool QuickPID::GetpOnE() { - return pOnE; -} // Utility functions ********************************************************** diff --git a/QuickPID.h b/QuickPID.h index 67e44b3..d61c3bc 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -11,8 +11,6 @@ class QuickPID #define MANUAL 0 #define DIRECT 0 #define REVERSE 1 -#define P_ON_M 0 -#define P_ON_E 1 #define FL_FX(a) (int32_t)(a*256.0) // float to fixed point #define FX_MUL(a,b) ((a*b)>>8) // fixed point multiply @@ -21,7 +19,7 @@ class QuickPID // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, bool, bool); + QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, float, bool); // Overload constructor with proportional mode. Links the PID to Input, Output, Setpoint and Tuning Parameters. QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, bool); @@ -44,7 +42,7 @@ class QuickPID void SetTunings(float, float, float); // Overload for specifying proportional mode. - void SetTunings(float, float, float, bool); + void SetTunings(float, float, float, float); // Sets the Direction, or "Action" of control. DIRECT means the output will increase when error is positive. // REVERSE means the opposite. It's very unlikely that this will be needed once it is set in the constructor. @@ -60,7 +58,6 @@ class QuickPID bool GetMode(); bool GetDirection(); int16_t GetError(); - bool GetpOnE(); // Utility functions ****************************************************************************************** int analogReadFast(int); @@ -68,18 +65,18 @@ class QuickPID private: void Initialize(); - float dispKp; // We'll hold on to the tuning parameters in user-entered format for display purposes. + float dispKp; // We'll hold on to the tuning parameters for display purposes. float dispKi; float dispKd; + float pOn; // proportional mode (0-1) default 1 (100% on Error, 0% on Measurement) float kp; // (P)roportional Tuning Parameter float ki; // (I)ntegral Tuning Parameter float kd; // (D)erivative Tuning Parameter - float kpd = kp + kd; - float kpi = kp + ki; + float kpi; // proportional on error amount + float kpd; // proportional on measurement amount bool controllerDirection; - bool pOn; int16_t *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a int16_t *myOutput; // hard link between the variables and the PID, freeing the user from having @@ -88,7 +85,7 @@ class QuickPID uint32_t SampleTimeUs, lastTime; int16_t outMin, outMax, error; int16_t lastInput, outputSum; - bool inAuto, pOnE; + bool inAuto; }; #endif diff --git a/README.md b/README.md index e03b70c..28131c6 100644 --- a/README.md +++ b/README.md @@ -2,8 +2,8 @@ This API (version 2.03) follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: -- This library named as **QuickPID** and can run alongside with Arduino **PID** if needed -- Quicker fixed point math in compute function for small tuning values, floating point math used for large tuning values +- Quicker hybrid fixed/floating point math in compute function +- POn parameter controls the setpoint weighting of Proportional on Error (PonE) to Proportional on Measurement (PonM) - Reorganized and more efficient PID algorithm - micros() timing resolution - Faster analog read function @@ -14,7 +14,7 @@ This API (version 2.03) follows the [ArduinoPID](https://github.com/br3ttb/Ardui | Compute | Kp | Ki | Kd | Step Time (µS) | | :----------------------------------- | ---- | ---- | ---- | -------------- | -| QuickPID | 2.0 | 15.0 | 0.05 | 68 | +| QuickPID | 2.0 | 15.0 | 0.05 | 72 | | Arduino PID | 2.0 | 15.0 | 0.05 | 104 | | **analogRead, Compute, analogWrite** | | | | | | QuickPID | 2.0 | 5.0 | 0.2 | 96 | @@ -22,7 +22,7 @@ This API (version 2.03) follows the [ArduinoPID](https://github.com/br3ttb/Ardui #### Self Test Example (RC Filter): -This example allows you to set an output voltage, then view the result of your tuning parameters. The mode of the P-Term automatically toggles from Proportional on Error to [Proportional on Measurement.](http://brettbeauregard.com/blog/2017/06/introducing-proportional-on-measurement/) +[This example](https://github.com/Dlloydev/QuickPID/wiki/QuickPID_RC_Filter) allows you to experiment with the four tuning parameters. ![pid_self_test_pom](https://user-images.githubusercontent.com/63488701/104389509-a66a8f00-5509-11eb-927b-1190231a1ee9.gif) diff --git a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino b/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino similarity index 74% rename from examples/QuickPID_Self_Test/QuickPID_Self_Test.ino rename to examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino index 51ea18c..2db6bc4 100644 --- a/examples/QuickPID_Self_Test/QuickPID_Self_Test.ino +++ b/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino @@ -1,9 +1,10 @@ -/************************************************************ - PID RC Filter Self Test Example: - One 47µF capacitor connected from GND to a 27K resistor +/************************************************************** + PID RC Filter Example: + One 47µF capacitor connected from GND to a 10K resistor terminated at pwm pin 3. Junction point of the RC filter is connected to A0. Use Serial Plotter to view results. - ************************************************************/ + https://github.com/Dlloydev/QuickPID/wiki/QuickPID_RC_Filter + **************************************************************/ #include "QuickPID.h" @@ -19,14 +20,14 @@ unsigned long before, after; int cnt = 0; //Specify the initial tuning parameters -float Kp = 2.0, Ki = 15.0, Kd = 0.05; +float Kp = 2.0, Ki = 15.0, Kd = 0.05, POn = 1.0; QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); void setup() { Serial.begin(115200); - myQuickPID.SetTunings(Kp, Ki, Kd, P_ON_E); + myQuickPID.SetTunings(Kp, Ki, Kd, POn); myQuickPID.SetMode(AUTOMATIC); analogWrite(PIN_OUTPUT, 0); // discharge capacitor delay(1000); @@ -59,6 +60,5 @@ void loop() analogWrite(PIN_OUTPUT, 0); delay(1000); // discharge capacitor cnt = 0; - myQuickPID.SetTunings(Kp, Ki, Kd, !(myQuickPID.GetpOnE())); //toggle P-Term mode } } diff --git a/keywords.txt b/keywords.txt index d856f2c..3044ccc 100644 --- a/keywords.txt +++ b/keywords.txt @@ -23,7 +23,6 @@ GetKi KEYWORD2 GetKd KEYWORD2 GetMode KEYWORD2 GetError KEYWORD2 -GetpOnE KEYWORD2 GetDirection KEYWORD2 analogReadFast KEYWORD2 @@ -35,5 +34,3 @@ AUTOMATIC LITERAL1 MANUAL LITERAL1 DIRECT LITERAL1 REVERSE LITERAL1 -P_ON_E LITERAL1 -P_ON_M LITERAL1 diff --git a/library.json b/library.json index 4d76c6e..19c9f79 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "A fast, fixed point PID controller that seeks to keep an 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).", + "description": "QuickPID controller - a faster implementation of the Arduino PID Library with more features. This hybrid fixed/floating point controller seeks to keep an output close to a desired setpoint by adjusting four parameters (P,I,D) and POn (PonE, PonM setpoint weighting).", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 2b8ce1c..2ab8eec 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=QuickPID version=2.0.3 -author=dlloydev -maintainer=dlloydev -sentence=QuickPID controller -paragraph=A fast, fixed point PID controller that seeks to keep an 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). +author=dlloydev +maintainer=David Lloyd +sentence=QuickPID controller - a faster hybrid fixed/floating point implementation of the Arduino PID Library with more features. +paragraph=This controller seeks to keep an output close to a desired setpoint by adjusting four parameters (P,I,D) and POn (PonE, PonM setpoint weighting). category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID -architectures=* +architectures=avr From 98c20fe03dde2b3633259cbe7717f1677c25fe4d Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 17 Jan 2021 03:17:41 -0500 Subject: [PATCH 013/101] Update Readme --- QuickPID.cpp | 10 ++- README.md | 183 ++++++++++++++++++++++++++++++++++++++++++--------- 2 files changed, 155 insertions(+), 38 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 9ffa3df..ac739c9 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -109,7 +109,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn) } /* SetTunings(...)************************************************************* - Set Tunings using the last-rembered POn setting + Set Tunings using the last remembered POn setting. ******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { SetTunings(Kp, Ki, Kd, pOn); @@ -131,10 +131,8 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) } /* SetOutputLimits(...)******************************************************** - This function will be used far more often than SetInputLimits. While - the input to the controller will generally be in the 0-1023 range, which is - the default already, the required output limits might be unique, like using - a time window of 0-8000 needing to clamp it from 0-125. + The PID controller is designed to vary its output within a given range. + By default this range is 0-255, the Arduino PWM range. ******************************************************************************/ void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) { @@ -168,7 +166,7 @@ void QuickPID::SetMode(bool Mode) } /* Initialize()**************************************************************** - Does all the things that need to happen to ensure a bumpless transfer + Does all the things that need to happen to ensure a bumpless transfer from manual to automatic mode. ******************************************************************************/ void QuickPID::Initialize() diff --git a/README.md b/README.md index 28131c6..9cf8015 100644 --- a/README.md +++ b/README.md @@ -1,56 +1,175 @@ # QuickPID -This API (version 2.03) follows the [ArduinoPID](https://github.com/br3ttb/Arduino-PID-Library) library, however there have been some significant updates as follows: +A fast hybrid fixed-point and floating-point PID controller for Arduino. + +### About + +This PID controller provides a faster *read-compute-write* cycle than alternatives as it has a more efficient and reduced algorithm that avoids time consuming partial calculations, it takes advantage of fixed point math and has a faster analog read function. The `Ki` and `Kd` are scaled by time (µs) and the new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required in `Compute()`. + +### Features + +Development began with a fork of the Arduino PID Library. Some modifications and new features have been added as follows: - Quicker hybrid fixed/floating point math in compute function -- POn parameter controls the setpoint weighting of Proportional on Error (PonE) to Proportional on Measurement (PonM) +- `POn` parameter now controls the setpoint weighting of Proportional on Error and Proportional on Measurement - Reorganized and more efficient PID algorithm - micros() timing resolution - Faster analog read function -- `GetError()`and `GetpOnE()`functions added for diagnostics and control benefits +- `GetError()` function added - Runs a complete PID cycle (*read-compute-write*) faster than just an `analogRead()` command in Arduino ### Performance -| Compute | Kp | Ki | Kd | Step Time (µS) | -| :----------------------------------- | ---- | ---- | ---- | -------------- | -| QuickPID | 2.0 | 15.0 | 0.05 | 72 | -| Arduino PID | 2.0 | 15.0 | 0.05 | 104 | -| **analogRead, Compute, analogWrite** | | | | | -| QuickPID | 2.0 | 5.0 | 0.2 | 96 | -| Arduino PID | 2.0 | 5.0 | 0.2 | 224 | - -#### Self Test Example (RC Filter): +| `Compute()` | Kp | Ki | Kd | Step Time (µS) | +| :----------------------------------------------------------- | ---- | ---- | ---- | -------------- | +| QuickPID | 2.0 | 15.0 | 0.05 | 72 | +| Arduino PID | 2.0 | 15.0 | 0.05 | 104 | +| **Full PID cycle:** **`analogRead(), Compute(), analogWrite()`** | | | | | +| QuickPID using `analogReadFast()` | 2.0 | 5.0 | 0.2 | 96 | +| Arduino PID using `analogRead()` | 2.0 | 5.0 | 0.2 | 224 | + +### Functions + +[QuickPID::QuickPID Constructor](#QuickPID::QuickPID Constructor) +[Compute()](#Compute()) +[SetTunings()](#SetTunings()) +[SetSampleTime()](#SetSampleTime()) +[SetOutputLimits()](#SetOutputLimits()) +[SetMode()](#SetMode()) +[Initialize()](#Initialize()) +[SetControllerDirection()](#SetControllerDirection()) +[Display Functions](#Display Functions()) +[Utility Functions](#Utility Functions()) + +### Self Test Example (RC Filter): [This example](https://github.com/Dlloydev/QuickPID/wiki/QuickPID_RC_Filter) allows you to experiment with the four tuning parameters. ![pid_self_test_pom](https://user-images.githubusercontent.com/63488701/104389509-a66a8f00-5509-11eb-927b-1190231a1ee9.gif) -### Simplified PID Algorithm +### Simplified PID Algorithm + +```c++ +outputSum += (kpi * error) - (kpd * dInput); +``` + +The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required The POn parameter controls the setpoint weighting of Proportional on Error and Proportional on Measurement. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated in the `QuickPID::SetTunings()` function as follows: + +```c++ + kpi = kp * pOn + ki; + kpd = kp * (1 - pOn) + kd; +``` + +#### QuickPID::QuickPID Constructor + +```c++ +QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, + float Kp, float Ki, float Kd, float POn, bool ControllerDirection) +``` + +- `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. +- `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. +- `POn` is the Proportional on Error weighting value (0.0-1.0). This controls the amount of Proportional on Error and Proportional on Measurement factor that's used in the compute algorithm. +- `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. DIRECT is most common. + +```c++ +QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, + float Kp, float Ki, float Kd, bool ControllerDirection) +``` + +This allows you to use Proportional on Error without explicitly saying so. + +#### Compute() + +```c++ +bool QuickPID::Compute() +``` + +This function contains the PID algorithm and it should be called once every loop(). Most of the time it will just return false without doing anything. However, at a frequency specified by `SetSampleTime` it will calculate a new Output and return true. + +#### SetTunings() + +```c++ +void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn) +``` + +This function allows the controller's dynamic performance to be adjusted. It's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. The parameters are as described in the constructor. + +```c++ +void QuickPID::SetTunings(float Kp, float Ki, float Kd) +``` + +Set Tunings using the last remembered POn setting. + +#### SetSampleTime() + +```c++ +void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) +``` + +Sets the period, in microseconds, at which the calculation is performed. The default is 100ms. + +#### SetOutputLimits() + +```c++ +void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) +``` + +The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. + +#### SetMode() + +```c++ +void QuickPID::SetMode(bool Mode) +``` + +Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (non-zero). when the transition from manual to automatic occurs, the controller is automatically initialized. + +#### Initialize() -| Proportional Term Mode | Algorithm | -| --------------------------- | --------------------------------------------- | -| Proportional on Error | `outputSum += (kpi * error) - (kd * dInput);` | -| Proportional on Measurement | `outputSum += (ki * error) - (kpd * dInput);` | +```c++ +void QuickPID::Initialize() +``` + +Does all the things that need to happen to ensure a bumpless transfer from manual to automatic mode. + +#### SetControllerDirection() + +```c++ +void QuickPID::SetControllerDirection(bool Direction) +``` + +The PID will either be connected to a DIRECT acting process (+Output leads to +Input) or a REVERSE acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. + +#### Display Functions + +```c++ +float QuickPID::GetKp() +float QuickPID::GetKi() +float QuickPID::GetKd() +bool QuickPID::GetMode() +bool QuickPID::GetDirection() +int16_t QuickPID::GetError() +``` + +These functions query the internal state of the PID. They're here for display purposes. + +#### Utility Functions + +```c++ +int QuickPID::analogReadFast(int ADCpin) +``` -### Variables +A faster configuration of `analogRead()`where a preset of 32 is used. Works with the following defines: +`__AVR_ATmega328P__` +`__AVR_ATtiny_Zero_One__` +`__AVR_ATmega_Zero__` +`__AVR_DA__` -| Variable | Arduino PID | QuickPID | -| :----------- | :---------- | :------- | -| Setpoint | double | int16_t | -| Input | double | int16_t | -| Output | double | int16_t | -| Kp | double | float | -| Ki | double | float | -| Kd | double | float | -| ratio | double | float | -| SampleTimeUs | double | uint32_t | -| outputSum | double | int16_t | -| error | double | int16_t | -| dInput | double | int16_t | +Uses Arduino's default settings and returns `analogRead()` if the definition isn't found. -### Original README +### Original README (Arduino PID) ``` *************************************************************** From 76877253ba35a32a0f7ed2c98b57297f72e91a48 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 17 Jan 2021 03:21:52 -0500 Subject: [PATCH 014/101] Update README.md --- README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/README.md b/README.md index 9cf8015..fa001e7 100644 --- a/README.md +++ b/README.md @@ -31,14 +31,23 @@ Development began with a fork of the Arduino PID Library. Some modifications and ### Functions [QuickPID::QuickPID Constructor](#QuickPID::QuickPID Constructor) + [Compute()](#Compute()) + [SetTunings()](#SetTunings()) + [SetSampleTime()](#SetSampleTime()) + [SetOutputLimits()](#SetOutputLimits()) + [SetMode()](#SetMode()) + [Initialize()](#Initialize()) + [SetControllerDirection()](#SetControllerDirection()) + [Display Functions](#Display Functions()) + [Utility Functions](#Utility Functions()) ### Self Test Example (RC Filter): From beac91df38d132c63c734a28ec5dfa726fc834a6 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 17 Jan 2021 03:28:56 -0500 Subject: [PATCH 015/101] Update README.md --- README.md | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index fa001e7..7b7a878 100644 --- a/README.md +++ b/README.md @@ -30,25 +30,25 @@ Development began with a fork of the Arduino PID Library. Some modifications and ### Functions -[QuickPID::QuickPID Constructor](#QuickPID::QuickPID Constructor) +[QuickPID Constructor](#QuickPID Constructor) -[Compute()](#Compute()) +[Compute](#Compute) -[SetTunings()](#SetTunings()) +[SetTunings](#SetTunings) -[SetSampleTime()](#SetSampleTime()) +[SetSampleTime](#SetSampleTime) -[SetOutputLimits()](#SetOutputLimits()) +[SetOutputLimits](#SetOutputLimits) -[SetMode()](#SetMode()) +[SetMode](#SetMode) -[Initialize()](#Initialize()) +[Initialize](#Initialize) -[SetControllerDirection()](#SetControllerDirection()) +[SetControllerDirection](#SetControllerDirection) -[Display Functions](#Display Functions()) +[Display Functions](#Display Functions) -[Utility Functions](#Utility Functions()) +[Utility Functions](#Utility Functions) ### Self Test Example (RC Filter): @@ -69,7 +69,7 @@ The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function kpd = kp * (1 - pOn) + kd; ``` -#### QuickPID::QuickPID Constructor +#### QuickPID Constructor ```c++ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, @@ -88,7 +88,7 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, This allows you to use Proportional on Error without explicitly saying so. -#### Compute() +#### Compute ```c++ bool QuickPID::Compute() @@ -96,7 +96,7 @@ bool QuickPID::Compute() This function contains the PID algorithm and it should be called once every loop(). Most of the time it will just return false without doing anything. However, at a frequency specified by `SetSampleTime` it will calculate a new Output and return true. -#### SetTunings() +#### SetTunings ```c++ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn) @@ -110,7 +110,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd) Set Tunings using the last remembered POn setting. -#### SetSampleTime() +#### SetSampleTime ```c++ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) @@ -118,7 +118,7 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) Sets the period, in microseconds, at which the calculation is performed. The default is 100ms. -#### SetOutputLimits() +#### SetOutputLimits ```c++ void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) @@ -126,7 +126,7 @@ void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. -#### SetMode() +#### SetMode ```c++ void QuickPID::SetMode(bool Mode) @@ -134,7 +134,7 @@ void QuickPID::SetMode(bool Mode) Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (non-zero). when the transition from manual to automatic occurs, the controller is automatically initialized. -#### Initialize() +#### Initialize ```c++ void QuickPID::Initialize() @@ -142,7 +142,7 @@ void QuickPID::Initialize() Does all the things that need to happen to ensure a bumpless transfer from manual to automatic mode. -#### SetControllerDirection() +#### SetControllerDirection ```c++ void QuickPID::SetControllerDirection(bool Direction) From eef81efd79bc925158fd9d1686bc854497576809 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 17 Jan 2021 03:35:52 -0500 Subject: [PATCH 016/101] Update README.md --- README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 7b7a878..971c481 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,7 @@ Development began with a fork of the Arduino PID Library. Some modifications and ### Functions -[QuickPID Constructor](#QuickPID Constructor) +[QuickPID_Constructor](#QuickPID_Constructor) [Compute](#Compute) @@ -46,9 +46,9 @@ Development began with a fork of the Arduino PID Library. Some modifications and [SetControllerDirection](#SetControllerDirection) -[Display Functions](#Display Functions) +[Display_Functions](#Display_Functions) -[Utility Functions](#Utility Functions) +[Utility_Functions](#Utility_Functions) ### Self Test Example (RC Filter): @@ -69,7 +69,7 @@ The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function kpd = kp * (1 - pOn) + kd; ``` -#### QuickPID Constructor +#### QuickPID_Constructor ```c++ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, @@ -150,7 +150,7 @@ void QuickPID::SetControllerDirection(bool Direction) The PID will either be connected to a DIRECT acting process (+Output leads to +Input) or a REVERSE acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. -#### Display Functions +#### Display_Functions ```c++ float QuickPID::GetKp() @@ -163,7 +163,7 @@ int16_t QuickPID::GetError() These functions query the internal state of the PID. They're here for display purposes. -#### Utility Functions +#### Utility_Functions ```c++ int QuickPID::analogReadFast(int ADCpin) From 4d1ebee7b3a3a6d22f06d300cbb6d9d9f6327a27 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 17 Jan 2021 13:00:36 -0500 Subject: [PATCH 017/101] QuickPID 2.0.4 Added QuickPID_AdaptiveTunings.ino, QuickPID_Basic.ino, QuickPID_PonM.ino and QuickPID_RelayOutput.ino to the examples folder. Note thatQuickPID_RelayOutput.ino has the added feature of minWindow setting that sets the minimum on time for the relay. --- QuickPID.cpp | 8 +-- README.md | 7 +- .../QuickPID_AdaptiveTunings.ino | 52 +++++++++++++++ examples/QuickPID_Basic/QuickPID_Basic.ino | 35 ++++++++++ examples/QuickPID_PonM/QuickPID_PonM.ino | 33 ++++++++++ .../QuickPID_RC_Filter/QuickPID_RC_Filter.ino | 11 ++-- .../QuickPID_RelayOutput.ino | 66 +++++++++++++++++++ library.properties | 2 +- 8 files changed, 202 insertions(+), 12 deletions(-) create mode 100644 examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino create mode 100644 examples/QuickPID_Basic/QuickPID_Basic.ino create mode 100644 examples/QuickPID_PonM/QuickPID_PonM.ino create mode 100644 examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino diff --git a/QuickPID.cpp b/QuickPID.cpp index ac739c9..3b3657c 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.0.3 + QuickPID Library for Arduino - Version 2.0.4 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard @@ -28,7 +28,6 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, QuickPID::SetOutputLimits(0, 255); // default is same as the arduino PWM limit SampleTimeUs = 100000; // default is 0.1 seconds - QuickPID::SetControllerDirection(ControllerDirection); QuickPID::SetTunings(Kp, Ki, Kd, POn); @@ -42,7 +41,7 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, float Kp, float Ki, float Kd, bool ControllerDirection) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn, ControllerDirection) + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, POn, ControllerDirection) { } @@ -91,6 +90,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn) if (Kp < 0 || Ki < 0 || Kd < 0) return; pOn = POn; + dispKp = Kp; dispKi = Ki; dispKd = Kd; float SampleTimeSec = (float)SampleTimeUs / 1000000; @@ -112,7 +112,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn) Set Tunings using the last remembered POn setting. ******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { - SetTunings(Kp, Ki, Kd, pOn); + SetTunings(Kp, Ki, Kd, POn); } /* SetSampleTime(...) ********************************************************* diff --git a/README.md b/README.md index 971c481..8f48014 100644 --- a/README.md +++ b/README.md @@ -62,7 +62,7 @@ Development began with a fork of the Arduino PID Library. Some modifications and outputSum += (kpi * error) - (kpd * dInput); ``` -The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required The POn parameter controls the setpoint weighting of Proportional on Error and Proportional on Measurement. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated in the `QuickPID::SetTunings()` function as follows: +The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required The pOn variable controls the setpoint weighting of Proportional on Error and Proportional on Measurement. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated as follows: ```c++ kpi = kp * pOn + ki; @@ -172,11 +172,14 @@ int QuickPID::analogReadFast(int ADCpin) A faster configuration of `analogRead()`where a preset of 32 is used. Works with the following defines: `__AVR_ATmega328P__` + `__AVR_ATtiny_Zero_One__` + `__AVR_ATmega_Zero__` + `__AVR_DA__` -Uses Arduino's default settings and returns `analogRead()` if the definition isn't found. + If the definition isn't found, normal `analogRead()`is used to return a value. ### Original README (Arduino PID) diff --git a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino new file mode 100644 index 0000000..0699c5d --- /dev/null +++ b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino @@ -0,0 +1,52 @@ +/******************************************************** + PID Adaptive Tuning Example + One of the benefits of the PID library is that you can + change the tuning parameters at any time. this can be + helpful if we want the controller to be agressive at some + times, and conservative at others. in the example below + we set the controller to use Conservative Tuning Parameters + when we're near setpoint and more agressive Tuning + Parameters when we're farther away. + ********************************************************/ + +#include + +#define PIN_INPUT 0 +#define PIN_OUTPUT 3 + +//Define Variables we'll be connecting to +int Setpoint, Input, Output; + +//Define the aggressive and conservative and POn Tuning Parameters +float aggKp = 4, aggKi = 0.2, aggKd = 1; +float consKp = 1, consKi = 0.05, consKd = 0.25; +float aggPOn = 1.0; // Range is 0.0 to 1.0 (1.0 is 100% P on Error, 0% P on Measurement) +float consPOn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) + +//Specify the links and initial tuning parameters +QuickPID myQuickPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, aggPOn, DIRECT); + +void setup() +{ + //initialize the variables we're linked to + Input = myQuickPID.analogReadFast(PIN_INPUT); + Setpoint = 100; + + //turn the PID on + myQuickPID.SetMode(AUTOMATIC); +} + +void loop() +{ + Input = myQuickPID.analogReadFast(PIN_INPUT); + + float gap = abs(Setpoint - Input); //distance away from setpoint + if (gap < 10) { //we're close to setpoint, use conservative tuning parameters + myQuickPID.SetTunings(consKp, consKi, consKd, consPOn); + } else { + //we're far from setpoint, use aggressive tuning parameters + myQuickPID.SetTunings(aggKp, aggKi, aggKd, aggPOn); + } + myQuickPID.Compute(); + analogWrite(PIN_OUTPUT, Output); +} diff --git a/examples/QuickPID_Basic/QuickPID_Basic.ino b/examples/QuickPID_Basic/QuickPID_Basic.ino new file mode 100644 index 0000000..3f1ade3 --- /dev/null +++ b/examples/QuickPID_Basic/QuickPID_Basic.ino @@ -0,0 +1,35 @@ +/******************************************************** + PID Basic Example + Reading analog input 0 to control analog PWM output 3 + ********************************************************/ + +#include + +#define PIN_INPUT 0 +#define PIN_OUTPUT 3 + +//Define Variables we'll be connecting to +int Setpoint, Input, Output; + +//Specify the links and initial tuning parameters +float Kp = 2, Ki = 5, Kd = 1; +float POn = 1.0; // Range is 0.0 to 1.0 (1.0 is 100% P on Error, 0% P on Measurement) + +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); + +void setup() +{ + //initialize the variables we're linked to + Input = myQuickPID.analogReadFast(PIN_INPUT); + Setpoint = 100; + + //turn the PID on + myQuickPID.SetMode(AUTOMATIC); +} + +void loop() +{ + Input = myQuickPID.analogReadFast(PIN_INPUT); + myQuickPID.Compute(); + analogWrite(PIN_OUTPUT, Output); +} diff --git a/examples/QuickPID_PonM/QuickPID_PonM.ino b/examples/QuickPID_PonM/QuickPID_PonM.ino new file mode 100644 index 0000000..d5acec8 --- /dev/null +++ b/examples/QuickPID_PonM/QuickPID_PonM.ino @@ -0,0 +1,33 @@ +/******************************************************** + PID Proportional on measurement Example + Setting the PID to use Proportional on measurement will + make the output move more smoothly when the setpoint + is changed. In addition, it can eliminate overshoot + in certain processes like sous-vides. + ********************************************************/ + +#include + +//Define Variables we'll be connecting to +int Setpoint, Input, Output; +float POn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) + +//Specify the links and initial tuning parameters +QuickPID myQuickPID(&Input, &Output, &Setpoint, 2, 5, 1, POn, DIRECT); + +void setup() +{ + //initialize the variables we're linked to + Input = myQuickPID.analogReadFast(0); + Setpoint = 100; + + //turn the PID on + myQuickPID.SetMode(AUTOMATIC); +} + +void loop() +{ + Input = myQuickPID.analogReadFast(0); + myQuickPID.Compute(); + analogWrite(3, Output); +} diff --git a/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino b/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino index 2db6bc4..24fc827 100644 --- a/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino +++ b/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino @@ -6,9 +6,9 @@ https://github.com/Dlloydev/QuickPID/wiki/QuickPID_RC_Filter **************************************************************/ -#include "QuickPID.h" +#include -#define PIN_INPUT A0 +#define PIN_INPUT 0 #define PIN_OUTPUT 3 //Define Variables @@ -20,14 +20,15 @@ unsigned long before, after; int cnt = 0; //Specify the initial tuning parameters -float Kp = 2.0, Ki = 15.0, Kd = 0.05, POn = 1.0; +float Kp = 2.0, Ki = 15.0, Kd = 0.05; +float POn = 1.0; // Range is 0.0 to 1.0 (1.0 is 100% P on Error, 0% P on Measurement) -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); void setup() { Serial.begin(115200); - myQuickPID.SetTunings(Kp, Ki, Kd, POn); + myQuickPID.SetTunings(Kp, Ki, Kd); myQuickPID.SetMode(AUTOMATIC); analogWrite(PIN_OUTPUT, 0); // discharge capacitor delay(1000); diff --git a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino new file mode 100644 index 0000000..5cd41c5 --- /dev/null +++ b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino @@ -0,0 +1,66 @@ +/******************************************************** + PID RelayOutput Example + Same as basic example, except that this time, the output + is going to a digital pin which (we presume) is controlling + a relay. the pid is designed to Output an analog value, + but the relay can only be On/Off. + + to connect them together we use "time proportioning + control" it's essentially a really slow version of PWM. + first we decide on a window size (5000mS say.) we then + set the pid to adjust its output between 0 and that window + size. lastly, we add some logic that translates the PID + output into "Relay On Time" with the remainder of the + window being "Relay Off Time" + The minWindow setting is a floor so that the relay would + be on for a minimum amount of time. + ********************************************************/ + +#include + +#define PIN_INPUT 0 +#define RELAY_PIN 6 + +//Define Variables we'll be connecting to +int Setpoint, Input, Output; + +//Specify the links and initial tuning parameters +float Kp = 2, Ki = 5, Kd = 1; +float POn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) + +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); + +unsigned int WindowSize = 5000; +unsigned int minWindow = 500; +unsigned long windowStartTime; + +void setup() +{ + pinMode(RELAY_PIN, OUTPUT); + windowStartTime = millis(); + + //initialize the variables we're linked to + Setpoint = 100; + + //tell the PID to range between 0 and the full window size + myQuickPID.SetOutputLimits(0, WindowSize); + + //turn the PID on + myQuickPID.SetMode(AUTOMATIC); +} + +void loop() +{ + Input = analogRead(PIN_INPUT); + + /************************************************ + turn the output pin on/off based on pid output + ************************************************/ + if (millis() - windowStartTime > WindowSize) + { //time to shift the Relay Window + windowStartTime += WindowSize; + myQuickPID.Compute(); + } + if (((unsigned int)Output > minWindow) && ((unsigned int)Output < (millis() - windowStartTime))) digitalWrite(RELAY_PIN, HIGH); + else digitalWrite(RELAY_PIN, LOW); +} diff --git a/library.properties b/library.properties index 2ab8eec..b81a3f9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.0.3 +version=2.0.4 author=dlloydev maintainer=David Lloyd sentence=QuickPID controller - a faster hybrid fixed/floating point implementation of the Arduino PID Library with more features. From a59ea08b6cfbd66ddf9dda1b674d4c80833eb98c Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 20 Jan 2021 01:11:46 -0500 Subject: [PATCH 018/101] Update README.md --- README.md | 47 ++++++++++++++++++++++------------------------- 1 file changed, 22 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index 8f48014..fd9067f 100644 --- a/README.md +++ b/README.md @@ -28,34 +28,10 @@ Development began with a fork of the Arduino PID Library. Some modifications and | QuickPID using `analogReadFast()` | 2.0 | 5.0 | 0.2 | 96 | | Arduino PID using `analogRead()` | 2.0 | 5.0 | 0.2 | 224 | -### Functions - -[QuickPID_Constructor](#QuickPID_Constructor) - -[Compute](#Compute) - -[SetTunings](#SetTunings) - -[SetSampleTime](#SetSampleTime) - -[SetOutputLimits](#SetOutputLimits) - -[SetMode](#SetMode) - -[Initialize](#Initialize) - -[SetControllerDirection](#SetControllerDirection) - -[Display_Functions](#Display_Functions) - -[Utility_Functions](#Utility_Functions) - ### Self Test Example (RC Filter): [This example](https://github.com/Dlloydev/QuickPID/wiki/QuickPID_RC_Filter) allows you to experiment with the four tuning parameters. -![pid_self_test_pom](https://user-images.githubusercontent.com/63488701/104389509-a66a8f00-5509-11eb-927b-1190231a1ee9.gif) - ### Simplified PID Algorithm ```c++ @@ -69,6 +45,8 @@ The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function kpd = kp * (1 - pOn) + kd; ``` +### Functions + #### QuickPID_Constructor ```c++ @@ -78,7 +56,10 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. -- `POn` is the Proportional on Error weighting value (0.0-1.0). This controls the amount of Proportional on Error and Proportional on Measurement factor that's used in the compute algorithm. +- `POn` is the Proportional on Error weighting value (0.0-1.0). This controls the mix of Proportional on Error (PonE) and Proportional on Measurement (PonM) that's used in the compute algorithm. Note that POn controls the PonE amount, where the remainder (1-PonE) is the PonM amount. Also, the default POn is 0 (100% PonM, 0% PonE). + +![POn](https://user-images.githubusercontent.com/63488701/104958919-fe3c4680-599e-11eb-851e-73f26291d3e5.gif) + - `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. DIRECT is most common. ```c++ @@ -181,6 +162,19 @@ A faster configuration of `analogRead()`where a preset of 32 is used. Works with If the definition isn't found, normal `analogRead()`is used to return a value. +### Change Log + +#### Version 2.0.4 + +- Added `QuickPID_AdaptiveTunings.ino`, `QuickPID_Basic.ino`, `QuickPID_PonM.ino` and `QuickPID_RelayOutput.ino` to the examples folder. +- `QuickPID_RelayOutput.ino` has the added feature of `minWindow` setting that sets the minimum on time for the relay. + +#### Version 2.0.3 + +- Initial Version with modifications as listed in [features.](#Features) + +------ + ### Original README (Arduino PID) ``` @@ -196,3 +190,6 @@ A faster configuration of `analogRead()`where a preset of 32 is used. Works with http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary + +------ + From 6be0fbe28fa2ea11e3b9b3eb6cdfcde37d8712c7 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 20 Jan 2021 19:10:51 -0500 Subject: [PATCH 019/101] Added MIT license, POn default = 1 --- LICENSE | 19 +++++++++++++++++++ QuickPID.cpp | 12 ++++++------ QuickPID.h | 2 +- README.md | 7 ++++++- .../QuickPID_AdaptiveTunings.ino | 2 +- examples/QuickPID_Basic/QuickPID_Basic.ino | 5 ++--- examples/QuickPID_PonM/QuickPID_PonM.ino | 2 +- .../QuickPID_RC_Filter/QuickPID_RC_Filter.ino | 2 +- .../QuickPID_RelayOutput.ino | 2 +- library.json | 2 +- library.properties | 4 ++-- 11 files changed, 41 insertions(+), 18 deletions(-) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..2893251 --- /dev/null +++ b/LICENSE @@ -0,0 +1,19 @@ +Copyright 2011-2017 Brett Beauregard brettbeauregard.com +Copyright 2021 David Lloyd + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of +the Software, and to permit persons to whom the Software is furnished to do so, +subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS +FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER +IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN +CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/QuickPID.cpp b/QuickPID.cpp index 3b3657c..0d412c2 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.0.4 + QuickPID Library for Arduino - Version 2.0.5 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard @@ -19,7 +19,7 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - float Kp, float Ki, float Kd, float POn, bool ControllerDirection) + float Kp, float Ki, float Kd, float POn = 1, bool ControllerDirection = 0) { myOutput = Output; myInput = Input; @@ -28,6 +28,7 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, QuickPID::SetOutputLimits(0, 255); // default is same as the arduino PWM limit SampleTimeUs = 100000; // default is 0.1 seconds + QuickPID::SetControllerDirection(ControllerDirection); QuickPID::SetTunings(Kp, Ki, Kd, POn); @@ -41,7 +42,7 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, float Kp, float Ki, float Kd, bool ControllerDirection) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, POn, ControllerDirection) + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = 0) { } @@ -85,12 +86,11 @@ bool QuickPID::Compute() it's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation ******************************************************************************/ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn) +void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) { if (Kp < 0 || Ki < 0 || Kd < 0) return; pOn = POn; - dispKp = Kp; dispKi = Ki; dispKd = Kd; float SampleTimeSec = (float)SampleTimeUs / 1000000; @@ -112,7 +112,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn) Set Tunings using the last remembered POn setting. ******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { - SetTunings(Kp, Ki, Kd, POn); + SetTunings(Kp, Ki, Kd, pOn); } /* SetSampleTime(...) ********************************************************* diff --git a/QuickPID.h b/QuickPID.h index d61c3bc..40869a9 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -69,7 +69,7 @@ class QuickPID float dispKi; float dispKd; - float pOn; // proportional mode (0-1) default 1 (100% on Error, 0% on Measurement) + float pOn; // proportional mode (0-1) default = 1, 100% Proportional on Error float kp; // (P)roportional Tuning Parameter float ki; // (I)ntegral Tuning Parameter float kd; // (D)erivative Tuning Parameter diff --git a/README.md b/README.md index fd9067f..8ed3a8a 100644 --- a/README.md +++ b/README.md @@ -56,7 +56,7 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. -- `POn` is the Proportional on Error weighting value (0.0-1.0). This controls the mix of Proportional on Error (PonE) and Proportional on Measurement (PonM) that's used in the compute algorithm. Note that POn controls the PonE amount, where the remainder (1-PonE) is the PonM amount. Also, the default POn is 0 (100% PonM, 0% PonE). +- `POn` is the Proportional on Error weighting value (0.0-1.0). This controls the mix of Proportional on Error (PonE) and Proportional on Measurement (PonM) that's used in the compute algorithm. Note that POn controls the PonE amount, where the remainder (1-PonE) is the PonM amount. Also, the default POn is 1 ![POn](https://user-images.githubusercontent.com/63488701/104958919-fe3c4680-599e-11eb-851e-73f26291d3e5.gif) @@ -164,6 +164,11 @@ A faster configuration of `analogRead()`where a preset of 32 is used. Works with ### Change Log +#### Version 2.0.5 (latest) + +- Added MIT license text file +- POn defaults to 1 + #### Version 2.0.4 - Added `QuickPID_AdaptiveTunings.ino`, `QuickPID_Basic.ino`, `QuickPID_PonM.ino` and `QuickPID_RelayOutput.ino` to the examples folder. diff --git a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino index 0699c5d..f64d470 100644 --- a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino +++ b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino @@ -9,7 +9,7 @@ Parameters when we're farther away. ********************************************************/ -#include +#include "QuickPID.h" #define PIN_INPUT 0 #define PIN_OUTPUT 3 diff --git a/examples/QuickPID_Basic/QuickPID_Basic.ino b/examples/QuickPID_Basic/QuickPID_Basic.ino index 3f1ade3..d36ca0f 100644 --- a/examples/QuickPID_Basic/QuickPID_Basic.ino +++ b/examples/QuickPID_Basic/QuickPID_Basic.ino @@ -3,7 +3,7 @@ Reading analog input 0 to control analog PWM output 3 ********************************************************/ -#include +#include "QuickPID.h" #define PIN_INPUT 0 #define PIN_OUTPUT 3 @@ -13,9 +13,8 @@ int Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -float POn = 1.0; // Range is 0.0 to 1.0 (1.0 is 100% P on Error, 0% P on Measurement) -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); void setup() { diff --git a/examples/QuickPID_PonM/QuickPID_PonM.ino b/examples/QuickPID_PonM/QuickPID_PonM.ino index d5acec8..436f3a1 100644 --- a/examples/QuickPID_PonM/QuickPID_PonM.ino +++ b/examples/QuickPID_PonM/QuickPID_PonM.ino @@ -6,7 +6,7 @@ in certain processes like sous-vides. ********************************************************/ -#include +#include "QuickPID.h" //Define Variables we'll be connecting to int Setpoint, Input, Output; diff --git a/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino b/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino index 24fc827..b7a01ae 100644 --- a/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino +++ b/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino @@ -6,7 +6,7 @@ https://github.com/Dlloydev/QuickPID/wiki/QuickPID_RC_Filter **************************************************************/ -#include +#include "QuickPID.h" #define PIN_INPUT 0 #define PIN_OUTPUT 3 diff --git a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino index 5cd41c5..be3997a 100644 --- a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino +++ b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino @@ -16,7 +16,7 @@ be on for a minimum amount of time. ********************************************************/ -#include +#include "QuickPID.h" #define PIN_INPUT 0 #define RELAY_PIN 6 diff --git a/library.json b/library.json index 19c9f79..6bba657 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "QuickPID controller - a faster implementation of the Arduino PID Library with more features. This hybrid fixed/floating point controller seeks to keep an output close to a desired setpoint by adjusting four parameters (P,I,D) and POn (PonE, PonM setpoint weighting).", + "description": "QuickPID controller - a faster implementation of the Arduino PID Library with more features. This hybrid fixed/floating point controller seeks to keep an output close to a desired setpoint by adjusting four parameters (P,I,D) and proportional setpoint weighting (POn).", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index b81a3f9..fe839f9 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=QuickPID -version=2.0.4 +version=2.0.5 author=dlloydev maintainer=David Lloyd sentence=QuickPID controller - a faster hybrid fixed/floating point implementation of the Arduino PID Library with more features. -paragraph=This controller seeks to keep an output close to a desired setpoint by adjusting four parameters (P,I,D) and POn (PonE, PonM setpoint weighting). +paragraph=This controller seeks to keep an output close to a desired setpoint by adjusting four parameters (P,I,D) and proportional setpoint weighting (POn). category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID architectures=avr From a55f3413be7a289620d37d491fdb13706b495e90 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 30 Jan 2021 14:11:31 -0500 Subject: [PATCH 020/101] New Version with AutoTune --- QuickPID.cpp | 139 ++++++++++++++---- QuickPID.h | 17 ++- README.md | 53 +++++-- .../AutoTune_RC_Filter/AutoTune_RC_Filter.ino | 66 +++++++++ .../QuickPID_RC_Filter/QuickPID_RC_Filter.ino | 65 -------- keywords.txt | 2 + library.json | 2 +- library.properties | 9 +- 8 files changed, 235 insertions(+), 118 deletions(-) create mode 100644 examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino delete mode 100644 examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino diff --git a/QuickPID.cpp b/QuickPID.cpp index 0d412c2..96fc2c4 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.0.5 + QuickPID Library for Arduino - Version 2.1.0 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard @@ -27,12 +27,12 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, inAuto = false; QuickPID::SetOutputLimits(0, 255); // default is same as the arduino PWM limit - SampleTimeUs = 100000; // default is 0.1 seconds + sampleTimeUs = 100000; // default is 0.1 seconds QuickPID::SetControllerDirection(ControllerDirection); QuickPID::SetTunings(Kp, Ki, Kd, POn); - lastTime = micros() - SampleTimeUs; + lastTime = micros() - sampleTimeUs; } /* Constructor ******************************************************************** @@ -58,22 +58,18 @@ bool QuickPID::Compute() if (!inAuto) return false; uint32_t now = micros(); uint32_t timeChange = (now - lastTime); - if (timeChange >= SampleTimeUs) - { - /*Compute all the working error variables*/ + + if (timeChange >= sampleTimeUs) { // Compute the working error variables int16_t input = *myInput; int16_t dInput = input - lastInput; error = *mySetpoint - input; - /*Working error, Proportional distribution and Remaining PID output*/ - if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); - else outputSum += (kpi * error) - (kpd * dInput); + if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); // fixed-point + else outputSum += (kpi * error) - (kpd * dInput); // floating-point - if (outputSum > outMax) outputSum = outMax; - if (outputSum < outMin) outputSum = outMin; + outputSum = QuickPID::Saturate(outputSum); *myOutput = outputSum; - /*Remember some variables for next time*/ lastInput = input; lastTime = now; return true; @@ -81,6 +77,81 @@ bool QuickPID::Compute() else return false; } +/* AutoTune(...)*************************************************************************** + This function uses the Relay Method to tune the loop without operator intervention. + It determines the critical gain (Ku) and critical period (Tu) which is used in the + Ziegler-Nichols tuning rules to compute Kp, Ki and Kd. + ******************************************************************************************/ +void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 30) { + const byte outputStep = 2; // ±2 = span of 4 + const byte hysteresis = 1; // ±1 = span of 2 + + static uint32_t stepPrevTime, stepTime; + int peakHigh = 341, peakLow = 341; // Why 341? Because its exacly 1/3 of the 10-bit + *myInput = 341; // ADC max and this perfectly matches the 8-bit + *mySetpoint = 341; // PWM value (85/255 = 1/3). We need 0% digital bias + *myOutput = 85; // for a symetrical waveform over the setpoint. + + float Ku, Tu; + bool stepDir = true; + + analogWrite(outputPin, 85); + if (Print == 1) Serial.print("Settling at 33% for 7 sec "); + for (int i = 0; i < 7; i++) { + delay(1000); + if (Print == 1) Serial.print("."); + } + if (Print == 1) Serial.println(); + if (Print == 1) Serial.print("AutoTune:"); + + for (int i = 0; i < 16; i++) { // fill the rolling average + *myInput = analogReadAvg(inputPin); + } + + timeout *= 1000; + + do { //oscillate the output based on the input's relation to the setpoint + if (*myInput > *mySetpoint + hysteresis) { // input passes +'ve hysteresis + *myOutput = 85 - outputStep; // step down + if (*myInput > peakHigh) peakHigh = *myInput; // update peakHigh + if (!stepDir) { // if previous direction was down + stepPrevTime = stepTime; // update step time variables + stepTime = micros(); + stepDir = true; + } + } + else if (*myInput < *mySetpoint - hysteresis) { // input passes -'ve hysteresis + *myOutput = 85 + outputStep; // step up + if (stepPrevTime && (*myInput < peakLow)) peakLow = *myInput; // update peakLow only if prev peakHigh timing > 0 + stepDir = false; + } + *myInput = analogReadAvg(inputPin); // update input (rolling average) + analogWrite(outputPin, *myOutput); // update output + Ku = (float)(4 * outputStep * 2) / (float)(3.14159 * sqrt (sq (peakHigh - peakLow) - sq (hysteresis * 2))); // critical gain + Tu = (float)(stepTime - stepPrevTime) / 1000000.0; // critical period + delay(2); // allow some iteration time + } while ((isinf(Ku) || isnan(Ku)) && (millis() <= timeout)); + + if (tuningRule == 0) { // Ziegler Nichols PID + kp = 0.6 * Ku; + ki = 1.2 * Ku / Tu; + kd = 0.075 * Ku * Tu; + } else { // Ziegler Nichols PI + kp = 0.45 * Ku; + ki = 0.54 * Ku / Tu; + kd = 0.0; + } + + if (Print == 1) { + Serial.print(" Ku: "); Serial.print(Ku); + Serial.print(" Tu: "); Serial.print(Tu); + Serial.print(" Kp: "); Serial.print(kp); + Serial.print(" Ki: "); Serial.print(ki); + Serial.print(" Kd: "); Serial.println(kd); + } + SetTunings(kp, ki, kd); +} + /* SetTunings(....)************************************************************ This function allows the controller's dynamic performance to be adjusted. it's called automatically from the constructor, but tunings can also @@ -93,12 +164,12 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) pOn = POn; dispKp = Kp; dispKi = Ki; dispKd = Kd; - float SampleTimeSec = (float)SampleTimeUs / 1000000; + float SampleTimeSec = (float)sampleTimeUs / 1000000; kp = Kp; ki = Ki * SampleTimeSec; kd = Kd / SampleTimeSec; - kpi = kp * pOn + ki; - kpd = kp * (1 - pOn) + kd; + kpi = (kp * pOn) + ki; + kpd = (kp * (1 - pOn)) + kd; if (controllerDirection == REVERSE) { @@ -120,16 +191,20 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { ******************************************************************************/ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) { - if (NewSampleTimeUs > 0) - { - float ratio = (float)NewSampleTimeUs / (float)SampleTimeUs; + if (NewSampleTimeUs > 0) { + float ratio = (float)NewSampleTimeUs / (float)sampleTimeUs; ki *= ratio; kd /= ratio; - - SampleTimeUs = NewSampleTimeUs; + sampleTimeUs = NewSampleTimeUs; } } +int16_t QuickPID::Saturate(int16_t Out) { + if (Out > outMax) Out = outMax; + else if (Out < outMin) Out = outMin; + return Out; +} + /* SetOutputLimits(...)******************************************************** The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. @@ -142,11 +217,8 @@ void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) if (inAuto) { - if (*myOutput > outMax) *myOutput = outMax; - else if (*myOutput < outMin) *myOutput = outMin; - - if (outputSum > outMax) outputSum = outMax; - else if (outputSum < outMin) outputSum = outMin; + *myOutput = QuickPID::Saturate(*myOutput); + outputSum = QuickPID::Saturate(outputSum); } } @@ -173,8 +245,7 @@ void QuickPID::Initialize() { outputSum = *myOutput; lastInput = *myInput; - if (outputSum > outMax) outputSum = outMax; - else if (outputSum < outMin) outputSum = outMin; + outputSum = QuickPID::Saturate(outputSum); } /* SetControllerDirection(...)************************************************* @@ -243,3 +314,17 @@ int QuickPID::analogReadFast(int ADCpin) { return analogRead(ADCpin); # endif } + +int QuickPID::analogReadAvg(int ADCpin) +{ + static int arrDat[16]; + static int dat; + static int pos; + static long sum; + dat = QuickPID::analogReadFast(ADCpin); + pos++; + if (pos >= 16) pos = 0; + sum = sum - arrDat[pos] + dat; + arrDat[pos] = dat; + return sum / 16; +} diff --git a/QuickPID.h b/QuickPID.h index 40869a9..4c176ba 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -7,15 +7,14 @@ class QuickPID public: //Constants used in some of the functions below -#define AUTOMATIC 1 -#define MANUAL 0 +#define AUTOMATIC 1 +#define MANUAL 0 #define DIRECT 0 #define REVERSE 1 #define FL_FX(a) (int32_t)(a*256.0) // float to fixed point #define FX_MUL(a,b) ((a*b)>>8) // fixed point multiply - // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. @@ -31,8 +30,13 @@ class QuickPID // can be set using SetMode and SetSampleTime respectively. bool Compute(); - // Clamps the output to a specific range. 0-255 by default, but it's likely the user will want to change this - // depending on the application. + // Automatically determines and sets the tuning parameters Kp, Ki and Kd. Use this in the setup loop. + void AutoTune(int, int, int, int, uint32_t); + + // Clamps the output to its pre-determined limits. + int16_t Saturate(int16_t); + + // Sets and clamps the output to a specific range (0-255 by default). void SetOutputLimits(int16_t, int16_t); // available but not commonly used functions ****************************************************************** @@ -61,6 +65,7 @@ class QuickPID // Utility functions ****************************************************************************************** int analogReadFast(int); + int analogReadAvg(int); private: void Initialize(); @@ -82,7 +87,7 @@ class QuickPID int16_t *myOutput; // hard link between the variables and the PID, freeing the user from having int16_t *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. - uint32_t SampleTimeUs, lastTime; + uint32_t sampleTimeUs, lastTime; int16_t outMin, outMax, error; int16_t lastInput, outputSum; bool inAuto; diff --git a/README.md b/README.md index 8ed3a8a..cc3e797 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # QuickPID -A fast hybrid fixed-point and floating-point PID controller for Arduino. +QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function. This controller can automatically determine and set parameters (Kp, Ki, Kd). The POn setting controls the mix of Proportional on Errror to Proportional on Measurement and can be used to set the desired amount of overshoot. ### About @@ -11,7 +11,8 @@ This PID controller provides a faster *read-compute-write* cycle than alternativ Development began with a fork of the Arduino PID Library. Some modifications and new features have been added as follows: - Quicker hybrid fixed/floating point math in compute function -- `POn` parameter now controls the setpoint weighting of Proportional on Error and Proportional on Measurement +- Built-in `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. Also determines critical gain `Ku` and critical period `Tu` of the control system +- `POn` parameter now controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement - Reorganized and more efficient PID algorithm - micros() timing resolution - Faster analog read function @@ -30,7 +31,7 @@ Development began with a fork of the Arduino PID Library. Some modifications and ### Self Test Example (RC Filter): -[This example](https://github.com/Dlloydev/QuickPID/wiki/QuickPID_RC_Filter) allows you to experiment with the four tuning parameters. +[This example](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) allows you to experiment with the AutoTune and POn control on an RC filter. ### Simplified PID Algorithm @@ -38,7 +39,7 @@ Development began with a fork of the Arduino PID Library. Some modifications and outputSum += (kpi * error) - (kpd * dInput); ``` -The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required The pOn variable controls the setpoint weighting of Proportional on Error and Proportional on Measurement. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated as follows: +The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required. The pOn variable controls the setpoint weighting of Proportional on Error and Proportional on Measurement. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated as follows: ```c++ kpi = kp * pOn + ki; @@ -77,6 +78,33 @@ bool QuickPID::Compute() This function contains the PID algorithm and it should be called once every loop(). Most of the time it will just return false without doing anything. However, at a frequency specified by `SetSampleTime` it will calculate a new Output and return true. +#### AutoTune + +```c++ +void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 30) +``` + +The `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. It also determines the critical gain `Ku` and critical period `Tu` of the control system. + +`int tuningRule = 0; // PID(0), PI(1)` + +Selects the appropriate Ziegler–Nichols tuning rule for the PID or PI type controller. + +| Controller | Kp | Ki | Kd | +| ---------- | ----------- | ---------------- | ----------------- | +| PI | `0.45 * Ku` | `0.54 * Ku / Tu` | `0` | +| PID | `0.6 * Ku` | `1.2 * Ku / Tu` | `0.075 * Ku * Tu` | + +`int Print = 0; // on(1), off(0)` + +When using Serial Monitor, turn on serial print output to view the AutoTune status and results. When using the Serial Plotter, turn off the AutoTune print output to prevent plot labels from being overwritten. + +`uint32_t timeout = 30` + +Sets the AutoTune timeout where the default is 30 seconds. + +For more inormation, see [QuickPID AutoTune.](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) + #### SetTunings ```c++ @@ -150,21 +178,16 @@ These functions query the internal state of the PID. They're here for display pu int QuickPID::analogReadFast(int ADCpin) ``` -A faster configuration of `analogRead()`where a preset of 32 is used. Works with the following defines: - -`__AVR_ATmega328P__` +A faster configuration of `analogRead()`where a preset of 32 is used. If the architecture definition isn't found, normal `analogRead()`is used to return a value. -`__AVR_ATtiny_Zero_One__` - -`__AVR_ATmega_Zero__` - -`__AVR_DA__` +### Change Log - If the definition isn't found, normal `analogRead()`is used to return a value. +#### Version 2.1.0 (latest) -### Change Log +- Added AutoTune function and documentation +- Added AutoTune_RC_Filter example and documentation -#### Version 2.0.5 (latest) +#### Version 2.0.5 - Added MIT license text file - POn defaults to 1 diff --git a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino new file mode 100644 index 0000000..26a5763 --- /dev/null +++ b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino @@ -0,0 +1,66 @@ +/************************************************************** + PID AutoTune RC Filter Example: + One 47µF capacitor connected from GND to a 10K-100K resistor + terminated at pwm pin 3. Junction point of the RC filter + is connected to A0. Use Serial Plotter to view results. + https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter + **************************************************************/ + +#include "QuickPID.h" + +const byte inputPin = 0; +const byte outputPin = 3; + +// Define Variables +int Input, Output, Setpoint; + +unsigned long before, after, timeout = 30; +int loopCount = 0; +int Print = 0; // on(1), off(0) +int tuningRule = 0; // PID(0), PI(1) + +// Specify the initial tuning parameters +float Kp = 0, Ki = 0, Kd = 0; +float POn = 0.5; // Mix of PonE to PonM (0.0-1.0) + +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); + +void setup() +{ + Serial.begin(115200); + myQuickPID.AutoTune(inputPin, outputPin, tuningRule, Print, timeout); + myQuickPID.SetMode(AUTOMATIC); + analogWrite(outputPin, 0); // discharge capacitor + delay(1000); + Setpoint = 700; +} + +void loop() +{ + Serial.print("Setpoint:"); + Serial.print(Setpoint); + Serial.print(","); + Serial.print("Input:"); + Serial.print(Input); + Serial.print(","); + Serial.print("Output:"); + Serial.print(Output); + Serial.print(","); + Serial.print("Runtime:"); + Serial.print(after - before); + Serial.println(","); + + Input = myQuickPID.analogReadFast(inputPin); + before = micros(); + myQuickPID.Compute(); + after = micros(); + analogWrite(outputPin, Output); + + delay(20); // sets loop timing + loopCount++; + if (loopCount >= 250) { + analogWrite(outputPin, 0); + delay(1000); // discharge capacitor + loopCount = 0; + } +} diff --git a/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino b/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino deleted file mode 100644 index b7a01ae..0000000 --- a/examples/QuickPID_RC_Filter/QuickPID_RC_Filter.ino +++ /dev/null @@ -1,65 +0,0 @@ -/************************************************************** - PID RC Filter Example: - One 47µF capacitor connected from GND to a 10K resistor - terminated at pwm pin 3. Junction point of the RC filter - is connected to A0. Use Serial Plotter to view results. - https://github.com/Dlloydev/QuickPID/wiki/QuickPID_RC_Filter - **************************************************************/ - -#include "QuickPID.h" - -#define PIN_INPUT 0 -#define PIN_OUTPUT 3 - -//Define Variables -int Setpoint = 700; -int Input; -int Output; - -unsigned long before, after; -int cnt = 0; - -//Specify the initial tuning parameters -float Kp = 2.0, Ki = 15.0, Kd = 0.05; -float POn = 1.0; // Range is 0.0 to 1.0 (1.0 is 100% P on Error, 0% P on Measurement) - -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); - -void setup() -{ - Serial.begin(115200); - myQuickPID.SetTunings(Kp, Ki, Kd); - myQuickPID.SetMode(AUTOMATIC); - analogWrite(PIN_OUTPUT, 0); // discharge capacitor - delay(1000); -} - -void loop() -{ - Serial.print("Setpoint:"); - Serial.print(Setpoint); - Serial.print(","); - Serial.print("Input:"); - Serial.print(Input); - Serial.print(","); - Serial.print("Output:"); - Serial.print(Output); - Serial.print(","); - Serial.print("Runtime:"); - Serial.print(after - before); - Serial.println(","); - - Input = myQuickPID.analogReadFast(PIN_INPUT); - before = micros(); - myQuickPID.Compute(); - after = micros(); - analogWrite(PIN_OUTPUT, Output); - - delay(10); - cnt++; - if (cnt == 250) { - analogWrite(PIN_OUTPUT, 0); - delay(1000); // discharge capacitor - cnt = 0; - } -} diff --git a/keywords.txt b/keywords.txt index 3044ccc..c4bb91e 100644 --- a/keywords.txt +++ b/keywords.txt @@ -14,6 +14,7 @@ QuickPID KEYWORD1 SetMode KEYWORD2 Compute KEYWORD2 +AutoTune KEYWORD2 SetOutputLimits KEYWORD2 SetTunings KEYWORD2 SetControllerDirection KEYWORD2 @@ -25,6 +26,7 @@ GetMode KEYWORD2 GetError KEYWORD2 GetDirection KEYWORD2 analogReadFast KEYWORD2 +analogReadAvg KEYWORD2 ########################################## # Constants (LITERAL1) diff --git a/library.json b/library.json index 6bba657..42ab2df 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "QuickPID controller - a faster implementation of the Arduino PID Library with more features. This hybrid fixed/floating point controller seeks to keep an output close to a desired setpoint by adjusting four parameters (P,I,D) and proportional setpoint weighting (POn).", + "description": "QuickPID is a fast fixed/floating point implementation of the Arduino PID library. With a built-in AutoTune function, this controller can automatically determine and set parameters (P,I,D). The POn setting controls the mix of Proportional on Errror to Proportional on Measurement and can be used to set the desired amount of overshoot.", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index fe839f9..6cd11c2 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,10 @@ name=QuickPID -version=2.0.5 +version=2.1.0 author=dlloydev maintainer=David Lloyd -sentence=QuickPID controller - a faster hybrid fixed/floating point implementation of the Arduino PID Library with more features. -paragraph=This controller seeks to keep an output close to a desired setpoint by adjusting four parameters (P,I,D) and proportional setpoint weighting (POn). +sentence=QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function. +paragraph=This controller can automatically determine and set parameters (P,I,D). The POn setting controls the mix of Proportional on Errror to Proportional on Measurement and can be used to set the desired amount of overshoot. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID -architectures=avr +architectures=* +includes=QuickPID.h From 275a0023cdc9000a8d4c2b12a4043b24bf2db13c Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 30 Jan 2021 17:41:32 -0500 Subject: [PATCH 021/101] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index cc3e797..fbe5b8f 100644 --- a/README.md +++ b/README.md @@ -29,7 +29,7 @@ Development began with a fork of the Arduino PID Library. Some modifications and | QuickPID using `analogReadFast()` | 2.0 | 5.0 | 0.2 | 96 | | Arduino PID using `analogRead()` | 2.0 | 5.0 | 0.2 | 224 | -### Self Test Example (RC Filter): +### AutoTune RC Filter [This example](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) allows you to experiment with the AutoTune and POn control on an RC filter. From d20a0cac792b6e3f5fae6a4695198be87e6b031d Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 3 Feb 2021 21:44:17 -0500 Subject: [PATCH 022/101] Update README.md --- README.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index fbe5b8f..5beccf1 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ # QuickPID +[![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) + QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function. This controller can automatically determine and set parameters (Kp, Ki, Kd). The POn setting controls the mix of Proportional on Errror to Proportional on Measurement and can be used to set the desired amount of overshoot. ### About @@ -29,9 +31,9 @@ Development began with a fork of the Arduino PID Library. Some modifications and | QuickPID using `analogReadFast()` | 2.0 | 5.0 | 0.2 | 96 | | Arduino PID using `analogRead()` | 2.0 | 5.0 | 0.2 | 224 | -### AutoTune RC Filter +### [AutoTune RC Filter](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) -[This example](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) allows you to experiment with the AutoTune and POn control on an RC filter. +This example allows you to experiment with the AutoTune and POn control on an RC filter. ### Simplified PID Algorithm @@ -182,7 +184,7 @@ A faster configuration of `analogRead()`where a preset of 32 is used. If the ar ### Change Log -#### Version 2.1.0 (latest) +#### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) - Added AutoTune function and documentation - Added AutoTune_RC_Filter example and documentation From ab8da729ea7f99b3950085ad4f178faaceb9fd5c Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Thu, 11 Feb 2021 17:51:42 -0500 Subject: [PATCH 023/101] QuickPID 2.2.0 Includes enhanced AutoTune, 8 tuning rules to choose from and GetKu() and GetTu() display functions. --- QuickPID.cpp | 231 ++++++++++-------- QuickPID.h | 23 +- README.md | 51 ++-- .../AutoTune_RC_Filter/AutoTune_RC_Filter.ino | 67 +++-- keywords.txt | 3 +- library.json | 2 +- library.properties | 6 +- 7 files changed, 209 insertions(+), 174 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 96fc2c4..b4e7621 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.1.0 + QuickPID Library for Arduino - Version 2.2.0 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard @@ -79,84 +79,78 @@ bool QuickPID::Compute() /* AutoTune(...)*************************************************************************** This function uses the Relay Method to tune the loop without operator intervention. - It determines the critical gain (Ku) and critical period (Tu) which is used in the + It determines the ultimate gain (Ku) and ultimate period (Tu) which is used in the Ziegler-Nichols tuning rules to compute Kp, Ki and Kd. ******************************************************************************************/ -void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 30) { - const byte outputStep = 2; // ±2 = span of 4 - const byte hysteresis = 1; // ±1 = span of 2 - - static uint32_t stepPrevTime, stepTime; - int peakHigh = 341, peakLow = 341; // Why 341? Because its exacly 1/3 of the 10-bit - *myInput = 341; // ADC max and this perfectly matches the 8-bit - *mySetpoint = 341; // PWM value (85/255 = 1/3). We need 0% digital bias - *myOutput = 85; // for a symetrical waveform over the setpoint. +void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 120) { + uint32_t stepPrevTime, stepTime; float Ku, Tu; - bool stepDir = true; - analogWrite(outputPin, 85); - if (Print == 1) Serial.print("Settling at 33% for 7 sec "); - for (int i = 0; i < 7; i++) { - delay(1000); - if (Print == 1) Serial.print("."); - } + const int Rule[10][3] = + { //ckp, cki, ckd x 1000 + { 450, 540, 0 }, // ZIEGLER_NICHOLS_PI + { 600, 1176, 75 }, // ZIEGLER_NICHOLS_PID + { 313, 142, 0 }, // TYREUS_LUYBEN_PI + { 454, 206, 72 }, // TYREUS_LUYBEN_PID + { 303, 1212, 0 }, // CIANCONE_MARLIN_PI + { 303, 1333, 37 }, // CIANCONE_MARLIN_PID + { 0, 0, 0 }, // AMIGOF_PI (reserved) + { 700, 1750, 105 }, // PESSEN_INTEGRAL_PID + { 333, 667, 111 }, // SOME_OVERSHOOT_PID + { 200, 400, 67 }, // NO_OVERSHOOT_PID + }; + const byte ckp = 0, cki = 1, ckd = 2; // c = column + peakHigh = atSetpoint; + peakLow = atSetpoint; + timeout *= 1000; if (Print == 1) Serial.println(); - if (Print == 1) Serial.print("AutoTune:"); - - for (int i = 0; i < 16; i++) { // fill the rolling average - *myInput = analogReadAvg(inputPin); - } + if (Print == 1) Serial.print("Stabilizing (33%) →"); + QuickPID::Stabilize(inputPin, outputPin, timeout); + if (Print == 1) Serial.print("→ Running AutoTune"); + QuickPID::StepUp(inputPin, outputPin, timeout); + stepPrevTime = micros(); + if (Print == 1) Serial.print(" ↑"); + QuickPID::StepDown(inputPin, outputPin, timeout); + if (Print == 1) Serial.print(" ↓"); + QuickPID::StepUp(inputPin, outputPin, timeout); + stepTime = micros(); + if (Print == 1) Serial.print(" ↑"); + QuickPID::StepDown(inputPin, outputPin, timeout); + if (Print == 1) Serial.print(" ↓"); + QuickPID::StepUp(inputPin, outputPin, timeout); + if (Print == 1) Serial.print(" ↑"); + stepTime = micros(); + Tu = (float)(stepTime - stepPrevTime) / 2000000.0; // ultimate period based on 2 cycles + dispTu = Tu; + Ku = (float)(4 * outputStep * 2) / (float)(3.14159 * sqrt (sq (peakHigh - peakLow) - sq (hysteresis))); // ultimate gain + dispKu = Ku; - timeout *= 1000; + if (Print == 1) { + Serial.println(); Serial.print("Ultimate Period Tu: "); Serial.print(Tu, 3); Serial.println("s"); + Serial.print("Ultimate Gain Ku: "); Serial.println(Ku, 3); - do { //oscillate the output based on the input's relation to the setpoint - if (*myInput > *mySetpoint + hysteresis) { // input passes +'ve hysteresis - *myOutput = 85 - outputStep; // step down - if (*myInput > peakHigh) peakHigh = *myInput; // update peakHigh - if (!stepDir) { // if previous direction was down - stepPrevTime = stepTime; // update step time variables - stepTime = micros(); - stepDir = true; - } - } - else if (*myInput < *mySetpoint - hysteresis) { // input passes -'ve hysteresis - *myOutput = 85 + outputStep; // step up - if (stepPrevTime && (*myInput < peakLow)) peakLow = *myInput; // update peakLow only if prev peakHigh timing > 0 - stepDir = false; - } - *myInput = analogReadAvg(inputPin); // update input (rolling average) - analogWrite(outputPin, *myOutput); // update output - Ku = (float)(4 * outputStep * 2) / (float)(3.14159 * sqrt (sq (peakHigh - peakLow) - sq (hysteresis * 2))); // critical gain - Tu = (float)(stepTime - stepPrevTime) / 1000000.0; // critical period - delay(2); // allow some iteration time - } while ((isinf(Ku) || isnan(Ku)) && (millis() <= timeout)); - - if (tuningRule == 0) { // Ziegler Nichols PID - kp = 0.6 * Ku; - ki = 1.2 * Ku / Tu; - kd = 0.075 * Ku * Tu; - } else { // Ziegler Nichols PI - kp = 0.45 * Ku; - ki = 0.54 * Ku / Tu; - kd = 0.0; + Serial.print("peakHigh: "); Serial.println(peakHigh); + Serial.print("peakLow: "); Serial.println(peakLow); } + kp = Rule[tuningRule][ckp] / 1000.0 * Ku; + ki = Rule[tuningRule][cki] / 1000.0 * Ku / Tu; + kd = Rule[tuningRule][ckd] / 1000.0 * Ku * Tu; + if (Print == 1) { - Serial.print(" Ku: "); Serial.print(Ku); - Serial.print(" Tu: "); Serial.print(Tu); - Serial.print(" Kp: "); Serial.print(kp); - Serial.print(" Ki: "); Serial.print(ki); - Serial.print(" Kd: "); Serial.println(kd); + Serial.print("Kp: "); Serial.print(kp, 3); + Serial.print(" Ki: "); Serial.print(ki, 3); + Serial.print(" Kd: "); Serial.println(kd, 3); } SetTunings(kp, ki, kd); } /* SetTunings(....)************************************************************ - This function allows the controller's dynamic performance to be adjusted. - it's called automatically from the constructor, but tunings can also - be adjusted on the fly during normal operation - ******************************************************************************/ + This function allows the controller's dynamic performance to be adjusted. + it's called automatically from the constructor, but tunings can also + be adjusted on the fly during normal operation +******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) { if (Kp < 0 || Ki < 0 || Kd < 0) return; @@ -180,15 +174,15 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) } /* SetTunings(...)************************************************************* - Set Tunings using the last remembered POn setting. - ******************************************************************************/ + Set Tunings using the last remembered POn setting. +******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { SetTunings(Kp, Ki, Kd, pOn); } /* SetSampleTime(...) ********************************************************* - Sets the period, in microseconds, at which the calculation is performed - ******************************************************************************/ + Sets the period, in microseconds, at which the calculation is performed +******************************************************************************/ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) { if (NewSampleTimeUs > 0) { @@ -199,16 +193,10 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) } } -int16_t QuickPID::Saturate(int16_t Out) { - if (Out > outMax) Out = outMax; - else if (Out < outMin) Out = outMin; - return Out; -} - /* SetOutputLimits(...)******************************************************** - The PID controller is designed to vary its output within a given range. - By default this range is 0-255, the Arduino PWM range. - ******************************************************************************/ + The PID controller is designed to vary its output within a given range. + By default this range is 0-255, the Arduino PWM range. +******************************************************************************/ void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) { if (Min >= Max) return; @@ -223,10 +211,10 @@ void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) } /* SetMode(...)**************************************************************** - Allows the controller Mode to be set to manual (0) or Automatic (non-zero) - when the transition from manual to auto occurs, the controller is - automatically initialized - ******************************************************************************/ + Allows the controller Mode to be set to manual (0) or Automatic (non-zero) + when the transition from manual to auto occurs, the controller is + automatically initialized +******************************************************************************/ void QuickPID::SetMode(bool Mode) { bool newAuto = (Mode == AUTOMATIC); @@ -238,9 +226,9 @@ void QuickPID::SetMode(bool Mode) } /* Initialize()**************************************************************** - Does all the things that need to happen to ensure a bumpless transfer - from manual to automatic mode. - ******************************************************************************/ + Does all the things that need to happen to ensure a bumpless transfer + from manual to automatic mode. +******************************************************************************/ void QuickPID::Initialize() { outputSum = *myOutput; @@ -249,11 +237,11 @@ void QuickPID::Initialize() } /* SetControllerDirection(...)************************************************* - The PID will either be connected to a DIRECT acting process (+Output leads - to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to - know which one, because otherwise we may increase the output when we should - be decreasing. This is called from the constructor. - ******************************************************************************/ + The PID will either be connected to a DIRECT acting process (+Output leads + to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to + know which one, because otherwise we may increase the output when we should + be decreasing. This is called from the constructor. +******************************************************************************/ void QuickPID::SetControllerDirection(bool Direction) { if (inAuto && Direction != controllerDirection) @@ -266,10 +254,10 @@ void QuickPID::SetControllerDirection(bool Direction) } /* Status Functions************************************************************ - Just because you set the Kp=-1 doesn't mean it actually happened. These - functions query the internal state of the PID. They're here for display - purposes. These are the functions the PID Front-end uses for example. - ******************************************************************************/ + Just because you set the Kp=-1 doesn't mean it actually happened. These + functions query the internal state of the PID. They're here for display + purposes. These are the functions the PID Front-end uses for example. +******************************************************************************/ float QuickPID::GetKp() { return dispKp; } @@ -279,15 +267,18 @@ float QuickPID::GetKi() { float QuickPID::GetKd() { return dispKd; } +float QuickPID::GetKu() { + return dispKu; +} +float QuickPID::GetTu() { + return dispTu; +} bool QuickPID::GetMode() { return inAuto ? AUTOMATIC : MANUAL; } bool QuickPID::GetDirection() { return controllerDirection; } -int16_t QuickPID::GetError() { - return error; -} // Utility functions ********************************************************** @@ -315,8 +306,7 @@ int QuickPID::analogReadFast(int ADCpin) { # endif } -int QuickPID::analogReadAvg(int ADCpin) -{ +float QuickPID::analogReadAvg(int ADCpin) { static int arrDat[16]; static int dat; static int pos; @@ -326,5 +316,50 @@ int QuickPID::analogReadAvg(int ADCpin) if (pos >= 16) pos = 0; sum = sum - arrDat[pos] + dat; arrDat[pos] = dat; - return sum / 16; + return (float)sum / 16.0; +} + +int16_t QuickPID::Saturate(int16_t Out) { + if (Out > outMax) Out = outMax; + else if (Out < outMin) Out = outMin; + return Out; +} + +void QuickPID::StepUp(int inputPin, int outputPin, uint32_t timeout) { + analogWrite (outputPin, (atOutput + outputStep)); // step up output, wait for input to reach +'ve hysteresis + while ((analogReadAvg(inputPin) < (atSetpoint + hysteresis)) && (millis() <= timeout)) { + float rdAvg = analogReadAvg(inputPin); + if (rdAvg > peakHigh) peakHigh = rdAvg; + if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; + delayMicroseconds(readPeriod); + } +} + +void QuickPID::StepDown(int inputPin, int outputPin, uint32_t timeout) { + analogWrite (outputPin, (atOutput - outputStep)); // step down output, wait for input to reach -'ve hysteresis + while ((analogReadAvg(inputPin) > (atSetpoint - hysteresis)) && (millis() <= timeout)) { + float rdAvg = analogReadAvg(inputPin); + if (rdAvg > peakHigh) peakHigh = rdAvg; + if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; + delayMicroseconds(readPeriod); + } +} + +void QuickPID::Stabilize(int inputPin, int outputPin, uint32_t timeout) { + // initial reading + for (int i = 0; i <= 16; i++) { + analogReadAvg(inputPin); + } + // coarse adjust + analogWrite (outputPin, 0); + while ((analogReadAvg(inputPin) > atSetpoint) && (millis() <= timeout)); + analogWrite(outputPin, atOutput * 2); + while ((analogReadAvg(inputPin) < atSetpoint) && (millis() <= timeout)); + + // fine adjust + analogWrite (outputPin, atOutput - outputStep - 1); + while ((analogReadAvg(inputPin) > atSetpoint) && (millis() <= timeout)); + analogWrite(outputPin, atOutput + outputStep + 1); + while ((analogReadAvg(inputPin) < atSetpoint) && (millis() <= timeout)); + analogWrite(outputPin, atOutput); } diff --git a/QuickPID.h b/QuickPID.h index 4c176ba..eec5f4e 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -33,9 +33,6 @@ class QuickPID // Automatically determines and sets the tuning parameters Kp, Ki and Kd. Use this in the setup loop. void AutoTune(int, int, int, int, uint32_t); - // Clamps the output to its pre-determined limits. - int16_t Saturate(int16_t); - // Sets and clamps the output to a specific range (0-255 by default). void SetOutputLimits(int16_t, int16_t); @@ -59,20 +56,27 @@ class QuickPID float GetKp(); // These functions query the pid for interal values. They were created mainly for float GetKi(); // the pid front-end, where it's important to know what is actually inside the PID. float GetKd(); + float GetKu(); + float GetTu(); bool GetMode(); bool GetDirection(); - int16_t GetError(); // Utility functions ****************************************************************************************** int analogReadFast(int); - int analogReadAvg(int); + float analogReadAvg(int); private: void Initialize(); + int16_t Saturate(int16_t); + void StepUp(int, int, uint32_t); + void StepDown(int, int, uint32_t); + void Stabilize(int, int, uint32_t); float dispKp; // We'll hold on to the tuning parameters for display purposes. float dispKi; float dispKd; + float dispKu; + float dispTu; float pOn; // proportional mode (0-1) default = 1, 100% Proportional on Error float kp; // (P)roportional Tuning Parameter @@ -91,6 +95,15 @@ class QuickPID int16_t outMin, outMax, error; int16_t lastInput, outputSum; bool inAuto; + + // AutoTune + float peakHigh, peakLow; + const word readPeriod = 250; + const byte outputStep = 1; + const byte hysteresis = 1; + const int atSetpoint = 341; // 1/3 of 10-bit ADC matches 8-bit PWM value of 85 for symetrical waveform + const int atOutput = 85; + }; #endif diff --git a/README.md b/README.md index 5beccf1..8a01cd0 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,6 @@ -# QuickPID +# QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -[![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) - -QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function. This controller can automatically determine and set parameters (Kp, Ki, Kd). The POn setting controls the mix of Proportional on Errror to Proportional on Measurement and can be used to set the desired amount of overshoot. +QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) function. This controller can automatically determine and set parameters (Kp, Ki, Kd) and additionally determine the ultimate gain `Ku` and ultimate period `Tu`. There are 8 tuning rules available to choose from. Also, there is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. ### About @@ -10,15 +8,13 @@ This PID controller provides a faster *read-compute-write* cycle than alternativ ### Features -Development began with a fork of the Arduino PID Library. Some modifications and new features have been added as follows: +Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the change log and below: - Quicker hybrid fixed/floating point math in compute function -- Built-in `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. Also determines critical gain `Ku` and critical period `Tu` of the control system +- Built-in `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. and also ultimate gain `Ku` and ultimate period `Tu` of the control system. There are 8 tuning rules to choose from +- [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) uses a precise and low control effort sequence that gets results quickly - `POn` parameter now controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement -- Reorganized and more efficient PID algorithm -- micros() timing resolution -- Faster analog read function -- `GetError()` function added +- Reorganized and more efficient PID algorithm, faster analog read function, micros() timing resolution - Runs a complete PID cycle (*read-compute-write*) faster than just an `analogRead()` command in Arduino ### Performance @@ -33,7 +29,9 @@ Development began with a fork of the Arduino PID Library. Some modifications and ### [AutoTune RC Filter](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) -This example allows you to experiment with the AutoTune and POn control on an RC filter. +This example allows you to experiment with the AutoTune, various tuning rules and the POn control on an RC filter. + +#### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) ### Simplified PID Algorithm @@ -80,7 +78,7 @@ bool QuickPID::Compute() This function contains the PID algorithm and it should be called once every loop(). Most of the time it will just return false without doing anything. However, at a frequency specified by `SetSampleTime` it will calculate a new Output and return true. -#### AutoTune +#### [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) ```c++ void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 30) @@ -88,24 +86,13 @@ void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = The `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. It also determines the critical gain `Ku` and critical period `Tu` of the control system. -`int tuningRule = 0; // PID(0), PI(1)` - -Selects the appropriate Ziegler–Nichols tuning rule for the PID or PI type controller. - -| Controller | Kp | Ki | Kd | -| ---------- | ----------- | ---------------- | ----------------- | -| PI | `0.45 * Ku` | `0.54 * Ku / Tu` | `0` | -| PID | `0.6 * Ku` | `1.2 * Ku / Tu` | `0.075 * Ku * Tu` | - `int Print = 0; // on(1), off(0)` When using Serial Monitor, turn on serial print output to view the AutoTune status and results. When using the Serial Plotter, turn off the AutoTune print output to prevent plot labels from being overwritten. -`uint32_t timeout = 30` +`uint32_t timeout = 120` -Sets the AutoTune timeout where the default is 30 seconds. - -For more inormation, see [QuickPID AutoTune.](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) +Sets the AutoTune timeout where the default is 120 seconds. #### SetTunings @@ -151,7 +138,7 @@ Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (non-zero). void QuickPID::Initialize() ``` -Does all the things that need to happen to ensure a bumpless transfer from manual to automatic mode. +Does all the things that need to happen to ensure a bump-less transfer from manual to automatic mode. #### SetControllerDirection @@ -167,9 +154,10 @@ The PID will either be connected to a DIRECT acting process (+Output leads to +I float QuickPID::GetKp() float QuickPID::GetKi() float QuickPID::GetKd() +float QuickPID::GetKu() +float QuickPID::GetTu() bool QuickPID::GetMode() bool QuickPID::GetDirection() -int16_t QuickPID::GetError() ``` These functions query the internal state of the PID. They're here for display purposes. @@ -186,7 +174,16 @@ A faster configuration of `analogRead()`where a preset of 32 is used. If the ar #### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) +- Improved AutoTune function +- Added 8 tuning rules +- Added `GetKu()` function to display ultimate gain +- Added `GetTu()` function to display ultimate period (time constant) +- Updated example and documentation + +#### Version 2.1.0 + - Added AutoTune function and documentation + - Added AutoTune_RC_Filter example and documentation #### Version 2.0.5 diff --git a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino index 26a5763..87c1d90 100644 --- a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino +++ b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino @@ -1,66 +1,55 @@ -/************************************************************** - PID AutoTune RC Filter Example: - One 47µF capacitor connected from GND to a 10K-100K resistor - terminated at pwm pin 3. Junction point of the RC filter - is connected to A0. Use Serial Plotter to view results. - https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter - **************************************************************/ +/****************************************************************************** + AutoTune RC Filter Example + Use Serial Monitor and Serial Plotter to view results. + Reference: https://github.com/Dlloydev/QuickPID/wiki/AutoTune + Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter + + TUNING RULE RECOMMENED FOR + 0 ZIEGLER_NICHOLS_PI Good noise and disturbance rejection + 1 ZIEGLER_NICHOLS_PID Good noise and disturbance rejection + 2 TYREUS_LUYBEN_PI Time-constant (lag) dominant processes (conservative) + 3 TYREUS_LUYBEN_PID Time-constant (lag) dominant processes (conservative) + 4 CIANCONE_MARLIN_PI Delay (dead-time) dominant processes + 5 CIANCONE_MARLIN_PID Delay (dead-time) dominant processes + 6 AMIGOF_PI TURNS CONTROLLER OFF (reserved for future version) + 7 PESSEN_INTEGRAL_PID Similar to ZN_PID but with better dynamic response + 8 SOME_OVERSHOOT_PID ZN_PID with lower proportional and integral gain + 9 NO_OVERSHOOT_PID ZN_PID with much lower P,I,D gain settings + ******************************************************************************/ #include "QuickPID.h" const byte inputPin = 0; const byte outputPin = 3; -// Define Variables -int Input, Output, Setpoint; - -unsigned long before, after, timeout = 30; -int loopCount = 0; -int Print = 0; // on(1), off(0) -int tuningRule = 0; // PID(0), PI(1) - // Specify the initial tuning parameters +int Print = 1; // on(1), off(0) +int tuningRule = 2; // see above table +float POn = 1.0; // Mix of PonE to PonM (0.0-1.0) +unsigned long timeout = 120; // AutoTune timeout (sec) + +int Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; -float POn = 0.5; // Mix of PonE to PonM (0.0-1.0) QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); void setup() { Serial.begin(115200); + myQuickPID.SetSampleTimeUs(5000); // 5ms (ideally 10x faster than shortest RC time constant under test) myQuickPID.AutoTune(inputPin, outputPin, tuningRule, Print, timeout); myQuickPID.SetMode(AUTOMATIC); - analogWrite(outputPin, 0); // discharge capacitor - delay(1000); Setpoint = 700; } void loop() { - Serial.print("Setpoint:"); - Serial.print(Setpoint); - Serial.print(","); - Serial.print("Input:"); - Serial.print(Input); - Serial.print(","); - Serial.print("Output:"); - Serial.print(Output); - Serial.print(","); - Serial.print("Runtime:"); - Serial.print(after - before); + Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); + Serial.print("Input:"); Serial.print(Input); Serial.print(","); + Serial.print("Output:"); Serial.print(Output); Serial.print(","); Serial.println(","); Input = myQuickPID.analogReadFast(inputPin); - before = micros(); myQuickPID.Compute(); - after = micros(); analogWrite(outputPin, Output); - - delay(20); // sets loop timing - loopCount++; - if (loopCount >= 250) { - analogWrite(outputPin, 0); - delay(1000); // discharge capacitor - loopCount = 0; - } } diff --git a/keywords.txt b/keywords.txt index c4bb91e..9cbf41f 100644 --- a/keywords.txt +++ b/keywords.txt @@ -22,8 +22,9 @@ SetSampleTimeUs KEYWORD2 GetKp KEYWORD2 GetKi KEYWORD2 GetKd KEYWORD2 +GetKu KEYWORD2 +GetTu KEYWORD2 GetMode KEYWORD2 -GetError KEYWORD2 GetDirection KEYWORD2 analogReadFast KEYWORD2 analogReadAvg KEYWORD2 diff --git a/library.json b/library.json index 42ab2df..4a89888 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "QuickPID is a fast fixed/floating point implementation of the Arduino PID library. With a built-in AutoTune function, this controller can automatically determine and set parameters (P,I,D). The POn setting controls the mix of Proportional on Errror to Proportional on Measurement and can be used to set the desired amount of overshoot.", + "description": "QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function with 8 tuning rules to choose from. This controller can automatically determine and set parameters (P,I,D). The POn setting controls the mix of Proportional on Error to Proportional on Measurement.", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 6cd11c2..0131062 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=QuickPID -version=2.1.0 +version=2.2.0 author=dlloydev maintainer=David Lloyd -sentence=QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function. -paragraph=This controller can automatically determine and set parameters (P,I,D). The POn setting controls the mix of Proportional on Errror to Proportional on Measurement and can be used to set the desired amount of overshoot. +sentence=QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function with 8 tuning rules to choose from. +paragraph=This controller can automatically determine and set parameters (P,I,D). The POn setting controls the mix of Proportional on Error to Proportional on Measurement. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID architectures=* From 12697fcfd4ec7c3253328cd97a2a287068a34941 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 15 Feb 2021 20:13:04 -0500 Subject: [PATCH 024/101] New Version 2.2.1 - Even faster AutoTune function - AutoTune now determines the controllability of the process - Added AMIGO_PID tuning rule - Added `GetTd()` function to display dead time --- QuickPID.cpp | 109 +++++++++--------- QuickPID.h | 9 +- README.md | 18 ++- .../AutoTune_RC_Filter/AutoTune_RC_Filter.ino | 30 +++-- keywords.txt | 1 + library.json | 4 +- library.properties | 8 +- 7 files changed, 101 insertions(+), 78 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index b4e7621..afc5680 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.2.0 + QuickPID Library for Arduino - Version 2.2.1 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard @@ -84,8 +84,8 @@ bool QuickPID::Compute() ******************************************************************************************/ void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 120) { - uint32_t stepPrevTime, stepTime; - float Ku, Tu; + uint32_t t0, t1, t2, t3; + float Ku, Tu, td; const int Rule[10][3] = { //ckp, cki, ckd x 1000 @@ -95,7 +95,7 @@ void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = { 454, 206, 72 }, // TYREUS_LUYBEN_PID { 303, 1212, 0 }, // CIANCONE_MARLIN_PI { 303, 1333, 37 }, // CIANCONE_MARLIN_PID - { 0, 0, 0 }, // AMIGOF_PI (reserved) + { 0, 0, 0 }, // AMIGOF_PID { 700, 1750, 105 }, // PESSEN_INTEGRAL_PID { 333, 667, 111 }, // SOME_OVERSHOOT_PID { 200, 400, 67 }, // NO_OVERSHOOT_PID @@ -104,46 +104,53 @@ void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = peakHigh = atSetpoint; peakLow = atSetpoint; timeout *= 1000; - if (Print == 1) Serial.println(); if (Print == 1) Serial.print("Stabilizing (33%) →"); QuickPID::Stabilize(inputPin, outputPin, timeout); - if (Print == 1) Serial.print("→ Running AutoTune"); - QuickPID::StepUp(inputPin, outputPin, timeout); - stepPrevTime = micros(); - if (Print == 1) Serial.print(" ↑"); - QuickPID::StepDown(inputPin, outputPin, timeout); - if (Print == 1) Serial.print(" ↓"); - QuickPID::StepUp(inputPin, outputPin, timeout); - stepTime = micros(); - if (Print == 1) Serial.print(" ↑"); - QuickPID::StepDown(inputPin, outputPin, timeout); + if (Print == 1) Serial.print(" Running AutoTune ↑"); + t0 = micros(); + + analogWrite (outputPin, (atOutput + outputStep)); // step up output, wait for input to reach +'ve hysteresis + while ((analogReadAvg(inputPin) < (atSetpoint + 0.2)) && (millis() <= timeout)) QuickPID::CheckPeak(inputPin); + t1 = micros(); + + while ((analogReadAvg(inputPin) < (atSetpoint + hysteresis)) && (millis() <= timeout)) QuickPID::CheckPeak(inputPin); + t2 = micros(); + if (Print == 1) Serial.print(" ↓"); - QuickPID::StepUp(inputPin, outputPin, timeout); + analogWrite (outputPin, (atOutput - outputStep)); // step down output, wait for input to reach -'ve hysteresis + while ((analogReadAvg(inputPin) > (atSetpoint - hysteresis)) && (millis() <= timeout)) QuickPID::CheckPeak(inputPin); + if (Print == 1) Serial.print(" ↑"); - stepTime = micros(); - Tu = (float)(stepTime - stepPrevTime) / 2000000.0; // ultimate period based on 2 cycles + analogWrite (outputPin, (atOutput + outputStep)); // step up output, wait for input to reach +'ve hysteresis + while ((analogReadAvg(inputPin) < (atSetpoint + hysteresis)) && (millis() <= timeout)) QuickPID::CheckPeak(inputPin); + t3 = micros(); + if (Print == 1) Serial.println(" Done."); + + td = (float)(t1 - t0) / 1000000.0; // dead time (seconds) + dispTd = td; + + Tu = (float)(t3 - t2) / 1000000.0; // ultimate period (seconds) dispTu = Tu; + Ku = (float)(4 * outputStep * 2) / (float)(3.14159 * sqrt (sq (peakHigh - peakLow) - sq (hysteresis))); // ultimate gain dispKu = Ku; - if (Print == 1) { - Serial.println(); Serial.print("Ultimate Period Tu: "); Serial.print(Tu, 3); Serial.println("s"); - Serial.print("Ultimate Gain Ku: "); Serial.println(Ku, 3); - - Serial.print("peakHigh: "); Serial.println(peakHigh); - Serial.print("peakLow: "); Serial.println(peakLow); + if (tuningRule == 6) { //AMIGO_PID + (td < readPeriod) ? td = readPeriod : td = td; + kp = (0.2 + 0.45 * (Tu / td)) / Ku; + float Ti = (((0.4 * td) + (0.8 * Tu)) / (td + (0.1 * Tu)) * td); + float Td = (0.5 * td * Tu) / ((0.3 * td) + Tu); + ki = kp / Ti; + kd = Td * kp; + } else { //other rules + kp = Rule[tuningRule][ckp] / 1000.0 * Ku; + ki = Rule[tuningRule][cki] / 1000.0 * Ku / Tu; + kd = Rule[tuningRule][ckd] / 1000.0 * Ku * Tu; } - kp = Rule[tuningRule][ckp] / 1000.0 * Ku; - ki = Rule[tuningRule][cki] / 1000.0 * Ku / Tu; - kd = Rule[tuningRule][ckd] / 1000.0 * Ku * Tu; - - if (Print == 1) { - Serial.print("Kp: "); Serial.print(kp, 3); - Serial.print(" Ki: "); Serial.print(ki, 3); - Serial.print(" Kd: "); Serial.println(kd, 3); - } - SetTunings(kp, ki, kd); + dispKp = kp; + dispKi = ki; + dispKd = kd; } /* SetTunings(....)************************************************************ @@ -273,6 +280,9 @@ float QuickPID::GetKu() { float QuickPID::GetTu() { return dispTu; } +float QuickPID::GetTd() { + return dispTd; +} bool QuickPID::GetMode() { return inAuto ? AUTOMATIC : MANUAL; } @@ -325,24 +335,11 @@ int16_t QuickPID::Saturate(int16_t Out) { return Out; } -void QuickPID::StepUp(int inputPin, int outputPin, uint32_t timeout) { - analogWrite (outputPin, (atOutput + outputStep)); // step up output, wait for input to reach +'ve hysteresis - while ((analogReadAvg(inputPin) < (atSetpoint + hysteresis)) && (millis() <= timeout)) { - float rdAvg = analogReadAvg(inputPin); - if (rdAvg > peakHigh) peakHigh = rdAvg; - if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; - delayMicroseconds(readPeriod); - } -} - -void QuickPID::StepDown(int inputPin, int outputPin, uint32_t timeout) { - analogWrite (outputPin, (atOutput - outputStep)); // step down output, wait for input to reach -'ve hysteresis - while ((analogReadAvg(inputPin) > (atSetpoint - hysteresis)) && (millis() <= timeout)) { - float rdAvg = analogReadAvg(inputPin); - if (rdAvg > peakHigh) peakHigh = rdAvg; - if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; - delayMicroseconds(readPeriod); - } +void QuickPID::CheckPeak(int inputPin) { + float rdAvg = analogReadAvg(inputPin); + if (rdAvg > peakHigh) peakHigh = rdAvg; + if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; + delayMicroseconds(readPeriod); } void QuickPID::Stabilize(int inputPin, int outputPin, uint32_t timeout) { @@ -352,14 +349,14 @@ void QuickPID::Stabilize(int inputPin, int outputPin, uint32_t timeout) { } // coarse adjust analogWrite (outputPin, 0); - while ((analogReadAvg(inputPin) > atSetpoint) && (millis() <= timeout)); - analogWrite(outputPin, atOutput * 2); + while ((analogReadAvg(inputPin) > (atSetpoint - hysteresis)) && (millis() <= timeout)); + analogWrite(outputPin, atOutput + 20); while ((analogReadAvg(inputPin) < atSetpoint) && (millis() <= timeout)); // fine adjust - analogWrite (outputPin, atOutput - outputStep - 1); + analogWrite (outputPin, atOutput - outputStep); while ((analogReadAvg(inputPin) > atSetpoint) && (millis() <= timeout)); - analogWrite(outputPin, atOutput + outputStep + 1); + analogWrite(outputPin, atOutput + outputStep); while ((analogReadAvg(inputPin) < atSetpoint) && (millis() <= timeout)); analogWrite(outputPin, atOutput); } diff --git a/QuickPID.h b/QuickPID.h index eec5f4e..77cd489 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -56,8 +56,9 @@ class QuickPID float GetKp(); // These functions query the pid for interal values. They were created mainly for float GetKi(); // the pid front-end, where it's important to know what is actually inside the PID. float GetKd(); - float GetKu(); - float GetTu(); + float GetKu(); // Ultimate Gain + float GetTu(); // Ultimate Period + float GetTd(); // Dead Time bool GetMode(); bool GetDirection(); @@ -68,6 +69,7 @@ class QuickPID private: void Initialize(); int16_t Saturate(int16_t); + void CheckPeak(int); void StepUp(int, int, uint32_t); void StepDown(int, int, uint32_t); void Stabilize(int, int, uint32_t); @@ -77,6 +79,7 @@ class QuickPID float dispKd; float dispKu; float dispTu; + float dispTd; float pOn; // proportional mode (0-1) default = 1, 100% Proportional on Error float kp; // (P)roportional Tuning Parameter @@ -98,7 +101,7 @@ class QuickPID // AutoTune float peakHigh, peakLow; - const word readPeriod = 250; + const word readPeriod = 1667; const byte outputStep = 1; const byte hysteresis = 1; const int atSetpoint = 341; // 1/3 of 10-bit ADC matches 8-bit PWM value of 85 for symetrical waveform diff --git a/README.md b/README.md index 8a01cd0..aa78728 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) function. This controller can automatically determine and set parameters (Kp, Ki, Kd) and additionally determine the ultimate gain `Ku` and ultimate period `Tu`. There are 8 tuning rules available to choose from. Also, there is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. +QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) function. This controller can automatically determine and set parameters (Kp, Ki, Kd). Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and controllability of the process are determined. There are 9 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. ### About @@ -11,9 +11,9 @@ This PID controller provides a faster *read-compute-write* cycle than alternativ Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the change log and below: - Quicker hybrid fixed/floating point math in compute function -- Built-in `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. and also ultimate gain `Ku` and ultimate period `Tu` of the control system. There are 8 tuning rules to choose from -- [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) uses a precise and low control effort sequence that gets results quickly -- `POn` parameter now controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement +- Built-in `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. and also ultimate gain `Ku` and ultimate period `Tu` of the control system. There are 9 tuning rules to choose from +- [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) uses a precise and low control effort sequence that gets results quickly. It also determines how difficult the process is to control. +- `POn` parameter controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement - Reorganized and more efficient PID algorithm, faster analog read function, micros() timing resolution - Runs a complete PID cycle (*read-compute-write*) faster than just an `analogRead()` command in Arduino @@ -29,7 +29,7 @@ Development began with a fork of the Arduino PID Library. Modifications and new ### [AutoTune RC Filter](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) -This example allows you to experiment with the AutoTune, various tuning rules and the POn control on an RC filter. +This example allows you to experiment with the AutoTune, various tuning rules and the POn control on an RC filter. It automatically determines and sets the tuning parameters. #### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) @@ -156,6 +156,7 @@ float QuickPID::GetKi() float QuickPID::GetKd() float QuickPID::GetKu() float QuickPID::GetTu() +float QuickPID::GetTd() bool QuickPID::GetMode() bool QuickPID::GetDirection() ``` @@ -174,6 +175,13 @@ A faster configuration of `analogRead()`where a preset of 32 is used. If the ar #### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) +- Even faster AutoTune function +- AutoTune now determines the controllability of the process +- Added AMIGO_PID tuning rule +- Added `GetTd()` function to display dead time + +#### Version 2.2.0 + - Improved AutoTune function - Added 8 tuning rules - Added `GetKu()` function to display ultimate gain diff --git a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino index 87c1d90..1a31a3b 100644 --- a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino +++ b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino @@ -11,7 +11,7 @@ 3 TYREUS_LUYBEN_PID Time-constant (lag) dominant processes (conservative) 4 CIANCONE_MARLIN_PI Delay (dead-time) dominant processes 5 CIANCONE_MARLIN_PID Delay (dead-time) dominant processes - 6 AMIGOF_PI TURNS CONTROLLER OFF (reserved for future version) + 6 AMIGO_PID More universal than ZN_PID (uses a dead time dependancy) 7 PESSEN_INTEGRAL_PID Similar to ZN_PID but with better dynamic response 8 SOME_OVERSHOOT_PID ZN_PID with lower proportional and integral gain 9 NO_OVERSHOOT_PID ZN_PID with much lower P,I,D gain settings @@ -22,11 +22,10 @@ const byte inputPin = 0; const byte outputPin = 3; -// Specify the initial tuning parameters -int Print = 1; // on(1), off(0) -int tuningRule = 2; // see above table -float POn = 1.0; // Mix of PonE to PonM (0.0-1.0) -unsigned long timeout = 120; // AutoTune timeout (sec) +int Print = 0; // on(1) monitor, off(0) plotter +int tuningRule = 1; // see above table +float POn = 1.0; // Mix of PonE to PonM (0.0-1.0) +unsigned long timeout = 120; // AutoTune timeout (sec) int Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; @@ -36,14 +35,29 @@ QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); void setup() { Serial.begin(115200); - myQuickPID.SetSampleTimeUs(5000); // 5ms (ideally 10x faster than shortest RC time constant under test) myQuickPID.AutoTune(inputPin, outputPin, tuningRule, Print, timeout); + myQuickPID.SetTunings(myQuickPID.GetKp(), myQuickPID.GetKi(), myQuickPID.GetKd()); + myQuickPID.SetSampleTimeUs(5000); // recommend 5000µs (5ms) minimum myQuickPID.SetMode(AUTOMATIC); Setpoint = 700; + + if (Print == 1) { + // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png + if (float(myQuickPID.GetTu() / myQuickPID.GetTd() + 0.0001) > 0.75) Serial.println("This process is easy to control."); + else if (float(myQuickPID.GetTu() / myQuickPID.GetTd() + 0.0001) > 0.25) Serial.println("This process has average controllability."); + else Serial.println("This process is difficult to control."); + Serial.print("Tu: "); Serial.print(myQuickPID.GetTu()); // Ultimate Period (sec) + Serial.print(" td: "); Serial.print(myQuickPID.GetTd()); // Dead Time (sec) + Serial.print(" Ku: "); Serial.print(myQuickPID.GetKu()); // Ultimate Gain + Serial.print(" Kp: "); Serial.print(myQuickPID.GetKp()); + Serial.print(" Ki: "); Serial.print(myQuickPID.GetKi()); + Serial.print(" Kd: "); Serial.println(myQuickPID.GetKd()); + delay(6000); + } } void loop() -{ +{ // plotter Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); Serial.print("Input:"); Serial.print(Input); Serial.print(","); Serial.print("Output:"); Serial.print(Output); Serial.print(","); diff --git a/keywords.txt b/keywords.txt index 9cbf41f..d3cc8bd 100644 --- a/keywords.txt +++ b/keywords.txt @@ -24,6 +24,7 @@ GetKi KEYWORD2 GetKd KEYWORD2 GetKu KEYWORD2 GetTu KEYWORD2 +GetTd KEYWORD2 GetMode KEYWORD2 GetDirection KEYWORD2 analogReadFast KEYWORD2 diff --git a/library.json b/library.json index 4a89888..e003537 100644 --- a/library.json +++ b/library.json @@ -1,13 +1,13 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function with 8 tuning rules to choose from. This controller can automatically determine and set parameters (P,I,D). The POn setting controls the mix of Proportional on Error to Proportional on Measurement.", + "description": "A fast fixed/floating point PID controller with AutoTune and 8 tuning rules to choose from. This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement.", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": [ { - "name": "dlloydev" + "name": "David Lloyd" } ], "repository": diff --git a/library.properties b/library.properties index 0131062..870bc40 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=QuickPID -version=2.2.0 -author=dlloydev +version=2.2.1 +author=David Lloyd maintainer=David Lloyd -sentence=QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function with 8 tuning rules to choose from. -paragraph=This controller can automatically determine and set parameters (P,I,D). The POn setting controls the mix of Proportional on Error to Proportional on Measurement. +sentence=A fast fixed/floating point PID controller with AutoTune and 8 tuning rules to choose from. +paragraph=This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID architectures=* From ec2ecdd97ade76cb233290ae6d2c72b18e58e5ea Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 29 Mar 2021 00:04:52 -0400 Subject: [PATCH 025/101] Fix #1 --- QuickPID.cpp | 12 ++++++------ QuickPID.h | 14 +++++++------- library.json | 2 +- library.properties | 2 +- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index afc5680..59b2240 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -19,7 +19,7 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - float Kp, float Ki, float Kd, float POn = 1, bool ControllerDirection = 0) + float Kp, float Ki, float Kd, float POn = 1, uint8_t ControllerDirection = 0) { myOutput = Output; myInput = Input; @@ -41,7 +41,7 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, **********************************************************************************/ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - float Kp, float Ki, float Kd, bool ControllerDirection) + float Kp, float Ki, float Kd, uint8_t ControllerDirection) : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = 0) { @@ -222,7 +222,7 @@ void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) when the transition from manual to auto occurs, the controller is automatically initialized ******************************************************************************/ -void QuickPID::SetMode(bool Mode) +void QuickPID::SetMode(uint8_t Mode) { bool newAuto = (Mode == AUTOMATIC); if (newAuto && !inAuto) @@ -249,7 +249,7 @@ void QuickPID::Initialize() know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. ******************************************************************************/ -void QuickPID::SetControllerDirection(bool Direction) +void QuickPID::SetControllerDirection(uint8_t Direction) { if (inAuto && Direction != controllerDirection) { @@ -283,10 +283,10 @@ float QuickPID::GetTu() { float QuickPID::GetTd() { return dispTd; } -bool QuickPID::GetMode() { +uint8_t QuickPID::GetMode() { return inAuto ? AUTOMATIC : MANUAL; } -bool QuickPID::GetDirection() { +uint8_t QuickPID::GetDirection() { return controllerDirection; } diff --git a/QuickPID.h b/QuickPID.h index 77cd489..959c2b9 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -18,13 +18,13 @@ class QuickPID // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, float, bool); + QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, float, uint8_t); // Overload constructor with proportional mode. Links the PID to Input, Output, Setpoint and Tuning Parameters. - QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, bool); + QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, uint8_t); // Sets PID to either Manual (0) or Auto (non-0). - void SetMode(bool Mode); + void SetMode(uint8_t Mode); // Performs the PID calculation. It should be called every time loop() cycles. ON/OFF and calculation frequency // can be set using SetMode and SetSampleTime respectively. @@ -47,7 +47,7 @@ class QuickPID // Sets the Direction, or "Action" of control. DIRECT means the output will increase when error is positive. // REVERSE means the opposite. It's very unlikely that this will be needed once it is set in the constructor. - void SetControllerDirection(bool); + void SetControllerDirection(uint8_t); // Sets the sample time in milliseconds with which each PID calculation is performed. Default is 100. void SetSampleTimeUs(uint32_t); @@ -59,8 +59,8 @@ class QuickPID float GetKu(); // Ultimate Gain float GetTu(); // Ultimate Period float GetTd(); // Dead Time - bool GetMode(); - bool GetDirection(); + uint8_t GetMode(); + uint8_t GetDirection(); // Utility functions ****************************************************************************************** int analogReadFast(int); @@ -88,7 +88,7 @@ class QuickPID float kpi; // proportional on error amount float kpd; // proportional on measurement amount - bool controllerDirection; + uint8_t controllerDirection; int16_t *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a int16_t *myOutput; // hard link between the variables and the PID, freeing the user from having diff --git a/library.json b/library.json index e003537..d01749f 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "A fast fixed/floating point PID controller with AutoTune and 8 tuning rules to choose from. This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement.", + "description": "A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement.", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 870bc40..6479169 100644 --- a/library.properties +++ b/library.properties @@ -2,7 +2,7 @@ name=QuickPID version=2.2.1 author=David Lloyd maintainer=David Lloyd -sentence=A fast fixed/floating point PID controller with AutoTune and 8 tuning rules to choose from. +sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. paragraph=This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID From 50c2f8b272b8d90f7f1fb302048cbf07c1d69ca7 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 29 Mar 2021 00:04:52 -0400 Subject: [PATCH 026/101] Fix #2 --- QuickPID.cpp | 12 ++++++------ QuickPID.h | 14 +++++++------- library.json | 2 +- library.properties | 2 +- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index afc5680..59b2240 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -19,7 +19,7 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - float Kp, float Ki, float Kd, float POn = 1, bool ControllerDirection = 0) + float Kp, float Ki, float Kd, float POn = 1, uint8_t ControllerDirection = 0) { myOutput = Output; myInput = Input; @@ -41,7 +41,7 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, **********************************************************************************/ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - float Kp, float Ki, float Kd, bool ControllerDirection) + float Kp, float Ki, float Kd, uint8_t ControllerDirection) : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = 0) { @@ -222,7 +222,7 @@ void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) when the transition from manual to auto occurs, the controller is automatically initialized ******************************************************************************/ -void QuickPID::SetMode(bool Mode) +void QuickPID::SetMode(uint8_t Mode) { bool newAuto = (Mode == AUTOMATIC); if (newAuto && !inAuto) @@ -249,7 +249,7 @@ void QuickPID::Initialize() know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. ******************************************************************************/ -void QuickPID::SetControllerDirection(bool Direction) +void QuickPID::SetControllerDirection(uint8_t Direction) { if (inAuto && Direction != controllerDirection) { @@ -283,10 +283,10 @@ float QuickPID::GetTu() { float QuickPID::GetTd() { return dispTd; } -bool QuickPID::GetMode() { +uint8_t QuickPID::GetMode() { return inAuto ? AUTOMATIC : MANUAL; } -bool QuickPID::GetDirection() { +uint8_t QuickPID::GetDirection() { return controllerDirection; } diff --git a/QuickPID.h b/QuickPID.h index 77cd489..959c2b9 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -18,13 +18,13 @@ class QuickPID // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, float, bool); + QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, float, uint8_t); // Overload constructor with proportional mode. Links the PID to Input, Output, Setpoint and Tuning Parameters. - QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, bool); + QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, uint8_t); // Sets PID to either Manual (0) or Auto (non-0). - void SetMode(bool Mode); + void SetMode(uint8_t Mode); // Performs the PID calculation. It should be called every time loop() cycles. ON/OFF and calculation frequency // can be set using SetMode and SetSampleTime respectively. @@ -47,7 +47,7 @@ class QuickPID // Sets the Direction, or "Action" of control. DIRECT means the output will increase when error is positive. // REVERSE means the opposite. It's very unlikely that this will be needed once it is set in the constructor. - void SetControllerDirection(bool); + void SetControllerDirection(uint8_t); // Sets the sample time in milliseconds with which each PID calculation is performed. Default is 100. void SetSampleTimeUs(uint32_t); @@ -59,8 +59,8 @@ class QuickPID float GetKu(); // Ultimate Gain float GetTu(); // Ultimate Period float GetTd(); // Dead Time - bool GetMode(); - bool GetDirection(); + uint8_t GetMode(); + uint8_t GetDirection(); // Utility functions ****************************************************************************************** int analogReadFast(int); @@ -88,7 +88,7 @@ class QuickPID float kpi; // proportional on error amount float kpd; // proportional on measurement amount - bool controllerDirection; + uint8_t controllerDirection; int16_t *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a int16_t *myOutput; // hard link between the variables and the PID, freeing the user from having diff --git a/library.json b/library.json index e003537..d01749f 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "A fast fixed/floating point PID controller with AutoTune and 8 tuning rules to choose from. This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement.", + "description": "A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement.", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 870bc40..6479169 100644 --- a/library.properties +++ b/library.properties @@ -2,7 +2,7 @@ name=QuickPID version=2.2.1 author=David Lloyd maintainer=David Lloyd -sentence=A fast fixed/floating point PID controller with AutoTune and 8 tuning rules to choose from. +sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. paragraph=This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID From e87ddb47a98f7068e78bd105416b8b65402bce0d Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 3 Apr 2021 00:31:27 -0400 Subject: [PATCH 027/101] Version 2.2.2 - Added compatibility with the ESP32 Arduino framework - Added full featured AnalogWrite methods for ESP32 to control up to 9 PWM and 2 DAC signals. --- QuickPID.cpp | 72 ++++++++++++++++++++++++++++++++++++++++++---- QuickPID.h | 49 ++++++++++++++++++++----------- README.md | 72 ++++++++++++++++++++++++++++++++++++++++------ keywords.txt | 4 +++ library.json | 2 +- library.properties | 4 +-- 6 files changed, 169 insertions(+), 34 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 59b2240..84c6cb4 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -18,7 +18,7 @@ The parameters specified here are those for for which we can't set up reliable defaults, so we need to have the user set them. **********************************************************************************/ -QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, +QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, float Kp, float Ki, float Kd, float POn = 1, uint8_t ControllerDirection = 0) { myOutput = Output; @@ -40,7 +40,7 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, to use Proportional on Error without explicitly saying so. **********************************************************************************/ -QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, +QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, float Kp, float Ki, float Kd, uint8_t ControllerDirection) : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = 0) { @@ -60,8 +60,8 @@ bool QuickPID::Compute() uint32_t timeChange = (now - lastTime); if (timeChange >= sampleTimeUs) { // Compute the working error variables - int16_t input = *myInput; - int16_t dInput = input - lastInput; + int input = *myInput; + int dInput = input - lastInput; error = *mySetpoint - input; if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); // fixed-point @@ -204,7 +204,7 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. ******************************************************************************/ -void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) +void QuickPID::SetOutputLimits(int Min, int Max) { if (Min >= Max) return; outMin = Min; @@ -329,7 +329,7 @@ float QuickPID::analogReadAvg(int ADCpin) { return (float)sum / 16.0; } -int16_t QuickPID::Saturate(int16_t Out) { +int QuickPID::Saturate(int Out) { if (Out > outMax) Out = outMax; else if (Out < outMin) Out = outMin; return Out; @@ -360,3 +360,63 @@ void QuickPID::Stabilize(int inputPin, int outputPin, uint32_t timeout) { while ((analogReadAvg(inputPin) < atSetpoint) && (millis() <= timeout)); analogWrite(outputPin, atOutput); } + +#if defined(ESP32) + +// Adds support for analogWrite() for up to 9 PWM pins plus pins DAC1 and DAC2 which are 8-bit true analog outputs. +// Also adds support for changing the PWM frequency (5000 Hz default) and timer resolution (13-bit default). + +analog_write_channel_t _analog_write_channels[9] = { { 2, 5000, 13}, //LED_BUILTIN + { 4, 5000, 13}, { 13, 5000, 13}, { 14, 5000, 13}, { 16, 5000, 13}, { 17, 5000, 13}, { 27, 5000, 13}, { 32, 5000, 13}, { 33, 5000, 13} +}; + +void analogWriteFrequency(float frequency) { + for (uint8_t i = 0; i < 9; i++) { + _analog_write_channels[i].frequency = frequency; + if (frequency < 0.0001) { + ledcDetachPin(_analog_write_channels[i].pin); + pinMode(_analog_write_channels[i].pin, INPUT); + } + } +} + +void analogWriteFrequency(uint8_t pin, float frequency) { + for (uint8_t i = 0; i < 9; i++) { + if (_analog_write_channels[i].pin == pin) { + _analog_write_channels[i].frequency = frequency; + if (frequency < 0.0001) { + ledcDetachPin(_analog_write_channels[i].pin); + pinMode(_analog_write_channels[i].pin, INPUT); + } + } + } +} + +void analogWriteResolution(uint8_t resolution) { + for (uint8_t i = 0; i < 9; i++) { + _analog_write_channels[i].resolution = resolution; + } +} + +void analogWriteResolution(uint8_t pin, uint8_t resolution) { + for (uint8_t i = 0; i < 9; i++) { + if (_analog_write_channels[i].pin == pin) { + _analog_write_channels[i].resolution = resolution; + ledcSetup(i, _analog_write_channels[i].frequency, resolution); + ledcAttachPin(_analog_write_channels[i].pin, i); + } + } +} + +void analogWrite(uint8_t pin, uint32_t value) { + for (uint8_t i = 0; i < 9; i++) { + if (_analog_write_channels[i].pin == pin) { + uint8_t resolution = _analog_write_channels[i].resolution; + uint32_t valueMax = (pow(2, resolution)) - 1; + if (value > valueMax) value = valueMax; + ledcWrite(i, value); + } + if (pin == DAC1 || pin == DAC2) dacWrite(pin, value & 255); + } +} +#endif diff --git a/QuickPID.h b/QuickPID.h index 959c2b9..37591c2 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -18,10 +18,10 @@ class QuickPID // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, float, uint8_t); + QuickPID(int*, int*, int*, float, float, float, float, uint8_t); // Overload constructor with proportional mode. Links the PID to Input, Output, Setpoint and Tuning Parameters. - QuickPID(int16_t*, int16_t*, int16_t*, float, float, float, uint8_t); + QuickPID(int*, int*, int*, float, float, float, uint8_t); // Sets PID to either Manual (0) or Auto (non-0). void SetMode(uint8_t Mode); @@ -34,7 +34,7 @@ class QuickPID void AutoTune(int, int, int, int, uint32_t); // Sets and clamps the output to a specific range (0-255 by default). - void SetOutputLimits(int16_t, int16_t); + void SetOutputLimits(int, int); // available but not commonly used functions ****************************************************************** @@ -68,35 +68,35 @@ class QuickPID private: void Initialize(); - int16_t Saturate(int16_t); + int Saturate(int); void CheckPeak(int); void StepUp(int, int, uint32_t); void StepDown(int, int, uint32_t); void Stabilize(int, int, uint32_t); - float dispKp; // We'll hold on to the tuning parameters for display purposes. + float dispKp; // We'll hold on to the tuning parameters for display purposes. float dispKi; float dispKd; float dispKu; float dispTu; float dispTd; - float pOn; // proportional mode (0-1) default = 1, 100% Proportional on Error - float kp; // (P)roportional Tuning Parameter - float ki; // (I)ntegral Tuning Parameter - float kd; // (D)erivative Tuning Parameter - float kpi; // proportional on error amount - float kpd; // proportional on measurement amount + float pOn; // proportional mode (0-1) default = 1, 100% Proportional on Error + float kp; // (P)roportional Tuning Parameter + float ki; // (I)ntegral Tuning Parameter + float kd; // (D)erivative Tuning Parameter + float kpi; // proportional on error amount + float kpd; // proportional on measurement amount uint8_t controllerDirection; - int16_t *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a - int16_t *myOutput; // hard link between the variables and the PID, freeing the user from having - int16_t *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. + int *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a + int *myOutput; // hard link between the variables and the PID, freeing the user from having + int *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. uint32_t sampleTimeUs, lastTime; - int16_t outMin, outMax, error; - int16_t lastInput, outputSum; + int outMin, outMax, error; + int lastInput, outputSum; bool inAuto; // AutoTune @@ -109,4 +109,21 @@ class QuickPID }; +#if defined(ESP32) +// Adds support for analogWrite() for up to 16 PWM pins plus pins DAC1 and DAC2 which are 8-bit true analog outputs. +// Also adds support for changing the PWM frequency (5000 Hz default) and timer resolution (13-bit default). + +typedef struct analog_write_channel { + int8_t pin; + double frequency; + uint8_t resolution; +} analog_write_channel_t; + +void analogWriteFrequency(float frequency = 5000); +void analogWriteFrequency(uint8_t pin, float frequency = 5000); +void analogWriteResolution(uint8_t resolution = 13); +void analogWriteResolution(uint8_t pin, uint8_t resolution = 13); +void analogWrite(uint8_t pin, uint32_t value = 0); +#endif + #endif diff --git a/README.md b/README.md index aa78728..3aac9ba 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,7 @@ Development began with a fork of the Arduino PID Library. Modifications and new - `POn` parameter controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement - Reorganized and more efficient PID algorithm, faster analog read function, micros() timing resolution - Runs a complete PID cycle (*read-compute-write*) faster than just an `analogRead()` command in Arduino +- Includes a complete`analogWrite()`function for ESP32 boards. This controls up to 9 independent PWM pins and 2 DAC pins. ### Performance @@ -51,8 +52,8 @@ The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function #### QuickPID_Constructor ```c++ -QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - float Kp, float Ki, float Kd, float POn, bool ControllerDirection) +QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, + float Kp, float Ki, float Kd, float POn, uint8_t ControllerDirection) ``` - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. @@ -64,8 +65,8 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. DIRECT is most common. ```c++ -QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint, - float Kp, float Ki, float Kd, bool ControllerDirection) +QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, + float Kp, float Ki, float Kd, uint8_t ControllerDirection) ``` This allows you to use Proportional on Error without explicitly saying so. @@ -119,7 +120,7 @@ Sets the period, in microseconds, at which the calculation is performed. The def #### SetOutputLimits ```c++ -void QuickPID::SetOutputLimits(int16_t Min, int16_t Max) +void QuickPID::SetOutputLimits(int Min, int Max) ``` The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. @@ -127,7 +128,7 @@ The PID controller is designed to vary its output within a given range. By defa #### SetMode ```c++ -void QuickPID::SetMode(bool Mode) +void QuickPID::SetMode(uint8_t Mode) ``` Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (non-zero). when the transition from manual to automatic occurs, the controller is automatically initialized. @@ -143,7 +144,7 @@ Does all the things that need to happen to ensure a bump-less transfer from manu #### SetControllerDirection ```c++ -void QuickPID::SetControllerDirection(bool Direction) +void QuickPID::SetControllerDirection(uint8_t Direction) ``` The PID will either be connected to a DIRECT acting process (+Output leads to +Input) or a REVERSE acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. @@ -157,8 +158,8 @@ float QuickPID::GetKd() float QuickPID::GetKu() float QuickPID::GetTu() float QuickPID::GetTd() -bool QuickPID::GetMode() -bool QuickPID::GetDirection() +uint8_t QuickPID::GetMode() +uint8_t QuickPID::GetDirection() ``` These functions query the internal state of the PID. They're here for display purposes. @@ -171,10 +172,63 @@ int QuickPID::analogReadFast(int ADCpin) A faster configuration of `analogRead()`where a preset of 32 is used. If the architecture definition isn't found, normal `analogRead()`is used to return a value. +#### AnalogWrite (PWM and DAC) for ESP32 + +```c++ +void analogWrite(uint8_t pin, uint32_t value) +``` + +Call this function just like in the standard Arduino framework. It controls up to 9 independent PWM outputs and 2 DAC outputs. The controllable GPIO pins are 2, 4, 13, 14, 16, 17, 27, 32 and 33 for PWM and DAC0 (GPIO25) and DAC1 (GPIO26) for true analog outputs. The default PWM frequency is 5000 Hz and the default resolution is 13-bit (0-8191). + +#### AnalogWrite Configuration Functions for ESP32 + +```c++ +void analogWriteFrequency(float frequency = 5000); +void analogWriteFrequency(uint8_t pin, float frequency = 5000); +void analogWriteResolution(uint8_t resolution = 13); +void analogWriteResolution(uint8_t pin, uint8_t resolution = 13); +``` + +Calling `analogWriteFrequency(frequency)`will set the PWM frequency for all 8 assigned pins. Using `analogWriteFrequency(0)`will detach the 9 assigned pins and set them as input. + +To independently assign a unique frequency to each PWM pin, use the `analogWriteFrequency(pin, frequency)` function. If the frequency is set to 0, this function will detach the referenced pin and configure it as an input. + +Calling `analogWriteResolution(resolution)` will set the resolution for all 9 assigned pins. Read more about the [Supported Range of Frequency and Duty Resolution](https://docs.espressif.com/projects/esp-idf/en/latest/api-reference/peripherals/ledc.html#ledc-api-supported-range-frequency-duty-resolution) here. + +To independently assign a unique frequency to each PWM pin, use the `analogWriteResolution(pin, resolution)` function. Note that it is required to call this function once prior to using AnalogWrite as this will automatically setup and attach (initialize) the pin. + +#### Fade Example for ESP32 + +```c++ +#include "QuickPID.h" + +#define LED_BUILTIN 2 +int brightness = 0; +int step = 1; + +void setup() { + analogWriteResolution(LED_BUILTIN, 10); +} + +void loop() { + analogWrite(LED_BUILTIN, brightness); + + brightness += step; + if ( brightness >= 1023) step = -1; + if ( brightness <= 0) step = 1; + delay(1); +} +``` + ### Change Log #### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) +- Added compatibility with the ESP32 Arduino framework +- Added full featured AnalogWrite methods for ESP32 to control up to 9 PWM and 2 DAC signals + +#### Version 2.2.1 + - Even faster AutoTune function - AutoTune now determines the controllability of the process - Added AMIGO_PID tuning rule diff --git a/keywords.txt b/keywords.txt index d3cc8bd..eed0dbb 100644 --- a/keywords.txt +++ b/keywords.txt @@ -7,6 +7,7 @@ ########################################## QuickPID KEYWORD1 +analog_write_channel_t KEYWORD1 ########################################## # Methods and Functions (KEYWORD2) @@ -29,6 +30,9 @@ GetMode KEYWORD2 GetDirection KEYWORD2 analogReadFast KEYWORD2 analogReadAvg KEYWORD2 +analogWrite KEYWORD2 +analogWriteFrequency KEYWORD2 +analogWriteResolution KEYWORD2 ########################################## # Constants (LITERAL1) diff --git a/library.json b/library.json index d01749f..2f411f5 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement.", + "description": "A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards.", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 6479169..281efd1 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=QuickPID -version=2.2.1 +version=2.2.2 author=David Lloyd maintainer=David Lloyd sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. -paragraph=This controller can automatically determine and set tuning parameters. An added feature is contolling the mix of Proportional on Error to Proportional on Measurement. +paragraph=This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID architectures=* From b6c2873f5198ac9deee8e85f0e7776db0f9c0a00 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 3 Apr 2021 10:18:39 -0400 Subject: [PATCH 028/101] Minor cleanup --- QuickPID.cpp | 9 +++++---- QuickPID.h | 5 ++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 84c6cb4..58553df 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.2.1 + QuickPID Library for Arduino - Version 2.2.2 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard @@ -147,7 +147,6 @@ void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = ki = Rule[tuningRule][cki] / 1000.0 * Ku / Tu; kd = Rule[tuningRule][ckd] / 1000.0 * Ku * Tu; } - dispKp = kp; dispKi = ki; dispKd = kd; @@ -362,7 +361,6 @@ void QuickPID::Stabilize(int inputPin, int outputPin, uint32_t timeout) { } #if defined(ESP32) - // Adds support for analogWrite() for up to 9 PWM pins plus pins DAC1 and DAC2 which are 8-bit true analog outputs. // Also adds support for changing the PWM frequency (5000 Hz default) and timer resolution (13-bit default). @@ -416,7 +414,10 @@ void analogWrite(uint8_t pin, uint32_t value) { if (value > valueMax) value = valueMax; ledcWrite(i, value); } - if (pin == DAC1 || pin == DAC2) dacWrite(pin, value & 255); + if (pin == DAC1 || pin == DAC2) { + if (value > 255) value = 255; + dacWrite(pin, value); + } } } #endif diff --git a/QuickPID.h b/QuickPID.h index 37591c2..7ce02a0 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -110,7 +110,7 @@ class QuickPID }; #if defined(ESP32) -// Adds support for analogWrite() for up to 16 PWM pins plus pins DAC1 and DAC2 which are 8-bit true analog outputs. +// Adds support for analogWrite() for up to 9 PWM pins plus pins DAC1 and DAC2 which are 8-bit true analog outputs. // Also adds support for changing the PWM frequency (5000 Hz default) and timer resolution (13-bit default). typedef struct analog_write_channel { @@ -125,5 +125,4 @@ void analogWriteResolution(uint8_t resolution = 13); void analogWriteResolution(uint8_t pin, uint8_t resolution = 13); void analogWrite(uint8_t pin, uint32_t value = 0); #endif - -#endif +#endif //QuickPID.h From 5aa4c33e2834ba77a424d44e0be605bb3230a00c Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 14 Apr 2021 15:05:01 -0400 Subject: [PATCH 029/101] Update compatibility with ESP32 --- QuickPID.cpp | 112 +++++------------------------------ QuickPID.h | 20 ++----- README.md | 56 +++--------------- library.properties | 2 +- utility/analogWrite.cpp | 128 ++++++++++++++++++++++++++++++++++++++++ utility/analogWrite.h | 33 +++++++++++ 6 files changed, 189 insertions(+), 162 deletions(-) create mode 100644 utility/analogWrite.cpp create mode 100644 utility/analogWrite.h diff --git a/QuickPID.cpp b/QuickPID.cpp index 58553df..926c53f 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.2.2 + QuickPID Library for Arduino - Version 2.2.3 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library by Brett Beauregard @@ -53,8 +53,7 @@ QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, PID Output needs to be computed. Returns true when the output is computed, false when nothing has been done. **********************************************************************************/ -bool QuickPID::Compute() -{ +bool QuickPID::Compute() { if (!inAuto) return false; uint32_t now = micros(); uint32_t timeChange = (now - lastTime); @@ -67,7 +66,7 @@ bool QuickPID::Compute() if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); // fixed-point else outputSum += (kpi * error) - (kpd * dInput); // floating-point - outputSum = QuickPID::Saturate(outputSum); + outputSum = CONSTRAIN(outputSum, outMin, outMax); *myOutput = outputSum; lastInput = input; @@ -157,10 +156,8 @@ void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = it's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation ******************************************************************************/ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) -{ +void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) { if (Kp < 0 || Ki < 0 || Kd < 0) return; - pOn = POn; dispKp = Kp; dispKi = Ki; dispKd = Kd; @@ -171,8 +168,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) kpi = (kp * pOn) + ki; kpd = (kp * (1 - pOn)) + kd; - if (controllerDirection == REVERSE) - { + if (controllerDirection == REVERSE) { kp = (0 - kp); ki = (0 - ki); kd = (0 - kd); @@ -189,8 +185,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { /* SetSampleTime(...) ********************************************************* Sets the period, in microseconds, at which the calculation is performed ******************************************************************************/ -void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) -{ +void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) { if (NewSampleTimeUs > 0) { float ratio = (float)NewSampleTimeUs / (float)sampleTimeUs; ki *= ratio; @@ -203,16 +198,14 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. ******************************************************************************/ -void QuickPID::SetOutputLimits(int Min, int Max) -{ +void QuickPID::SetOutputLimits(int Min, int Max) { if (Min >= Max) return; outMin = Min; outMax = Max; - if (inAuto) - { - *myOutput = QuickPID::Saturate(*myOutput); - outputSum = QuickPID::Saturate(outputSum); + if (inAuto) { + *myOutput = CONSTRAIN(*myOutput, outMin, outMax); + outputSum = CONSTRAIN(outputSum, outMin, outMax); } } @@ -221,11 +214,9 @@ void QuickPID::SetOutputLimits(int Min, int Max) when the transition from manual to auto occurs, the controller is automatically initialized ******************************************************************************/ -void QuickPID::SetMode(uint8_t Mode) -{ +void QuickPID::SetMode(uint8_t Mode) { bool newAuto = (Mode == AUTOMATIC); - if (newAuto && !inAuto) - { /*we just went from manual to auto*/ + if (newAuto && !inAuto) { //we just went from manual to auto QuickPID::Initialize(); } inAuto = newAuto; @@ -235,11 +226,10 @@ void QuickPID::SetMode(uint8_t Mode) Does all the things that need to happen to ensure a bumpless transfer from manual to automatic mode. ******************************************************************************/ -void QuickPID::Initialize() -{ +void QuickPID::Initialize() { outputSum = *myOutput; lastInput = *myInput; - outputSum = QuickPID::Saturate(outputSum); + outputSum = CONSTRAIN(outputSum, outMin, outMax); } /* SetControllerDirection(...)************************************************* @@ -248,10 +238,8 @@ void QuickPID::Initialize() know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. ******************************************************************************/ -void QuickPID::SetControllerDirection(uint8_t Direction) -{ - if (inAuto && Direction != controllerDirection) - { +void QuickPID::SetControllerDirection(uint8_t Direction) { + if (inAuto && Direction != controllerDirection) { kp = (0 - kp); ki = (0 - ki); kd = (0 - kd); @@ -328,12 +316,6 @@ float QuickPID::analogReadAvg(int ADCpin) { return (float)sum / 16.0; } -int QuickPID::Saturate(int Out) { - if (Out > outMax) Out = outMax; - else if (Out < outMin) Out = outMin; - return Out; -} - void QuickPID::CheckPeak(int inputPin) { float rdAvg = analogReadAvg(inputPin); if (rdAvg > peakHigh) peakHigh = rdAvg; @@ -359,65 +341,3 @@ void QuickPID::Stabilize(int inputPin, int outputPin, uint32_t timeout) { while ((analogReadAvg(inputPin) < atSetpoint) && (millis() <= timeout)); analogWrite(outputPin, atOutput); } - -#if defined(ESP32) -// Adds support for analogWrite() for up to 9 PWM pins plus pins DAC1 and DAC2 which are 8-bit true analog outputs. -// Also adds support for changing the PWM frequency (5000 Hz default) and timer resolution (13-bit default). - -analog_write_channel_t _analog_write_channels[9] = { { 2, 5000, 13}, //LED_BUILTIN - { 4, 5000, 13}, { 13, 5000, 13}, { 14, 5000, 13}, { 16, 5000, 13}, { 17, 5000, 13}, { 27, 5000, 13}, { 32, 5000, 13}, { 33, 5000, 13} -}; - -void analogWriteFrequency(float frequency) { - for (uint8_t i = 0; i < 9; i++) { - _analog_write_channels[i].frequency = frequency; - if (frequency < 0.0001) { - ledcDetachPin(_analog_write_channels[i].pin); - pinMode(_analog_write_channels[i].pin, INPUT); - } - } -} - -void analogWriteFrequency(uint8_t pin, float frequency) { - for (uint8_t i = 0; i < 9; i++) { - if (_analog_write_channels[i].pin == pin) { - _analog_write_channels[i].frequency = frequency; - if (frequency < 0.0001) { - ledcDetachPin(_analog_write_channels[i].pin); - pinMode(_analog_write_channels[i].pin, INPUT); - } - } - } -} - -void analogWriteResolution(uint8_t resolution) { - for (uint8_t i = 0; i < 9; i++) { - _analog_write_channels[i].resolution = resolution; - } -} - -void analogWriteResolution(uint8_t pin, uint8_t resolution) { - for (uint8_t i = 0; i < 9; i++) { - if (_analog_write_channels[i].pin == pin) { - _analog_write_channels[i].resolution = resolution; - ledcSetup(i, _analog_write_channels[i].frequency, resolution); - ledcAttachPin(_analog_write_channels[i].pin, i); - } - } -} - -void analogWrite(uint8_t pin, uint32_t value) { - for (uint8_t i = 0; i < 9; i++) { - if (_analog_write_channels[i].pin == pin) { - uint8_t resolution = _analog_write_channels[i].resolution; - uint32_t valueMax = (pow(2, resolution)) - 1; - if (value > valueMax) value = valueMax; - ledcWrite(i, value); - } - if (pin == DAC1 || pin == DAC2) { - if (value > 255) value = 255; - dacWrite(pin, value); - } - } -} -#endif diff --git a/QuickPID.h b/QuickPID.h index 7ce02a0..c0a05c9 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -6,7 +6,7 @@ class QuickPID public: - //Constants used in some of the functions below + //Constants and macros #define AUTOMATIC 1 #define MANUAL 0 #define DIRECT 0 @@ -14,6 +14,7 @@ class QuickPID #define FL_FX(a) (int32_t)(a*256.0) // float to fixed point #define FX_MUL(a,b) ((a*b)>>8) // fixed point multiply +#define CONSTRAIN(x,lower,upper) ((x)<(lower)?(lower):((x)>(upper)?(upper):(x))) // commonly used functions ************************************************************************************ @@ -68,7 +69,6 @@ class QuickPID private: void Initialize(); - int Saturate(int); void CheckPeak(int); void StepUp(int, int, uint32_t); void StepDown(int, int, uint32_t); @@ -110,19 +110,7 @@ class QuickPID }; #if defined(ESP32) -// Adds support for analogWrite() for up to 9 PWM pins plus pins DAC1 and DAC2 which are 8-bit true analog outputs. -// Also adds support for changing the PWM frequency (5000 Hz default) and timer resolution (13-bit default). - -typedef struct analog_write_channel { - int8_t pin; - double frequency; - uint8_t resolution; -} analog_write_channel_t; - -void analogWriteFrequency(float frequency = 5000); -void analogWriteFrequency(uint8_t pin, float frequency = 5000); -void analogWriteResolution(uint8_t resolution = 13); -void analogWriteResolution(uint8_t pin, uint8_t resolution = 13); -void analogWrite(uint8_t pin, uint32_t value = 0); +#include "utility/analogWrite.h" #endif + #endif //QuickPID.h diff --git a/README.md b/README.md index 3aac9ba..1717e54 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ Development began with a fork of the Arduino PID Library. Modifications and new - `POn` parameter controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement - Reorganized and more efficient PID algorithm, faster analog read function, micros() timing resolution - Runs a complete PID cycle (*read-compute-write*) faster than just an `analogRead()` command in Arduino -- Includes a complete`analogWrite()`function for ESP32 boards. This controls up to 9 independent PWM pins and 2 DAC pins. +- Includes a complete [analogWrite function for ESP32](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) boards. This controls up to 8 independent PWM pins and 2 DAC pins. ### Performance @@ -172,60 +172,18 @@ int QuickPID::analogReadFast(int ADCpin) A faster configuration of `analogRead()`where a preset of 32 is used. If the architecture definition isn't found, normal `analogRead()`is used to return a value. -#### AnalogWrite (PWM and DAC) for ESP32 +#### [AnalogWrite (PWM and DAC) for ESP32](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) -```c++ -void analogWrite(uint8_t pin, uint32_t value) -``` - -Call this function just like in the standard Arduino framework. It controls up to 9 independent PWM outputs and 2 DAC outputs. The controllable GPIO pins are 2, 4, 13, 14, 16, 17, 27, 32 and 33 for PWM and DAC0 (GPIO25) and DAC1 (GPIO26) for true analog outputs. The default PWM frequency is 5000 Hz and the default resolution is 13-bit (0-8191). - -#### AnalogWrite Configuration Functions for ESP32 - -```c++ -void analogWriteFrequency(float frequency = 5000); -void analogWriteFrequency(uint8_t pin, float frequency = 5000); -void analogWriteResolution(uint8_t resolution = 13); -void analogWriteResolution(uint8_t pin, uint8_t resolution = 13); -``` - -Calling `analogWriteFrequency(frequency)`will set the PWM frequency for all 8 assigned pins. Using `analogWriteFrequency(0)`will detach the 9 assigned pins and set them as input. - -To independently assign a unique frequency to each PWM pin, use the `analogWriteFrequency(pin, frequency)` function. If the frequency is set to 0, this function will detach the referenced pin and configure it as an input. - -Calling `analogWriteResolution(resolution)` will set the resolution for all 9 assigned pins. Read more about the [Supported Range of Frequency and Duty Resolution](https://docs.espressif.com/projects/esp-idf/en/latest/api-reference/peripherals/ledc.html#ledc-api-supported-range-frequency-duty-resolution) here. +#### Change Log -To independently assign a unique frequency to each PWM pin, use the `analogWriteResolution(pin, resolution)` function. Note that it is required to call this function once prior to using AnalogWrite as this will automatically setup and attach (initialize) the pin. - -#### Fade Example for ESP32 - -```c++ -#include "QuickPID.h" - -#define LED_BUILTIN 2 -int brightness = 0; -int step = 1; - -void setup() { - analogWriteResolution(LED_BUILTIN, 10); -} - -void loop() { - analogWrite(LED_BUILTIN, brightness); - - brightness += step; - if ( brightness >= 1023) step = -1; - if ( brightness <= 0) step = 1; - delay(1); -} -``` +#### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -### Change Log +- Updated compatibility with the ESP32 Arduino framework -#### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) +#### Version 2.2.2 - Added compatibility with the ESP32 Arduino framework -- Added full featured AnalogWrite methods for ESP32 to control up to 9 PWM and 2 DAC signals +- Added full featured AnalogWrite methods for ESP32 and ESP32S2 #### Version 2.2.1 diff --git a/library.properties b/library.properties index 281efd1..eaa2cac 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.2.2 +version=2.2.3 author=David Lloyd maintainer=David Lloyd sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. diff --git a/utility/analogWrite.cpp b/utility/analogWrite.cpp new file mode 100644 index 0000000..b70b70b --- /dev/null +++ b/utility/analogWrite.cpp @@ -0,0 +1,128 @@ +/********************************************************************************** + AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 1.0.0 + by dlloydev https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite + This Library is licensed under the MIT License + **********************************************************************************/ + +#include +#include "analogWrite.h" + +pinStatus_t pinsStatus[16] = { + { 1, -1, 5000, 13, 0 }, { 3, -1, 5000, 13, 0 }, + { 5, -1, 5000, 13, 0 }, { 7, -1, 5000, 13, 0 }, + { 9, -1, 5000, 13, 0 }, {11, -1, 5000, 13, 0 }, + {13, -1, 5000, 13, 0 }, {15, -1, 5000, 13, 0 } +}; + +void analogWrite(int8_t pin, int32_t value) { + if (pin == DAC1 || pin == DAC2) { //dac + if (value > 255) value = 255; + dacWrite(pin, value); + } else { //pwm + int8_t ch = getChannel(pin); + if (ch >= 0) { + if (value == -1) { //detach pin + pinsStatus[ch / 2].pin = -1; + pinsStatus[ch / 2].frequency = 5000; + pinsStatus[ch / 2].resolution = 13; + ledcDetachPin(pinsStatus[ch / 2].pin); + REG_SET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL, GPIO_MODE_DEF_DISABLE); + } else { + int32_t valueMax = (pow(2, pinsStatus[ch / 2].resolution)) - 1; + if (value > valueMax) { + value = valueMax + 1; + ledcDetachPin(pin); + pinMode(pin, OUTPUT); + digitalWrite(pin, HIGH); + } else { + ledcSetup(ch, pinsStatus[ch / 2].frequency, pinsStatus[ch / 2].resolution); + ledcWrite(ch, value); + } + pinsStatus[ch / 2].value = value; + } + } + } +} + +float analogWriteFrequency(int8_t pin, float frequency) { + int8_t ch = getChannel(pin); + if (ch >= 0) { + pinsStatus[ch / 2].frequency = frequency; + pinsStatus[ch / 2].pin = pin; + ledcSetup(ch, frequency, pinsStatus[ch / 2].resolution); + ledcWrite(ch, pinsStatus[ch / 2].value); + } + return ledcReadFreq(ch); +} + +int32_t analogWriteResolution(int8_t pin, uint8_t resolution) { + int8_t ch = getChannel(pin); + if (ch >= 0) { + pinsStatus[ch / 2].resolution = resolution; + pinsStatus[ch / 2].pin = pin; + ledcSetup(ch, pinsStatus[ch].frequency, resolution); + ledcWrite(ch, pinsStatus[ch].value); + } + return pow(2, resolution); +} + +int8_t getChannel(int8_t pin) { + if ((pinMask >> pin) & 1) { //valid pin number? + if (REG_GET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL)) { //gpio pin function? + for (int8_t i = 0; i < 8; i++) { //search channels for the pin + if (pinsStatus[i].pin == pin) { //pin found + return pinsStatus[i].channel; + break; + } + } + return -99; //pin is being used externally + } else { //pin is not used + for (int8_t i = 0; i < 8; i++) { //search for free channel + if (pinsStatus[i].pin == -1) { //channel is free + pinsStatus[i].pin = pin; + ledcAttachPin(pin, pinsStatus[i].channel); + return pinsStatus[i].channel; + break; + } + } + } + } + return -88; //no available resources +} + +void printPinsStatus() { + Serial.print("PWM pins: "); + for (int i = 0; i < muxSize; i++) { + if ((pinMask >> i) & 1) { + Serial.print(i); Serial.print(", "); + if (i == 18) Serial.println(); + if (i == 18) Serial.print(" "); + } + } + Serial.println(); + Serial.println(); + for (int i = 0; i < 8; i++) { + Serial.print("ch"); Serial.print(pinsStatus[i].channel); Serial.print(" "); + if (pinsStatus[i].channel < 10) Serial.print(" "); + if (pinsStatus[i].pin == -1) { + Serial.print(pinsStatus[i].pin); Serial.print(" "); + } else { + Serial.print("pin"); Serial.print(pinsStatus[i].pin); Serial.print(" "); + } + if (pinsStatus[i].pin < 10) Serial.print(" "); + if (pinsStatus[i].frequency < 10000) Serial.print(" "); + if (pinsStatus[i].frequency < 1000) Serial.print(" "); + if (pinsStatus[i].frequency < 100) Serial.print(" "); + if (pinsStatus[i].frequency < 10) Serial.print(" "); + Serial.print(pinsStatus[i].frequency); Serial.print(" Hz "); + if (pinsStatus[i].resolution < 10) Serial.print(" "); + Serial.print(pinsStatus[i].resolution); Serial.print("-bit "); + Serial.print("val "); + if (pinsStatus[i].value < 10000) Serial.print(" "); + if (pinsStatus[i].value < 1000) Serial.print(" "); + if (pinsStatus[i].value < 100) Serial.print(" "); + if (pinsStatus[i].value < 10) Serial.print(" "); + Serial.print(pinsStatus[i].value); + Serial.println(); + } +} diff --git a/utility/analogWrite.h b/utility/analogWrite.h new file mode 100644 index 0000000..36b7782 --- /dev/null +++ b/utility/analogWrite.h @@ -0,0 +1,33 @@ +#ifndef _ESP32_ESP32S2_ANALOG_WRITE_ +#define _ESP32_ESP32S2_ANALOG_WRITE_ +#include + +#if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) +#define NUM_OUTPUT_PINS 45 +#define DAC1 17 +#define DAC2 18 +const uint8_t muxSize = 48; +const uint64_t pinMask = 0x7FE00207FFE; //PWM +#else +#define NUM_OUTPUT_PINS 34 +#define DAC1 25 +#define DAC2 26 +const uint8_t muxSize = 40; +const uint64_t pinMask = 0x308EFF034; //PWM +#endif + +typedef struct pinStatus { + int8_t channel; + int8_t pin; + float frequency; + uint8_t resolution; + uint32_t value; +} pinStatus_t; + +float analogWriteFrequency(int8_t pin, float frequency = 5000); +int32_t analogWriteResolution(int8_t pin, uint8_t resolution = 13); +void analogWrite(int8_t pin, int32_t value = 0); +int8_t getChannel(int8_t pin); +void printPinsStatus(void); + +#endif From c05efc23d5ff404c32f3b07d0138f669fdbbef77 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Thu, 15 Apr 2021 11:40:20 -0400 Subject: [PATCH 030/101] Update Readme --- QuickPID.cpp | 42 ++++++++----------- QuickPID.h | 10 +++-- README.md | 40 +++++++++--------- .../AutoTune_RC_Filter/AutoTune_RC_Filter.ino | 4 +- 4 files changed, 46 insertions(+), 50 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 926c53f..486caaf 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,9 +1,7 @@ /********************************************************************************** QuickPID Library for Arduino - Version 2.2.3 by dlloydev https://github.com/Dlloydev/QuickPID - Based on the Arduino PID Library by Brett Beauregard - - This Library is licensed under the MIT License + Based on the Arduino PID Library, licensed under the MIT License **********************************************************************************/ #if ARDUINO >= 100 @@ -11,7 +9,6 @@ #else #include "WProgram.h" #endif - #include "QuickPID.h" /* Constructor ******************************************************************** @@ -19,16 +16,15 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, - float Kp, float Ki, float Kd, float POn = 1, uint8_t ControllerDirection = 0) -{ + float Kp, float Ki, float Kd, float POn = 1, uint8_t ControllerDirection = 0) { + myOutput = Output; myInput = Input; mySetpoint = Setpoint; inAuto = false; - QuickPID::SetOutputLimits(0, 255); // default is same as the arduino PWM limit - sampleTimeUs = 100000; // default is 0.1 seconds - + QuickPID::SetOutputLimits(0, 255); // same default as Arduino PWM limit + sampleTimeUs = 100000; // 0.1 sec default QuickPID::SetControllerDirection(ControllerDirection); QuickPID::SetTunings(Kp, Ki, Kd, POn); @@ -39,19 +35,16 @@ QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, To allow backwards compatability for v1.1, or for people that just want to use Proportional on Error without explicitly saying so. **********************************************************************************/ - QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, float Kp, float Ki, float Kd, uint8_t ControllerDirection) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = 0) -{ + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = 0) { } /* Compute() ********************************************************************** - This, as they say, is where the magic happens. This function should be called - every time "void loop()" executes. The function will decide whether a new - PID Output needs to be computed. Returns true when the output is computed, - false when nothing has been done. + This function should be called every time "void loop()" executes. The function + will decide whether a new PID Output needs to be computed. Returns true + when the output is computed, false when nothing has been done. **********************************************************************************/ bool QuickPID::Compute() { if (!inAuto) return false; @@ -63,7 +56,7 @@ bool QuickPID::Compute() { int dInput = input - lastInput; error = *mySetpoint - input; - if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); // fixed-point + if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); // fixed point else outputSum += (kpi * error) - (kpd * dInput); // floating-point outputSum = CONSTRAIN(outputSum, outMin, outMax); @@ -99,10 +92,11 @@ void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = { 333, 667, 111 }, // SOME_OVERSHOOT_PID { 200, 400, 67 }, // NO_OVERSHOOT_PID }; - const byte ckp = 0, cki = 1, ckd = 2; // c = column + const byte ckp = 0, cki = 1, ckd = 2; //c = column peakHigh = atSetpoint; peakLow = atSetpoint; timeout *= 1000; + if (Print == 1) Serial.print("Stabilizing (33%) →"); QuickPID::Stabilize(inputPin, outputPin, timeout); if (Print == 1) Serial.print(" Running AutoTune ↑"); @@ -234,7 +228,7 @@ void QuickPID::Initialize() { /* SetControllerDirection(...)************************************************* The PID will either be connected to a DIRECT acting process (+Output leads - to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to + to +Input) or a REVERSE acting process(+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. ******************************************************************************/ @@ -282,19 +276,19 @@ uint8_t QuickPID::GetDirection() { int QuickPID::analogReadFast(int ADCpin) { #if defined(__AVR_ATmega328P__) byte ADCregOriginal = ADCSRA; - ADCSRA = (ADCSRA & B11111000) | 5; //32 prescaler + ADCSRA = (ADCSRA & B11111000) | 5; // 32 prescaler int adc = analogRead(ADCpin); ADCSRA = ADCregOriginal; return adc; #elif defined(__AVR_ATtiny_Zero_One__) || defined(__AVR_ATmega_Zero__) byte ADCregOriginal = ADC0_CTRLC; - ADC0_CTRLC = 0x54; //reduced cap, Vdd ref, 32 prescaler + ADC0_CTRLC = 0x54; // reduced cap, Vdd ref, 32 prescaler int adc = analogRead(ADCpin); ADC0_CTRLC = ADCregOriginal; return adc; #elif defined(__AVR_DA__) byte ADCregOriginal = ADC0.CTRLC; - ADC0.CTRLC = ADC_PRESC_DIV32_gc; //32 prescaler + ADC0.CTRLC = ADC_PRESC_DIV32_gc; // 32 prescaler int adc = analogRead(ADCpin); ADC0.CTRLC = ADCregOriginal; return adc; @@ -325,9 +319,7 @@ void QuickPID::CheckPeak(int inputPin) { void QuickPID::Stabilize(int inputPin, int outputPin, uint32_t timeout) { // initial reading - for (int i = 0; i <= 16; i++) { - analogReadAvg(inputPin); - } + for (int i = 0; i <= 16; i++) analogReadAvg(inputPin); // coarse adjust analogWrite (outputPin, 0); while ((analogReadAvg(inputPin) > (atSetpoint - hysteresis)) && (millis() <= timeout)); diff --git a/QuickPID.h b/QuickPID.h index c0a05c9..e5e4673 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -1,8 +1,7 @@ #ifndef QuickPID_h #define QuickPID_h -class QuickPID -{ +class QuickPID { public: @@ -12,6 +11,9 @@ class QuickPID #define DIRECT 0 #define REVERSE 1 +static const byte TRY_DIRECT = 0; +static const byte TRY_AUTOMATIC = 1; + #define FL_FX(a) (int32_t)(a*256.0) // float to fixed point #define FX_MUL(a,b) ((a*b)>>8) // fixed point multiply #define CONSTRAIN(x,lower,upper) ((x)<(lower)?(lower):((x)>(upper)?(upper):(x))) @@ -74,7 +76,7 @@ class QuickPID void StepDown(int, int, uint32_t); void Stabilize(int, int, uint32_t); - float dispKp; // We'll hold on to the tuning parameters for display purposes. + float dispKp; // tuning parameters for display purposes. float dispKi; float dispKd; float dispKu; @@ -113,4 +115,4 @@ class QuickPID #include "utility/analogWrite.h" #endif -#endif //QuickPID.h +#endif // QuickPID.h diff --git a/README.md b/README.md index 1717e54..9b33a3b 100644 --- a/README.md +++ b/README.md @@ -53,20 +53,20 @@ The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function ```c++ QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, - float Kp, float Ki, float Kd, float POn, uint8_t ControllerDirection) + float Kp, float Ki, float Kd, float POn, uint8_t ControllerDirection); ``` - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. - `POn` is the Proportional on Error weighting value (0.0-1.0). This controls the mix of Proportional on Error (PonE) and Proportional on Measurement (PonM) that's used in the compute algorithm. Note that POn controls the PonE amount, where the remainder (1-PonE) is the PonM amount. Also, the default POn is 1 -![POn](https://user-images.githubusercontent.com/63488701/104958919-fe3c4680-599e-11eb-851e-73f26291d3e5.gif) +POn - `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. DIRECT is most common. ```c++ QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, - float Kp, float Ki, float Kd, uint8_t ControllerDirection) + float Kp, float Ki, float Kd, uint8_t ControllerDirection); ``` This allows you to use Proportional on Error without explicitly saying so. @@ -74,7 +74,7 @@ This allows you to use Proportional on Error without explicitly saying so. #### Compute ```c++ -bool QuickPID::Compute() +bool QuickPID::Compute(); ``` This function contains the PID algorithm and it should be called once every loop(). Most of the time it will just return false without doing anything. However, at a frequency specified by `SetSampleTime` it will calculate a new Output and return true. @@ -82,7 +82,7 @@ This function contains the PID algorithm and it should be called once every loop #### [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) ```c++ -void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 30) +void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 30); ``` The `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. It also determines the critical gain `Ku` and critical period `Tu` of the control system. @@ -98,13 +98,13 @@ Sets the AutoTune timeout where the default is 120 seconds. #### SetTunings ```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn) +void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn); ``` This function allows the controller's dynamic performance to be adjusted. It's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. The parameters are as described in the constructor. ```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd) +void QuickPID::SetTunings(float Kp, float Ki, float Kd); ``` Set Tunings using the last remembered POn setting. @@ -112,7 +112,7 @@ Set Tunings using the last remembered POn setting. #### SetSampleTime ```c++ -void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) +void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs); ``` Sets the period, in microseconds, at which the calculation is performed. The default is 100ms. @@ -120,7 +120,7 @@ Sets the period, in microseconds, at which the calculation is performed. The def #### SetOutputLimits ```c++ -void QuickPID::SetOutputLimits(int Min, int Max) +void QuickPID::SetOutputLimits(int Min, int Max); ``` The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. @@ -128,7 +128,7 @@ The PID controller is designed to vary its output within a given range. By defa #### SetMode ```c++ -void QuickPID::SetMode(uint8_t Mode) +void QuickPID::SetMode(uint8_t Mode); ``` Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (non-zero). when the transition from manual to automatic occurs, the controller is automatically initialized. @@ -136,7 +136,7 @@ Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (non-zero). #### Initialize ```c++ -void QuickPID::Initialize() +void QuickPID::Initialize(); ``` Does all the things that need to happen to ensure a bump-less transfer from manual to automatic mode. @@ -152,14 +152,14 @@ The PID will either be connected to a DIRECT acting process (+Output leads to +I #### Display_Functions ```c++ -float QuickPID::GetKp() -float QuickPID::GetKi() -float QuickPID::GetKd() -float QuickPID::GetKu() -float QuickPID::GetTu() -float QuickPID::GetTd() -uint8_t QuickPID::GetMode() -uint8_t QuickPID::GetDirection() +float QuickPID::GetKp(); +float QuickPID::GetKi(); +float QuickPID::GetKd(); +float QuickPID::GetKu(); +float QuickPID::GetTu(); +float QuickPID::GetTd(); +uint8_t QuickPID::GetMode(); +uint8_t QuickPID::GetDirection(); ``` These functions query the internal state of the PID. They're here for display purposes. @@ -174,6 +174,8 @@ A faster configuration of `analogRead()`where a preset of 32 is used. If the ar #### [AnalogWrite (PWM and DAC) for ESP32](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) +Use this link for reference. Note that if you're using QuickPID, there's no need to install the AnalogWrite library as this feature is already included. + #### Change Log #### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) diff --git a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino index 1a31a3b..3780fe2 100644 --- a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino +++ b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino @@ -24,7 +24,7 @@ const byte outputPin = 3; int Print = 0; // on(1) monitor, off(0) plotter int tuningRule = 1; // see above table -float POn = 1.0; // Mix of PonE to PonM (0.0-1.0) +float POn = 1.0; // mix of PonE to PonM (0.0-1.0) unsigned long timeout = 120; // AutoTune timeout (sec) int Input, Output, Setpoint; @@ -52,7 +52,7 @@ void setup() Serial.print(" Kp: "); Serial.print(myQuickPID.GetKp()); Serial.print(" Ki: "); Serial.print(myQuickPID.GetKi()); Serial.print(" Kd: "); Serial.println(myQuickPID.GetKd()); - delay(6000); + delay(3000); } } From 099be925b3db31bbc86dcb0aac3ed53bff4133a3 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 18 Apr 2021 23:26:28 -0400 Subject: [PATCH 031/101] Added ESP32-S2 support --- QuickPID.cpp | 2 +- README.md | 4 ++ library.properties | 2 +- utility/analogWrite.cpp | 145 +++++++++++++++++++++++----------------- utility/analogWrite.h | 8 ++- 5 files changed, 95 insertions(+), 66 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 486caaf..9467214 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.2.3 + QuickPID Library for Arduino - Version 2.2.4 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library, licensed under the MIT License **********************************************************************************/ diff --git a/README.md b/README.md index 9b33a3b..c73e7ae 100644 --- a/README.md +++ b/README.md @@ -180,6 +180,10 @@ Use this link for reference. Note that if you're using QuickPID, there's no need #### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) +- Added ESP32-S2 support + +#### Version 2.2.3 + - Updated compatibility with the ESP32 Arduino framework #### Version 2.2.2 diff --git a/library.properties b/library.properties index eaa2cac..92c0589 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.2.3 +version=2.2.4 author=David Lloyd maintainer=David Lloyd sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. diff --git a/utility/analogWrite.cpp b/utility/analogWrite.cpp index b70b70b..7a323a6 100644 --- a/utility/analogWrite.cpp +++ b/utility/analogWrite.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 1.0.0 + AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 1.1.0 by dlloydev https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite This Library is licensed under the MIT License **********************************************************************************/ @@ -7,12 +7,23 @@ #include #include "analogWrite.h" -pinStatus_t pinsStatus[16] = { - { 1, -1, 5000, 13, 0 }, { 3, -1, 5000, 13, 0 }, - { 5, -1, 5000, 13, 0 }, { 7, -1, 5000, 13, 0 }, - { 9, -1, 5000, 13, 0 }, {11, -1, 5000, 13, 0 }, - {13, -1, 5000, 13, 0 }, {15, -1, 5000, 13, 0 } +#if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) +pinStatus_t pinsStatus[8] = { + {0, -1, 5000, 13, 0 }, {2, -1, 5000, 13, 0 }, + {4, -1, 5000, 13, 0 }, {6, -1, 5000, 13, 0 }, + {1, -1, 5000, 13, 0 }, {3, -1, 5000, 13, 0 }, + {5, -1, 5000, 13, 0 }, {7, -1, 5000, 13, 0 } }; +const uint8_t chd = 1; +#else //ESP32 +pinStatus_t pinsStatus[8] = { + { 0, -1, 5000, 13, 0 }, { 2, -1, 5000, 13, 0 }, + { 4, -1, 5000, 13, 0 }, { 6, -1, 5000, 13, 0 }, + { 8, -1, 5000, 13, 0 }, {10, -1, 5000, 13, 0 }, + {12, -1, 5000, 13, 0 }, {14, -1, 5000, 13, 0 } +}; +const uint8_t chd = 2; +#endif void analogWrite(int8_t pin, int32_t value) { if (pin == DAC1 || pin == DAC2) { //dac @@ -22,23 +33,23 @@ void analogWrite(int8_t pin, int32_t value) { int8_t ch = getChannel(pin); if (ch >= 0) { if (value == -1) { //detach pin - pinsStatus[ch / 2].pin = -1; - pinsStatus[ch / 2].frequency = 5000; - pinsStatus[ch / 2].resolution = 13; - ledcDetachPin(pinsStatus[ch / 2].pin); + pinsStatus[ch / chd].pin = -1; + pinsStatus[ch / chd].frequency = 5000; + pinsStatus[ch / chd].resolution = 13; + ledcDetachPin(pinsStatus[ch / chd].pin); REG_SET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL, GPIO_MODE_DEF_DISABLE); - } else { - int32_t valueMax = (pow(2, pinsStatus[ch / 2].resolution)) - 1; - if (value > valueMax) { + } else { // attached + int32_t valueMax = (pow(2, pinsStatus[ch / chd].resolution)) - 1; + if (value > valueMax) { // full ON value = valueMax + 1; ledcDetachPin(pin); pinMode(pin, OUTPUT); digitalWrite(pin, HIGH); - } else { - ledcSetup(ch, pinsStatus[ch / 2].frequency, pinsStatus[ch / 2].resolution); + } else { // write PWM + ledcSetup(ch, pinsStatus[ch / chd].frequency, pinsStatus[ch / chd].resolution); ledcWrite(ch, value); } - pinsStatus[ch / 2].value = value; + pinsStatus[ch / chd].value = value; } } } @@ -47,10 +58,12 @@ void analogWrite(int8_t pin, int32_t value) { float analogWriteFrequency(int8_t pin, float frequency) { int8_t ch = getChannel(pin); if (ch >= 0) { - pinsStatus[ch / 2].frequency = frequency; - pinsStatus[ch / 2].pin = pin; - ledcSetup(ch, frequency, pinsStatus[ch / 2].resolution); - ledcWrite(ch, pinsStatus[ch / 2].value); + if ((pinsStatus[ch / chd].pin) > 47) return -1; + pinsStatus[ch / chd].pin = pin; + pinsStatus[ch / chd].frequency = frequency; + //ledcChangeFrequency(ch, frequency, pinsStatus[ch / chd].resolution); + ledcSetup(ch, frequency, pinsStatus[ch / chd].resolution); + ledcWrite(ch, pinsStatus[ch / chd].value); } return ledcReadFreq(ch); } @@ -58,36 +71,45 @@ float analogWriteFrequency(int8_t pin, float frequency) { int32_t analogWriteResolution(int8_t pin, uint8_t resolution) { int8_t ch = getChannel(pin); if (ch >= 0) { - pinsStatus[ch / 2].resolution = resolution; - pinsStatus[ch / 2].pin = pin; - ledcSetup(ch, pinsStatus[ch].frequency, resolution); - ledcWrite(ch, pinsStatus[ch].value); + if ((pinsStatus[ch / chd].pin) > 47) return -1; + pinsStatus[ch / chd].pin = pin; + pinsStatus[ch / chd].resolution = resolution; + ledcSetup(ch, pinsStatus[ch / chd].frequency, resolution); + ledcWrite(ch, pinsStatus[ch / chd].value); } return pow(2, resolution); } int8_t getChannel(int8_t pin) { - if ((pinMask >> pin) & 1) { //valid pin number? - if (REG_GET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL)) { //gpio pin function? - for (int8_t i = 0; i < 8; i++) { //search channels for the pin - if (pinsStatus[i].pin == pin) { //pin found - return pinsStatus[i].channel; + if (!((pinMask >> pin) & 1)) return -1; //not pwm pin + for (int8_t i = 0; i < 8; i++) { + int8_t ch = pinsStatus[i].channel; + if (pinsStatus[ch / chd].pin == pin) { + return ch; + break; + } + } + for (int8_t i = 0; i < 8; i++) { + int8_t ch = pinsStatus[i].channel; + if ((REG_GET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL)) == 0) { //free pin + if (pinsStatus[ch / chd].pin == -1) { //free channel + if ((ledcRead(ch) < 1) && (ledcReadFreq(ch) < 1)) { //free timer + pinsStatus[ch / chd].pin = pin; + ledcAttachPin(pin, ch); + return ch; break; - } - } - return -99; //pin is being used externally - } else { //pin is not used - for (int8_t i = 0; i < 8; i++) { //search for free channel - if (pinsStatus[i].pin == -1) { //channel is free - pinsStatus[i].pin = pin; - ledcAttachPin(pin, pinsStatus[i].channel); - return pinsStatus[i].channel; + } else { + pinsStatus[ch / chd].pin = 88; //occupied timer + return -1; break; } } + } else { + return -1; //occupied pin + break; } } - return -88; //no available resources + return -1; } void printPinsStatus() { @@ -95,34 +117,33 @@ void printPinsStatus() { for (int i = 0; i < muxSize; i++) { if ((pinMask >> i) & 1) { Serial.print(i); Serial.print(", "); - if (i == 18) Serial.println(); - if (i == 18) Serial.print(" "); } } Serial.println(); + Serial.println(); for (int i = 0; i < 8; i++) { - Serial.print("ch"); Serial.print(pinsStatus[i].channel); Serial.print(" "); - if (pinsStatus[i].channel < 10) Serial.print(" "); - if (pinsStatus[i].pin == -1) { - Serial.print(pinsStatus[i].pin); Serial.print(" "); - } else { - Serial.print("pin"); Serial.print(pinsStatus[i].pin); Serial.print(" "); - } - if (pinsStatus[i].pin < 10) Serial.print(" "); - if (pinsStatus[i].frequency < 10000) Serial.print(" "); - if (pinsStatus[i].frequency < 1000) Serial.print(" "); - if (pinsStatus[i].frequency < 100) Serial.print(" "); - if (pinsStatus[i].frequency < 10) Serial.print(" "); - Serial.print(pinsStatus[i].frequency); Serial.print(" Hz "); - if (pinsStatus[i].resolution < 10) Serial.print(" "); - Serial.print(pinsStatus[i].resolution); Serial.print("-bit "); - Serial.print("val "); - if (pinsStatus[i].value < 10000) Serial.print(" "); - if (pinsStatus[i].value < 1000) Serial.print(" "); - if (pinsStatus[i].value < 100) Serial.print(" "); - if (pinsStatus[i].value < 10) Serial.print(" "); - Serial.print(pinsStatus[i].value); + int ch = pinsStatus[i].channel; + Serial.print("ch: "); + if (ch < 10) Serial.print(" "); Serial.print(ch); Serial.print(" "); + Serial.print("Pin: "); + if ((pinsStatus[ch / chd].pin >= 0) && (pinsStatus[ch / chd].pin < 10)) Serial.print(" "); + Serial.print(pinsStatus[ch / chd].pin); Serial.print(" "); + Serial.print("Hz: "); + if (ledcReadFreq(ch) < 10000) Serial.print(" "); + if (ledcReadFreq(ch) < 1000) Serial.print(" "); + if (ledcReadFreq(ch) < 100) Serial.print(" "); + if (ledcReadFreq(ch) < 10) Serial.print(" "); + Serial.print(ledcReadFreq(ch)); Serial.print(" "); + Serial.print("Bits: "); + if (pinsStatus[ch / chd].resolution < 10) Serial.print(" "); + Serial.print(pinsStatus[ch / chd].resolution); Serial.print(" "); + Serial.print("Duty: "); + if (pinsStatus[ch / chd].value < 10000) Serial.print(" "); + if (pinsStatus[ch / chd].value < 1000) Serial.print(" "); + if (pinsStatus[ch / chd].value < 100) Serial.print(" "); + if (pinsStatus[ch / chd].value < 10) Serial.print(" "); + Serial.print(pinsStatus[ch / chd].value); Serial.println(); } } diff --git a/utility/analogWrite.h b/utility/analogWrite.h index 36b7782..f3bb8f0 100644 --- a/utility/analogWrite.h +++ b/utility/analogWrite.h @@ -1,13 +1,16 @@ #ifndef _ESP32_ESP32S2_ANALOG_WRITE_ #define _ESP32_ESP32S2_ANALOG_WRITE_ + #include +#if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) + #if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) #define NUM_OUTPUT_PINS 45 #define DAC1 17 #define DAC2 18 const uint8_t muxSize = 48; -const uint64_t pinMask = 0x7FE00207FFE; //PWM +const uint64_t pinMask = 0x27FE00207FFE; //PWM #else #define NUM_OUTPUT_PINS 34 #define DAC1 25 @@ -30,4 +33,5 @@ void analogWrite(int8_t pin, int32_t value = 0); int8_t getChannel(int8_t pin); void printPinsStatus(void); -#endif +#endif //ESP32 or ARDUINO_ARCH_ESP32 +#endif //_ESP32_ESP32S2_ANALOG_WRITE_ From f408d872086fb369cb6649b04502139c4e3d9e51 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 2 May 2021 22:24:07 -0400 Subject: [PATCH 032/101] Update ESP32 Support --- QuickPID.cpp | 2 +- QuickPID.h | 2 +- library.properties | 2 +- utility/analogWrite.cpp | 296 +++++++++++++++++++++++++++++----------- utility/analogWrite.h | 27 +++- 5 files changed, 245 insertions(+), 84 deletions(-) diff --git a/QuickPID.cpp b/QuickPID.cpp index 9467214..16100c0 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.2.4 + QuickPID Library for Arduino - Version 2.2.5 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library, licensed under the MIT License **********************************************************************************/ diff --git a/QuickPID.h b/QuickPID.h index e5e4673..495814c 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -111,7 +111,7 @@ static const byte TRY_AUTOMATIC = 1; }; -#if defined(ESP32) +#if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) #include "utility/analogWrite.h" #endif diff --git a/library.properties b/library.properties index 92c0589..56373be 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.2.4 +version=2.2.5 author=David Lloyd maintainer=David Lloyd sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. diff --git a/utility/analogWrite.cpp b/utility/analogWrite.cpp index 7a323a6..6d4d20a 100644 --- a/utility/analogWrite.cpp +++ b/utility/analogWrite.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 1.1.0 + AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 2.0.6 by dlloydev https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite This Library is licensed under the MIT License **********************************************************************************/ @@ -7,80 +7,56 @@ #include #include "analogWrite.h" +namespace aw { + #if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) pinStatus_t pinsStatus[8] = { - {0, -1, 5000, 13, 0 }, {2, -1, 5000, 13, 0 }, - {4, -1, 5000, 13, 0 }, {6, -1, 5000, 13, 0 }, - {1, -1, 5000, 13, 0 }, {3, -1, 5000, 13, 0 }, - {5, -1, 5000, 13, 0 }, {7, -1, 5000, 13, 0 } + {0, -1, 980, 8, 0, 0 }, {2, -1, 980, 8, 0, 0 }, + {4, -1, 980, 8, 0, 0 }, {6, -1, 980, 8, 0, 0 }, + {1, -1, 980, 8, 0, 0 }, {3, -1, 980, 8, 0, 0 }, + {5, -1, 980, 8, 0, 0 }, {7, -1, 980, 8, 0, 0 } }; const uint8_t chd = 1; #else //ESP32 pinStatus_t pinsStatus[8] = { - { 0, -1, 5000, 13, 0 }, { 2, -1, 5000, 13, 0 }, - { 4, -1, 5000, 13, 0 }, { 6, -1, 5000, 13, 0 }, - { 8, -1, 5000, 13, 0 }, {10, -1, 5000, 13, 0 }, - {12, -1, 5000, 13, 0 }, {14, -1, 5000, 13, 0 } + { 0, -1, 980, 8, 0, 0 }, { 2, -1, 980, 8, 0, 0 }, + { 4, -1, 980, 8, 0, 0 }, { 6, -1, 980, 8, 0, 0 }, + { 8, -1, 980, 8, 0, 0 }, {10, -1, 980, 8, 0, 0 }, + {12, -1, 980, 8, 0, 0 }, {14, -1, 980, 8, 0, 0 } }; const uint8_t chd = 2; #endif -void analogWrite(int8_t pin, int32_t value) { - if (pin == DAC1 || pin == DAC2) { //dac - if (value > 255) value = 255; - dacWrite(pin, value); - } else { //pwm - int8_t ch = getChannel(pin); - if (ch >= 0) { - if (value == -1) { //detach pin - pinsStatus[ch / chd].pin = -1; - pinsStatus[ch / chd].frequency = 5000; - pinsStatus[ch / chd].resolution = 13; - ledcDetachPin(pinsStatus[ch / chd].pin); - REG_SET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL, GPIO_MODE_DEF_DISABLE); - } else { // attached - int32_t valueMax = (pow(2, pinsStatus[ch / chd].resolution)) - 1; - if (value > valueMax) { // full ON - value = valueMax + 1; - ledcDetachPin(pin); - pinMode(pin, OUTPUT); - digitalWrite(pin, HIGH); - } else { // write PWM - ledcSetup(ch, pinsStatus[ch / chd].frequency, pinsStatus[ch / chd].resolution); - ledcWrite(ch, value); - } - pinsStatus[ch / chd].value = value; - } - } - } +float awLedcSetup(uint8_t ch, double frequency, uint8_t bits) { +#if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) + frequency *= 80; //workaround for issue 5050 + return ledcSetup(ch, frequency, bits) / 80; +#else //ESP32 + return ledcSetup(ch, frequency, bits); +#endif } -float analogWriteFrequency(int8_t pin, float frequency) { - int8_t ch = getChannel(pin); - if (ch >= 0) { - if ((pinsStatus[ch / chd].pin) > 47) return -1; - pinsStatus[ch / chd].pin = pin; - pinsStatus[ch / chd].frequency = frequency; - //ledcChangeFrequency(ch, frequency, pinsStatus[ch / chd].resolution); - ledcSetup(ch, frequency, pinsStatus[ch / chd].resolution); - ledcWrite(ch, pinsStatus[ch / chd].value); - } - return ledcReadFreq(ch); +void awDetachPin(uint8_t pin, uint8_t ch) { + pinsStatus[ch / chd].pin = -1; + pinsStatus[ch / chd].value = 0; + pinsStatus[ch / chd].frequency = 980; + pinsStatus[ch / chd].resolution = 8; + pinsStatus[ch / chd].phase = 0; + ledcWrite(ch / chd, 0); + ledcSetup(ch / chd, 0, 0); + ledcDetachPin(pinsStatus[ch / chd].pin); + REG_SET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL, GPIO_MODE_DEF_DISABLE); } -int32_t analogWriteResolution(int8_t pin, uint8_t resolution) { - int8_t ch = getChannel(pin); - if (ch >= 0) { - if ((pinsStatus[ch / chd].pin) > 47) return -1; - pinsStatus[ch / chd].pin = pin; - pinsStatus[ch / chd].resolution = resolution; - ledcSetup(ch, pinsStatus[ch / chd].frequency, resolution); - ledcWrite(ch, pinsStatus[ch / chd].value); - } - return pow(2, resolution); +float awLedcReadFreq(uint8_t ch) { +#if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) + return ledcReadFreq(ch) / 80; //workaround for issue 5050 +#else //ESP32 + return ledcReadFreq(ch); +#endif } -int8_t getChannel(int8_t pin) { +int8_t awGetChannel(int8_t pin) { if (!((pinMask >> pin) & 1)) return -1; //not pwm pin for (int8_t i = 0; i < 8; i++) { int8_t ch = pinsStatus[i].channel; @@ -95,6 +71,7 @@ int8_t getChannel(int8_t pin) { if (pinsStatus[ch / chd].pin == -1) { //free channel if ((ledcRead(ch) < 1) && (ledcReadFreq(ch) < 1)) { //free timer pinsStatus[ch / chd].pin = pin; + aw::awLedcSetup(ch, pinsStatus[ch / chd].frequency, pinsStatus[ch / chd].resolution); ledcAttachPin(pin, ch); return ch; break; @@ -112,10 +89,172 @@ int8_t getChannel(int8_t pin) { return -1; } +} //namespace aw + +float analogWrite(int8_t pin, int32_t value) { + if (pin == DAC1 || pin == DAC2) { //dac + if (value > 255) value = 255; + dacWrite(pin, value); + } else { + int8_t ch = aw::awGetChannel(pin); + if (ch >= 0) { + if (value == -1) aw::awDetachPin(pin, ch); + else { // write PWM + uint8_t bits = aw::pinsStatus[ch / aw::chd].resolution; + if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain + if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high + if (ledcRead(ch) != value) ledcWrite(ch, value); + aw::pinsStatus[ch / aw::chd].value = value; + } + } + return aw::awLedcReadFreq(ch); + } + return 0; +} + +float analogWrite(int8_t pin, int32_t value, float frequency) { + if (pin == DAC1 || pin == DAC2) { //dac + if (value > 255) value = 255; + dacWrite(pin, value); + } else { + int8_t ch = aw::awGetChannel(pin); + if (ch >= 0) { + if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; + if (value == -1) aw::awDetachPin(pin, ch); + else { // write PWM + uint8_t bits = aw::pinsStatus[ch / aw::chd].resolution; + if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain + if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high + if (aw::pinsStatus[ch / aw::chd].frequency != frequency) { + aw::awLedcSetup(ch, frequency, bits); + ledcWrite(ch, value); + aw::pinsStatus[ch / aw::chd].frequency = frequency; + } + if (aw::pinsStatus[ch / aw::chd].value != value) { + ledcWrite(ch, value); + aw::pinsStatus[ch / aw::chd].value = value; + } + } + } + return aw::awLedcReadFreq(ch); + } + return 0; +} + +float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution) { + if (pin == DAC1 || pin == DAC2) { //dac + if (value > 255) value = 255; + dacWrite(pin, value); + } else { + int8_t ch = aw::awGetChannel(pin); + if (ch >= 0) { + if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; + if (value == -1) aw::awDetachPin(pin, ch); + else { // write PWM + uint8_t bits = resolution & 0xF; + if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain + if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high + if ((aw::pinsStatus[ch / aw::chd].frequency != frequency) || (aw::pinsStatus[ch / aw::chd].resolution != bits)) { + aw::awLedcSetup(ch, frequency, bits); + ledcWrite(ch, value); + aw::pinsStatus[ch / aw::chd].frequency = frequency; + aw::pinsStatus[ch / aw::chd].resolution = bits; + } + if (aw::pinsStatus[ch / aw::chd].value != value) { + ledcWrite(ch, value); + aw::pinsStatus[ch / aw::chd].value = value; + } + } + } + return aw::awLedcReadFreq(ch); + } + return 0; +} + +float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution, uint32_t phase) { + if (pin == DAC1 || pin == DAC2) { //dac + if (value > 255) value = 255; + dacWrite(pin, value); + } else { + int8_t ch = aw::awGetChannel(pin); + if (ch >= 0) { + if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; + if (value == -1) aw::awDetachPin(pin, ch); + else { // write PWM + uint8_t bits = resolution & 0xF; + if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain + if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high + if ((aw::pinsStatus[ch / aw::chd].frequency != frequency) || (aw::pinsStatus[ch / aw::chd].resolution != bits)) { + aw::awLedcSetup(ch, frequency, bits); + ledcWrite(ch, value); + aw::pinsStatus[ch / aw::chd].frequency = frequency; + aw::pinsStatus[ch / aw::chd].resolution = bits; + } + if (aw::pinsStatus[ch / aw::chd].phase != phase) { + uint32_t group = (ch / 8), timer = ((ch / 2) % 4); + aw::ledc_channel_config_t ledc_channel { + (uint8_t)pin, + (aw::ledc_mode_t)group, + (aw::ledc_channel_t)ch, + aw::LEDC_INTR_DISABLE, + (aw::ledc_timer_t)timer, + (uint32_t)value, + (int)phase, + }; + ledc_channel_config(&ledc_channel); + ledc_set_duty_with_hpoint((aw::ledc_mode_t)group, (aw::ledc_channel_t)ch, value, phase); + aw::pinsStatus[ch / aw::chd].phase = phase; + } + if (aw::pinsStatus[ch / aw::chd].value != value) { + ledcWrite(ch, value); + aw::pinsStatus[ch / aw::chd].value = value; + } + } + } + return aw::awLedcReadFreq(ch); + } + return 0; +} + +float analogWriteFrequency(int8_t pin, float frequency) { + int8_t ch = aw::awGetChannel(pin); + if (ch >= 0) { + if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; + if (aw::pinsStatus[ch / aw::chd].frequency != frequency) { + aw::awLedcSetup(ch, frequency, aw::pinsStatus[ch / aw::chd].resolution); + ledcWrite(ch, aw::pinsStatus[ch / aw::chd].value); + aw::pinsStatus[ch / aw::chd].frequency = frequency; + } + } + return aw::awLedcReadFreq(ch); +} + +int32_t analogWriteResolution(int8_t pin, uint8_t resolution) { + int8_t ch = aw::awGetChannel(pin); + if (ch >= 0) { + if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; + if (aw::pinsStatus[ch / aw::chd].resolution != resolution) { + aw::awLedcSetup(ch, aw::pinsStatus[ch / aw::chd].frequency, resolution & 0xF); + ledcWrite(ch, aw::pinsStatus[ch / aw::chd].value); + aw::pinsStatus[ch / aw::chd].resolution = resolution & 0xF; + } + } + return 1 << resolution & 0xF; +} + +void setPinsStatusDefaults(int32_t value, float frequency, uint8_t resolution, uint32_t phase) { + for (int8_t i = 0; i < 8; i++) { + aw::pinsStatus[i].value = value; + aw::pinsStatus[i].frequency = frequency; + aw::pinsStatus[i].resolution = resolution; + aw::pinsStatus[i].phase = phase; + } +} + void printPinsStatus() { Serial.print("PWM pins: "); - for (int i = 0; i < muxSize; i++) { - if ((pinMask >> i) & 1) { + for (int i = 0; i < aw::muxSize; i++) { + if ((aw::pinMask >> i) & 1) { Serial.print(i); Serial.print(", "); } } @@ -123,27 +262,32 @@ void printPinsStatus() { Serial.println(); for (int i = 0; i < 8; i++) { - int ch = pinsStatus[i].channel; + int ch = aw::pinsStatus[i].channel; Serial.print("ch: "); if (ch < 10) Serial.print(" "); Serial.print(ch); Serial.print(" "); Serial.print("Pin: "); - if ((pinsStatus[ch / chd].pin >= 0) && (pinsStatus[ch / chd].pin < 10)) Serial.print(" "); - Serial.print(pinsStatus[ch / chd].pin); Serial.print(" "); + if ((aw::pinsStatus[ch / aw::chd].pin >= 0) && (aw::pinsStatus[ch / aw::chd].pin < 10)) Serial.print(" "); + Serial.print(aw::pinsStatus[ch / aw::chd].pin); Serial.print(" "); Serial.print("Hz: "); - if (ledcReadFreq(ch) < 10000) Serial.print(" "); - if (ledcReadFreq(ch) < 1000) Serial.print(" "); - if (ledcReadFreq(ch) < 100) Serial.print(" "); - if (ledcReadFreq(ch) < 10) Serial.print(" "); - Serial.print(ledcReadFreq(ch)); Serial.print(" "); + if (aw::awLedcReadFreq(ch) < 10000) Serial.print(" "); + if (aw::awLedcReadFreq(ch) < 1000) Serial.print(" "); + if (aw::awLedcReadFreq(ch) < 100) Serial.print(" "); + if (aw::awLedcReadFreq(ch) < 10) Serial.print(" "); + Serial.print(aw::awLedcReadFreq(ch)); Serial.print(" "); Serial.print("Bits: "); - if (pinsStatus[ch / chd].resolution < 10) Serial.print(" "); - Serial.print(pinsStatus[ch / chd].resolution); Serial.print(" "); + if (aw::pinsStatus[ch / aw::chd].resolution < 10) Serial.print(" "); + Serial.print(aw::pinsStatus[ch / aw::chd].resolution); Serial.print(" "); Serial.print("Duty: "); - if (pinsStatus[ch / chd].value < 10000) Serial.print(" "); - if (pinsStatus[ch / chd].value < 1000) Serial.print(" "); - if (pinsStatus[ch / chd].value < 100) Serial.print(" "); - if (pinsStatus[ch / chd].value < 10) Serial.print(" "); - Serial.print(pinsStatus[ch / chd].value); + if (aw::pinsStatus[ch / aw::chd].value < 10000) Serial.print(" "); + if (aw::pinsStatus[ch / aw::chd].value < 1000) Serial.print(" "); + if (aw::pinsStatus[ch / aw::chd].value < 100) Serial.print(" "); + if (aw::pinsStatus[ch / aw::chd].value < 10) Serial.print(" "); + Serial.print(aw::pinsStatus[ch / aw::chd].value); Serial.print(" "); + Serial.print("Ø: "); + if (aw::pinsStatus[ch / aw::chd].phase < 1000) Serial.print(" "); + if (aw::pinsStatus[ch / aw::chd].phase < 100) Serial.print(" "); + if (aw::pinsStatus[ch / aw::chd].phase < 10) Serial.print(" "); + Serial.print(aw::pinsStatus[ch / aw::chd].phase); Serial.println(); } } diff --git a/utility/analogWrite.h b/utility/analogWrite.h index f3bb8f0..e584060 100644 --- a/utility/analogWrite.h +++ b/utility/analogWrite.h @@ -5,13 +5,19 @@ #if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) +namespace aw { + +#include "driver/ledc.h" + #if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) + #define NUM_OUTPUT_PINS 45 #define DAC1 17 #define DAC2 18 const uint8_t muxSize = 48; const uint64_t pinMask = 0x27FE00207FFE; //PWM -#else + +#else //ESP32 #define NUM_OUTPUT_PINS 34 #define DAC1 25 #define DAC2 26 @@ -25,12 +31,23 @@ typedef struct pinStatus { float frequency; uint8_t resolution; uint32_t value; + uint32_t phase; } pinStatus_t; -float analogWriteFrequency(int8_t pin, float frequency = 5000); -int32_t analogWriteResolution(int8_t pin, uint8_t resolution = 13); -void analogWrite(int8_t pin, int32_t value = 0); -int8_t getChannel(int8_t pin); +float awLedcSetup(uint8_t ch, double frequency, uint8_t bits); +void awDetachPin(uint8_t pin, uint8_t ch); +float awLedcReadFreq(uint8_t ch); +int8_t awGetChannel(int8_t pin); + +} //namespace aw + +float analogWriteFrequency(int8_t pin, float frequency = 980); +int32_t analogWriteResolution(int8_t pin, uint8_t resolution = 8); +float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution, uint32_t phase); +float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution); +float analogWrite(int8_t pin, int32_t value, float frequency); +float analogWrite(int8_t pin, int32_t value); +void setPinsStatusDefaults(int32_t value = 0, float frequency = 980, uint8_t resolution = 8, uint32_t phase = 0); void printPinsStatus(void); #endif //ESP32 or ARDUINO_ARCH_ESP32 From 755867b0e915f730462e28fd6d67b1bdbfedf886 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Thu, 6 May 2021 10:52:55 -0400 Subject: [PATCH 033/101] Update - Changed Input, Output and Setpoint parameters to float. - Fix recent compile issue for AVR boards --- QuickPID.cpp | 7 ++++--- QuickPID.h | 12 ++++++------ README.md | 11 +++++------ utility/analogWrite.cpp => analogWrite.cpp | 5 ++++- utility/analogWrite.h => analogWrite.h | 0 examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino | 7 +++++-- .../QuickPID_AdaptiveTunings.ino | 2 +- examples/QuickPID_Basic/QuickPID_Basic.ino | 2 +- examples/QuickPID_PonM/QuickPID_PonM.ino | 2 +- .../QuickPID_RelayOutput/QuickPID_RelayOutput.ino | 2 +- library.json | 2 ++ library.properties | 2 +- 12 files changed, 31 insertions(+), 23 deletions(-) rename utility/analogWrite.cpp => analogWrite.cpp (99%) rename utility/analogWrite.h => analogWrite.h (100%) diff --git a/QuickPID.cpp b/QuickPID.cpp index 16100c0..da72e50 100644 --- a/QuickPID.cpp +++ b/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.2.5 + QuickPID Library for Arduino - Version 2.2.6 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library, licensed under the MIT License **********************************************************************************/ @@ -9,13 +9,14 @@ #else #include "WProgram.h" #endif + #include "QuickPID.h" /* Constructor ******************************************************************** The parameters specified here are those for for which we can't set up reliable defaults, so we need to have the user set them. **********************************************************************************/ -QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, +QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, float POn = 1, uint8_t ControllerDirection = 0) { myOutput = Output; @@ -35,7 +36,7 @@ QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, To allow backwards compatability for v1.1, or for people that just want to use Proportional on Error without explicitly saying so. **********************************************************************************/ -QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, +QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, uint8_t ControllerDirection) : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = 0) { diff --git a/QuickPID.h b/QuickPID.h index 495814c..66dc03e 100644 --- a/QuickPID.h +++ b/QuickPID.h @@ -21,10 +21,10 @@ static const byte TRY_AUTOMATIC = 1; // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(int*, int*, int*, float, float, float, float, uint8_t); + QuickPID(float*, float*, float*, float, float, float, float, uint8_t); // Overload constructor with proportional mode. Links the PID to Input, Output, Setpoint and Tuning Parameters. - QuickPID(int*, int*, int*, float, float, float, uint8_t); + QuickPID(float*, float*, float*, float, float, float, uint8_t); // Sets PID to either Manual (0) or Auto (non-0). void SetMode(uint8_t Mode); @@ -92,9 +92,9 @@ static const byte TRY_AUTOMATIC = 1; uint8_t controllerDirection; - int *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a - int *myOutput; // hard link between the variables and the PID, freeing the user from having - int *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. + float *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a + float *myOutput; // hard link between the variables and the PID, freeing the user from having + float *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. uint32_t sampleTimeUs, lastTime; int outMin, outMax, error; @@ -112,7 +112,7 @@ static const byte TRY_AUTOMATIC = 1; }; #if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) -#include "utility/analogWrite.h" +#include "analogWrite.h" #endif #endif // QuickPID.h diff --git a/README.md b/README.md index c73e7ae..b5002b5 100644 --- a/README.md +++ b/README.md @@ -52,7 +52,7 @@ The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function #### QuickPID_Constructor ```c++ -QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, +QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, float POn, uint8_t ControllerDirection); ``` @@ -65,7 +65,7 @@ QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, - `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. DIRECT is most common. ```c++ -QuickPID::QuickPID(int* Input, int* Output, int* Setpoint, +QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, uint8_t ControllerDirection); ``` @@ -180,11 +180,10 @@ Use this link for reference. Note that if you're using QuickPID, there's no need #### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -- Added ESP32-S2 support +#### Version 2.2.6 -#### Version 2.2.3 - -- Updated compatibility with the ESP32 Arduino framework +- Changed Input, Output and Setpoint parameters to float. +- Updated compatibility with the ESP32 AnalogWrite #### Version 2.2.2 diff --git a/utility/analogWrite.cpp b/analogWrite.cpp similarity index 99% rename from utility/analogWrite.cpp rename to analogWrite.cpp index 6d4d20a..9cec264 100644 --- a/utility/analogWrite.cpp +++ b/analogWrite.cpp @@ -3,8 +3,9 @@ by dlloydev https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite This Library is licensed under the MIT License **********************************************************************************/ - #include + +#if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) #include "analogWrite.h" namespace aw { @@ -18,6 +19,7 @@ pinStatus_t pinsStatus[8] = { }; const uint8_t chd = 1; #else //ESP32 + pinStatus_t pinsStatus[8] = { { 0, -1, 980, 8, 0, 0 }, { 2, -1, 980, 8, 0, 0 }, { 4, -1, 980, 8, 0, 0 }, { 6, -1, 980, 8, 0, 0 }, @@ -291,3 +293,4 @@ void printPinsStatus() { Serial.println(); } } +#endif //ESP32 diff --git a/utility/analogWrite.h b/analogWrite.h similarity index 100% rename from utility/analogWrite.h rename to analogWrite.h diff --git a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino index 3780fe2..83685b5 100644 --- a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino +++ b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino @@ -27,7 +27,7 @@ int tuningRule = 1; // see above table float POn = 1.0; // mix of PonE to PonM (0.0-1.0) unsigned long timeout = 120; // AutoTune timeout (sec) -int Input, Output, Setpoint; +float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); @@ -35,6 +35,8 @@ QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); void setup() { Serial.begin(115200); + Serial.println(); + myQuickPID.AutoTune(inputPin, outputPin, tuningRule, Print, timeout); myQuickPID.SetTunings(myQuickPID.GetKp(), myQuickPID.GetKi(), myQuickPID.GetKd()); myQuickPID.SetSampleTimeUs(5000); // recommend 5000µs (5ms) minimum @@ -52,7 +54,8 @@ void setup() Serial.print(" Kp: "); Serial.print(myQuickPID.GetKp()); Serial.print(" Ki: "); Serial.print(myQuickPID.GetKi()); Serial.print(" Kd: "); Serial.println(myQuickPID.GetKd()); - delay(3000); + Serial.println(); + delay(5000); //view results } } diff --git a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino index f64d470..0db3b2c 100644 --- a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino +++ b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino @@ -15,7 +15,7 @@ #define PIN_OUTPUT 3 //Define Variables we'll be connecting to -int Setpoint, Input, Output; +float Setpoint, Input, Output; //Define the aggressive and conservative and POn Tuning Parameters float aggKp = 4, aggKi = 0.2, aggKd = 1; diff --git a/examples/QuickPID_Basic/QuickPID_Basic.ino b/examples/QuickPID_Basic/QuickPID_Basic.ino index d36ca0f..8792a33 100644 --- a/examples/QuickPID_Basic/QuickPID_Basic.ino +++ b/examples/QuickPID_Basic/QuickPID_Basic.ino @@ -9,7 +9,7 @@ #define PIN_OUTPUT 3 //Define Variables we'll be connecting to -int Setpoint, Input, Output; +float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; diff --git a/examples/QuickPID_PonM/QuickPID_PonM.ino b/examples/QuickPID_PonM/QuickPID_PonM.ino index 436f3a1..5df4e73 100644 --- a/examples/QuickPID_PonM/QuickPID_PonM.ino +++ b/examples/QuickPID_PonM/QuickPID_PonM.ino @@ -9,7 +9,7 @@ #include "QuickPID.h" //Define Variables we'll be connecting to -int Setpoint, Input, Output; +float Setpoint, Input, Output; float POn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) //Specify the links and initial tuning parameters diff --git a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino index be3997a..80e4b2b 100644 --- a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino +++ b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino @@ -22,7 +22,7 @@ #define RELAY_PIN 6 //Define Variables we'll be connecting to -int Setpoint, Input, Output; +float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; diff --git a/library.json b/library.json index 2f411f5..e7e761d 100644 --- a/library.json +++ b/library.json @@ -2,6 +2,8 @@ "name": "QuickPID", "keywords": "PID, controller, signal", "description": "A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards.", + "license": "MIT", + "version": "2.2.6", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 56373be..3ce43ab 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.2.5 +version=2.2.6 author=David Lloyd maintainer=David Lloyd sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. From f326bbbc1b2db83bb23b33481f97728575f8d12f Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 8 May 2021 13:40:36 -0400 Subject: [PATCH 034/101] Update - Fixed REVERSE acting controller mode. - now using src folder for source code - replaced defines with enumerated types and inline functions --- LICENSE | 3 +- README.md | 10 +++ .../AutoTune_RC_Filter/AutoTune_RC_Filter.ino | 16 +++-- .../QuickPID_AdaptiveTunings.ino | 4 +- examples/QuickPID_Basic/QuickPID_Basic.ino | 4 +- examples/QuickPID_PonM/QuickPID_PonM.ino | 4 +- .../QuickPID_RelayOutput.ino | 4 +- keywords.txt | 2 + library.json | 2 +- library.properties | 2 +- QuickPID.cpp => src/QuickPID.cpp | 46 +++++-------- QuickPID.h => src/QuickPID.h | 65 ++++++++++--------- analogWrite.cpp => src/analogWrite.cpp | 0 analogWrite.h => src/analogWrite.h | 1 + 14 files changed, 88 insertions(+), 75 deletions(-) rename QuickPID.cpp => src/QuickPID.cpp (92%) rename QuickPID.h => src/QuickPID.h (65%) rename analogWrite.cpp => src/analogWrite.cpp (100%) rename analogWrite.h => src/analogWrite.h (99%) diff --git a/LICENSE b/LICENSE index 2893251..1d6e014 100644 --- a/LICENSE +++ b/LICENSE @@ -1,4 +1,3 @@ -Copyright 2011-2017 Brett Beauregard brettbeauregard.com Copyright 2021 David Lloyd Permission is hereby granted, free of charge, to any person obtaining a copy of @@ -16,4 +15,4 @@ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN -CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. \ No newline at end of file diff --git a/README.md b/README.md index b5002b5..3759100 100644 --- a/README.md +++ b/README.md @@ -47,6 +47,10 @@ The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function kpd = kp * (1 - pOn) + kd; ``` +### Controller Action + +If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). Since the PWM and ADC peripherals on microcontrollers only operate with positive values, QuickPID only uses positive values for Input, Output and Setpoint. When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). See the example [AutoTune_RC_Filter.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino) for details. + ### Functions #### QuickPID_Constructor @@ -180,6 +184,12 @@ Use this link for reference. Note that if you're using QuickPID, there's no need #### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) +#### Version 2.2.7 + +- Fixed REVERSE acting controller mode. +- now using src folder for source code +- replaced defines with enumerated types and inline functions + #### Version 2.2.6 - Changed Input, Output and Setpoint parameters to float. diff --git a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino index 83685b5..f045e55 100644 --- a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino +++ b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino @@ -30,7 +30,11 @@ unsigned long timeout = 120; // AutoTune timeout (sec) float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); +// choose controller direction: +// DIRECT: Input increases when the output is increased or the error is positive (heating). +// REVERSE: Input decreases when the output is increased or when the error is positive (cooling). +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); +//QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::REVERSE); void setup() { @@ -39,8 +43,8 @@ void setup() myQuickPID.AutoTune(inputPin, outputPin, tuningRule, Print, timeout); myQuickPID.SetTunings(myQuickPID.GetKp(), myQuickPID.GetKi(), myQuickPID.GetKd()); - myQuickPID.SetSampleTimeUs(5000); // recommend 5000µs (5ms) minimum - myQuickPID.SetMode(AUTOMATIC); + myQuickPID.SetSampleTimeUs(4000); // 4ms + myQuickPID.SetMode(QuickPID::AUTOMATIC); Setpoint = 700; if (Print == 1) { @@ -66,7 +70,11 @@ void loop() Serial.print("Output:"); Serial.print(Output); Serial.print(","); Serial.println(","); - Input = myQuickPID.analogReadFast(inputPin); + if (myQuickPID.GetDirection() == QuickPID::REVERSE) { + Input = (1023 - myQuickPID.analogReadFast(inputPin)); // simulate reverse acting controller (cooling) + } else { + Input = (myQuickPID.analogReadFast(inputPin)); + } myQuickPID.Compute(); analogWrite(outputPin, Output); } diff --git a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino index 0db3b2c..83c31e1 100644 --- a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino +++ b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino @@ -24,7 +24,7 @@ float aggPOn = 1.0; // Range is 0.0 to 1.0 (1.0 is 100% P on Error, 0% P on Meas float consPOn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) //Specify the links and initial tuning parameters -QuickPID myQuickPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, aggPOn, DIRECT); +QuickPID myQuickPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, aggPOn, QuickPID::DIRECT); void setup() { @@ -33,7 +33,7 @@ void setup() Setpoint = 100; //turn the PID on - myQuickPID.SetMode(AUTOMATIC); + myQuickPID.SetMode(QuickPID::AUTOMATIC); } void loop() diff --git a/examples/QuickPID_Basic/QuickPID_Basic.ino b/examples/QuickPID_Basic/QuickPID_Basic.ino index 8792a33..1aa4ede 100644 --- a/examples/QuickPID_Basic/QuickPID_Basic.ino +++ b/examples/QuickPID_Basic/QuickPID_Basic.ino @@ -14,7 +14,7 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); void setup() { @@ -23,7 +23,7 @@ void setup() Setpoint = 100; //turn the PID on - myQuickPID.SetMode(AUTOMATIC); + myQuickPID.SetMode(QuickPID::AUTOMATIC); } void loop() diff --git a/examples/QuickPID_PonM/QuickPID_PonM.ino b/examples/QuickPID_PonM/QuickPID_PonM.ino index 5df4e73..febf186 100644 --- a/examples/QuickPID_PonM/QuickPID_PonM.ino +++ b/examples/QuickPID_PonM/QuickPID_PonM.ino @@ -13,7 +13,7 @@ float Setpoint, Input, Output; float POn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) //Specify the links and initial tuning parameters -QuickPID myQuickPID(&Input, &Output, &Setpoint, 2, 5, 1, POn, DIRECT); +QuickPID myQuickPID(&Input, &Output, &Setpoint, 2, 5, 1, POn, QuickPID::DIRECT); void setup() { @@ -22,7 +22,7 @@ void setup() Setpoint = 100; //turn the PID on - myQuickPID.SetMode(AUTOMATIC); + myQuickPID.SetMode(QuickPID::AUTOMATIC); } void loop() diff --git a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino index 80e4b2b..6c13323 100644 --- a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino +++ b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino @@ -28,7 +28,7 @@ float Setpoint, Input, Output; float Kp = 2, Ki = 5, Kd = 1; float POn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DIRECT); +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); unsigned int WindowSize = 5000; unsigned int minWindow = 500; @@ -46,7 +46,7 @@ void setup() myQuickPID.SetOutputLimits(0, WindowSize); //turn the PID on - myQuickPID.SetMode(AUTOMATIC); + myQuickPID.SetMode(QuickPID::AUTOMATIC); } void loop() diff --git a/keywords.txt b/keywords.txt index eed0dbb..157a12b 100644 --- a/keywords.txt +++ b/keywords.txt @@ -7,6 +7,8 @@ ########################################## QuickPID KEYWORD1 +mode_t KEYWORD1 +direction_t KEYWORD1 analog_write_channel_t KEYWORD1 ########################################## diff --git a/library.json b/library.json index e7e761d..ac3358f 100644 --- a/library.json +++ b/library.json @@ -3,7 +3,7 @@ "keywords": "PID, controller, signal", "description": "A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards.", "license": "MIT", - "version": "2.2.6", + "version": "2.2.7", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 3ce43ab..d55dbca 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.2.6 +version=2.2.7 author=David Lloyd maintainer=David Lloyd sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. diff --git a/QuickPID.cpp b/src/QuickPID.cpp similarity index 92% rename from QuickPID.cpp rename to src/QuickPID.cpp index da72e50..73b991f 100644 --- a/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.2.6 + QuickPID Library for Arduino - Version 2.2.7 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library, licensed under the MIT License **********************************************************************************/ @@ -17,7 +17,7 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, float POn = 1, uint8_t ControllerDirection = 0) { + float Kp, float Ki, float Kd, float POn = 1, QuickPID::direction_t ControllerDirection = DIRECT) { myOutput = Output; myInput = Input; @@ -37,9 +37,8 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, to use Proportional on Error without explicitly saying so. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, uint8_t ControllerDirection) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = 0) { - + float Kp, float Ki, float Kd, direction_t ControllerDirection = DIRECT) + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = DIRECT) { } /* Compute() ********************************************************************** @@ -56,9 +55,12 @@ bool QuickPID::Compute() { int input = *myInput; int dInput = input - lastInput; error = *mySetpoint - input; - - if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); // fixed point - else outputSum += (kpi * error) - (kpd * dInput); // floating-point + if (controllerDirection == REVERSE) { + error = -error; + dInput = -dInput; + } + if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); // fixed point + else outputSum += (kpi * error) - (kpd * dInput); // floating-point outputSum = CONSTRAIN(outputSum, outMin, outMax); *myOutput = outputSum; @@ -162,12 +164,6 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) { kd = Kd / SampleTimeSec; kpi = (kp * pOn) + ki; kpd = (kp * (1 - pOn)) + kd; - - if (controllerDirection == REVERSE) { - kp = (0 - kp); - ki = (0 - ki); - kd = (0 - kd); - } } /* SetTunings(...)************************************************************* @@ -209,7 +205,7 @@ void QuickPID::SetOutputLimits(int Min, int Max) { when the transition from manual to auto occurs, the controller is automatically initialized ******************************************************************************/ -void QuickPID::SetMode(uint8_t Mode) { +void QuickPID::SetMode(mode_t Mode) { bool newAuto = (Mode == AUTOMATIC); if (newAuto && !inAuto) { //we just went from manual to auto QuickPID::Initialize(); @@ -229,17 +225,10 @@ void QuickPID::Initialize() { /* SetControllerDirection(...)************************************************* The PID will either be connected to a DIRECT acting process (+Output leads - to +Input) or a REVERSE acting process(+Output leads to -Input.) We need to - know which one, because otherwise we may increase the output when we should - be decreasing. This is called from the constructor. + to +Input) or a REVERSE acting process(+Output leads to -Input.) ******************************************************************************/ -void QuickPID::SetControllerDirection(uint8_t Direction) { - if (inAuto && Direction != controllerDirection) { - kp = (0 - kp); - ki = (0 - ki); - kd = (0 - kd); - } - controllerDirection = Direction; +void QuickPID::SetControllerDirection(direction_t ControllerDirection) { + controllerDirection = ControllerDirection; } /* Status Functions************************************************************ @@ -265,10 +254,11 @@ float QuickPID::GetTu() { float QuickPID::GetTd() { return dispTd; } -uint8_t QuickPID::GetMode() { - return inAuto ? AUTOMATIC : MANUAL; +QuickPID::mode_t QuickPID::GetMode() { + return inAuto ? QuickPID::AUTOMATIC : QuickPID::MANUAL; } -uint8_t QuickPID::GetDirection() { + +QuickPID::direction_t QuickPID::GetDirection() { return controllerDirection; } diff --git a/QuickPID.h b/src/QuickPID.h similarity index 65% rename from QuickPID.h rename to src/QuickPID.h index 66dc03e..e6bf678 100644 --- a/QuickPID.h +++ b/src/QuickPID.h @@ -1,3 +1,4 @@ +#pragma once #ifndef QuickPID_h #define QuickPID_h @@ -5,55 +6,50 @@ class QuickPID { public: - //Constants and macros -#define AUTOMATIC 1 -#define MANUAL 0 -#define DIRECT 0 -#define REVERSE 1 + // enumerated types -static const byte TRY_DIRECT = 0; -static const byte TRY_AUTOMATIC = 1; + // controller mode + typedef enum { MANUAL = 0, AUTOMATIC = 1 } mode_t; -#define FL_FX(a) (int32_t)(a*256.0) // float to fixed point -#define FX_MUL(a,b) ((a*b)>>8) // fixed point multiply -#define CONSTRAIN(x,lower,upper) ((x)<(lower)?(lower):((x)>(upper)?(upper):(x))) + // DIRECT: intput increases when the error is positive. REVERSE: intput decreases when the error is positive. + typedef enum { DIRECT = 0, REVERSE = 1 } direction_t; // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(float*, float*, float*, float, float, float, float, uint8_t); + QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, float POn, direction_t ControllerDirection); // Overload constructor with proportional mode. Links the PID to Input, Output, Setpoint and Tuning Parameters. - QuickPID(float*, float*, float*, float, float, float, uint8_t); + QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, direction_t ControllerDirection); // Sets PID to either Manual (0) or Auto (non-0). - void SetMode(uint8_t Mode); + void SetMode(mode_t Mode); // Performs the PID calculation. It should be called every time loop() cycles. ON/OFF and calculation frequency // can be set using SetMode and SetSampleTime respectively. bool Compute(); // Automatically determines and sets the tuning parameters Kp, Ki and Kd. Use this in the setup loop. - void AutoTune(int, int, int, int, uint32_t); + void AutoTune(int inputPin, int outputPin, int tuningRule, int Print, uint32_t timeout); // Sets and clamps the output to a specific range (0-255 by default). - void SetOutputLimits(int, int); + void SetOutputLimits(int Min, int Max); // available but not commonly used functions ****************************************************************** // While most users will set the tunings once in the constructor, this function gives the user the option of // changing tunings during runtime for Adaptive control. - void SetTunings(float, float, float); + void SetTunings(float Kp, float Ki, float Kd); // Overload for specifying proportional mode. - void SetTunings(float, float, float, float); + void SetTunings(float Kp, float Ki, float Kd, float POn); - // Sets the Direction, or "Action" of control. DIRECT means the output will increase when error is positive. - // REVERSE means the opposite. It's very unlikely that this will be needed once it is set in the constructor. - void SetControllerDirection(uint8_t); + // Sets the controller Direction or Action. DIRECT means the output will increase when the error is positive. + // REVERSE means the output will decrease when the error is positive. + void SetControllerDirection(direction_t ControllerDirection); // Sets the sample time in milliseconds with which each PID calculation is performed. Default is 100. - void SetSampleTimeUs(uint32_t); + void SetSampleTimeUs(uint32_t NewSampleTimeUs); //Display functions ****************************************************************************************** float GetKp(); // These functions query the pid for interal values. They were created mainly for @@ -62,19 +58,17 @@ static const byte TRY_AUTOMATIC = 1; float GetKu(); // Ultimate Gain float GetTu(); // Ultimate Period float GetTd(); // Dead Time - uint8_t GetMode(); - uint8_t GetDirection(); + mode_t GetMode(); + direction_t GetDirection(); // Utility functions ****************************************************************************************** - int analogReadFast(int); - float analogReadAvg(int); + int analogReadFast(int ADCpin); + float analogReadAvg(int ADCpin); private: void Initialize(); - void CheckPeak(int); - void StepUp(int, int, uint32_t); - void StepDown(int, int, uint32_t); - void Stabilize(int, int, uint32_t); + void CheckPeak(int inputPin); + void Stabilize(int inputPin, int outputPin, uint32_t timeout); float dispKp; // tuning parameters for display purposes. float dispKi; @@ -90,17 +84,26 @@ static const byte TRY_AUTOMATIC = 1; float kpi; // proportional on error amount float kpd; // proportional on measurement amount - uint8_t controllerDirection; - float *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a float *myOutput; // hard link between the variables and the PID, freeing the user from having float *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. + direction_t controllerDirection; uint32_t sampleTimeUs, lastTime; int outMin, outMax, error; int lastInput, outputSum; bool inAuto; + inline int32_t FL_FX(float a) { + return (a * 256.0); // float to fixed point + } + inline int32_t FX_MUL(int32_t a, int32_t b) { + return ((a * b) >> 8); // fixed point multiply + } + inline int32_t CONSTRAIN(int32_t x, int32_t lower, int32_t upper) { + return ((x) < (lower) ? (lower) : ((x) > (upper) ? (upper) : (x))); + } + // AutoTune float peakHigh, peakLow; const word readPeriod = 1667; diff --git a/analogWrite.cpp b/src/analogWrite.cpp similarity index 100% rename from analogWrite.cpp rename to src/analogWrite.cpp diff --git a/analogWrite.h b/src/analogWrite.h similarity index 99% rename from analogWrite.h rename to src/analogWrite.h index e584060..34975bd 100644 --- a/analogWrite.h +++ b/src/analogWrite.h @@ -1,3 +1,4 @@ +#pragma once #ifndef _ESP32_ESP32S2_ANALOG_WRITE_ #define _ESP32_ESP32S2_ANALOG_WRITE_ From 42873bb2f246ee3e90f95db6e9c91d158cf7fcec Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 11 May 2021 12:58:17 -0400 Subject: [PATCH 035/101] Update AutoTune AutoTune function is now non-blocking, no timeouts are required, exists in a sketch and uses Input and Output variables directly. --- README.md | 6 +- .../AutoTune_RC_Filter/AutoTune_RC_Filter.ino | 207 ++++++++++++++---- keywords.txt | 7 +- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 130 +---------- src/QuickPID.h | 49 ++--- 7 files changed, 196 insertions(+), 207 deletions(-) diff --git a/README.md b/README.md index 3759100..103165c 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) function. This controller can automatically determine and set parameters (Kp, Ki, Kd). Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and controllability of the process are determined. There are 9 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. +QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) function. This controller can automatically determine and set parameters (Kp, Ki, Kd). Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and controllability of the process are determined. There are 9 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. ### About @@ -184,6 +184,10 @@ Use this link for reference. Note that if you're using QuickPID, there's no need #### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) +#### Version 2.2.8 + +- AutoTune is now independent of the QuickPID library and can be run from a sketch. AutoTune is now non-blocking, no timeouts are required and it uses Input and Output variables directly. See the example [AutoTune_RC_Filter.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino) for details. + #### Version 2.2.7 - Fixed REVERSE acting controller mode. diff --git a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino index f045e55..5ade2f4 100644 --- a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino +++ b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino @@ -22,59 +22,184 @@ const byte inputPin = 0; const byte outputPin = 3; -int Print = 0; // on(1) monitor, off(0) plotter -int tuningRule = 1; // see above table -float POn = 1.0; // mix of PonE to PonM (0.0-1.0) -unsigned long timeout = 120; // AutoTune timeout (sec) +int Print = 0; // on(1) monitor, off(0) plotter +int tuningRule = 1; // see above table +float POn = 1.0; // mix of PonE to PonM (0.0-1.0) +byte aTune = 0; // autoTune status, done = 10 float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; -// choose controller direction: -// DIRECT: Input increases when the output is increased or the error is positive (heating). -// REVERSE: Input decreases when the output is increased or when the error is positive (cooling). QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); -//QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::REVERSE); -void setup() -{ +void setup() { Serial.begin(115200); Serial.println(); +} - myQuickPID.AutoTune(inputPin, outputPin, tuningRule, Print, timeout); - myQuickPID.SetTunings(myQuickPID.GetKp(), myQuickPID.GetKi(), myQuickPID.GetKd()); - myQuickPID.SetSampleTimeUs(4000); // 4ms - myQuickPID.SetMode(QuickPID::AUTOMATIC); - Setpoint = 700; - - if (Print == 1) { - // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png - if (float(myQuickPID.GetTu() / myQuickPID.GetTd() + 0.0001) > 0.75) Serial.println("This process is easy to control."); - else if (float(myQuickPID.GetTu() / myQuickPID.GetTd() + 0.0001) > 0.25) Serial.println("This process has average controllability."); - else Serial.println("This process is difficult to control."); - Serial.print("Tu: "); Serial.print(myQuickPID.GetTu()); // Ultimate Period (sec) - Serial.print(" td: "); Serial.print(myQuickPID.GetTd()); // Dead Time (sec) - Serial.print(" Ku: "); Serial.print(myQuickPID.GetKu()); // Ultimate Gain - Serial.print(" Kp: "); Serial.print(myQuickPID.GetKp()); - Serial.print(" Ki: "); Serial.print(myQuickPID.GetKi()); - Serial.print(" Kd: "); Serial.println(myQuickPID.GetKd()); - Serial.println(); - delay(5000); //view results +void loop() { + Input = avg(myQuickPID.analogReadFast(inputPin)); + if (aTune < 10) autoTune(); + if (aTune == 9) { // apply new tunings + myQuickPID.SetTunings(Kp, Ki, Kd); + myQuickPID.SetMode(QuickPID::AUTOMATIC); + Setpoint = 700; + } + analogWrite(outputPin, Output); + if (aTune == 10) { // compute + if (Print == 0) { // serial plotter + Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); + Serial.print("Input:"); Serial.print(Input); Serial.print(","); + Serial.print("Output:"); Serial.print(Output); Serial.print(","); + Serial.println(","); + } + Input = avg(myQuickPID.analogReadFast(inputPin)); + myQuickPID.Compute();; + analogWrite(outputPin, Output); } } -void loop() -{ // plotter - Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); - Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.print(","); - Serial.println(","); +void autoTune() { + const int Rule[10][3] = + { //ckp, cki, ckd x 1000 + { 450, 540, 0 }, // ZIEGLER_NICHOLS_PI + { 600, 176, 75 }, // ZIEGLER_NICHOLS_PID + { 313, 142, 0 }, // TYREUS_LUYBEN_PI + { 454, 206, 72 }, // TYREUS_LUYBEN_PID + { 303, 1212, 0 }, // CIANCONE_MARLIN_PI + { 303, 1333, 37 }, // CIANCONE_MARLIN_PID + { 0, 0, 0 }, // AMIGOF_PID + { 700, 1750, 105 }, // PESSEN_INTEGRAL_PID + { 333, 667, 111 }, // SOME_OVERSHOOT_PID + { 200, 400, 67 }, // NO_OVERSHOOT_PID + }; + const byte ckp = 0, cki = 1, ckd = 2; //c = column + + const byte outputStep = 1; + const byte hysteresis = 1; + const int atSetpoint = 341; // 1/3 of 10-bit ADC matches 8-bit PWM value of 85 for symetrical waveform + const int atOutput = 85; - if (myQuickPID.GetDirection() == QuickPID::REVERSE) { - Input = (1023 - myQuickPID.analogReadFast(inputPin)); // simulate reverse acting controller (cooling) - } else { - Input = (myQuickPID.analogReadFast(inputPin)); + static uint32_t t0, t1, t2, t3; + static float Ku, Tu, td, kp, ki, kd, rdAvg, peakHigh, peakLow; + + switch (aTune) { + case 0: + peakHigh = atSetpoint; + peakLow = atSetpoint; + if (Print == 1) Serial.print("Stabilizing (33%) →"); + for (int i = 0; i < 16; i++) avg(Input); // initialize + Output = 0; + aTune++; + break; + case 1: // start coarse adjust + if (avg(Input) < (atSetpoint - hysteresis)) { + Output = atOutput + 20; + aTune++; + } + break; + case 2: // start fine adjust + if (avg(Input) > atSetpoint) { + Output = atOutput - outputStep; + aTune++; + } + break; + case 3: // run AutoTune + if (avg(Input) < atSetpoint) { + if (Print == 1) Serial.print(" Running AutoTune"); + Output = atOutput + outputStep; + aTune++; + } + break; + case 4: // get t0 + if (avg(Input) > atSetpoint) { + t0 = micros(); + if (Print == 1) Serial.print(" ↑"); + aTune++; + } + break; + case 5: // get t1 + if (avg(Input) > atSetpoint + 0.2) { + t1 = micros(); + aTune++; + } + break; + case 6: // get t2 + rdAvg = avg(Input); + if (rdAvg > peakHigh) peakHigh = rdAvg; + if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; + + if (rdAvg > atSetpoint + hysteresis) { + t2 = micros(); + if (Print == 1) Serial.print(" ↓"); + Output = atOutput - outputStep; + aTune++; + } + break; + case 7: // begin t3 + rdAvg = avg(Input); + if (rdAvg > peakHigh) peakHigh = rdAvg; + if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; + if (rdAvg < atSetpoint - hysteresis) { + if (Print == 1) Serial.print(" ↑"); + Output = atOutput + outputStep; + aTune++; + } + break; + case 8: // get t3 + rdAvg = avg(Input); + if (rdAvg > peakHigh) peakHigh = rdAvg; + if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; + if (rdAvg > atSetpoint + hysteresis) { + t3 = micros(); + if (Print == 1) Serial.println(" Done."); + aTune++; + td = (float)(t1 - t0) / 1000000.0; // dead time (seconds) + Tu = (float)(t3 - t2) / 1000000.0; // ultimate period (seconds) + Ku = (float)(4 * outputStep * 2) / (float)(3.14159 * sqrt (sq (peakHigh - peakLow) - sq (hysteresis))); // ultimate gain + if (tuningRule == 6) { //AMIGO_PID + (td < 10) ? td = 10 : td = td; // 10µs minimum + kp = (0.2 + 0.45 * (Tu / td)) / Ku; + float Ti = (((0.4 * td) + (0.8 * Tu)) / (td + (0.1 * Tu)) * td); + float Td = (0.5 * td * Tu) / ((0.3 * td) + Tu); + ki = kp / Ti; + kd = Td * kp; + } else { //other rules + kp = Rule[tuningRule][ckp] / 1000.0 * Ku; + ki = Rule[tuningRule][cki] / 1000.0 * Ku / Tu; + kd = Rule[tuningRule][ckd] / 1000.0 * Ku * Tu; + } + Kp = kp; + Ki = ki; + Kd = kd; + if (Print == 1) { + // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png + if ((Tu / td + 0.0001) > 0.75) Serial.println("This process is easy to control."); + else if ((Tu / td + 0.0001) > 0.25) Serial.println("This process has average controllability."); + else Serial.println("This process is difficult to control."); + Serial.print("Tu: "); Serial.print(Tu); // Ultimate Period (sec) + Serial.print(" td: "); Serial.print(td); // Dead Time (sec) + Serial.print(" Ku: "); Serial.print(Ku); // Ultimate Gain + Serial.print(" Kp: "); Serial.print(Kp); + Serial.print(" Ki: "); Serial.print(Ki); + Serial.print(" Kd: "); Serial.println(Kd); + Serial.println(); + } + } + break; + case 9: // ready to set tunings + aTune++; + break; } - myQuickPID.Compute(); - analogWrite(outputPin, Output); +} + +float avg(int inputVal) { + static int arrDat[16]; + static int pos; + static long sum; + pos++; + if (pos >= 16) pos = 0; + sum = sum - arrDat[pos] + inputVal; + arrDat[pos] = inputVal; + return (float)sum / 16.0; } diff --git a/keywords.txt b/keywords.txt index 157a12b..01aa409 100644 --- a/keywords.txt +++ b/keywords.txt @@ -7,9 +7,7 @@ ########################################## QuickPID KEYWORD1 -mode_t KEYWORD1 -direction_t KEYWORD1 -analog_write_channel_t KEYWORD1 +myQuickPID KEYWORD1 ########################################## # Methods and Functions (KEYWORD2) @@ -44,3 +42,6 @@ AUTOMATIC LITERAL1 MANUAL LITERAL1 DIRECT LITERAL1 REVERSE LITERAL1 +mode_t LITERAL1 +direction_t LITERAL1 +analog_write_channel_t LITERAL1 diff --git a/library.json b/library.json index ac3358f..5457652 100644 --- a/library.json +++ b/library.json @@ -3,7 +3,7 @@ "keywords": "PID, controller, signal", "description": "A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards.", "license": "MIT", - "version": "2.2.7", + "version": "2.2.8", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index d55dbca..f047789 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.2.7 +version=2.2.8 author=David Lloyd maintainer=David Lloyd sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 73b991f..894dbd8 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.2.7 + QuickPID Library for Arduino - Version 2.2.8 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library, licensed under the MIT License **********************************************************************************/ @@ -17,7 +17,8 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, float POn = 1, QuickPID::direction_t ControllerDirection = DIRECT) { + float Kp, float Ki, float Kd, float POn = 1, + QuickPID::direction_t ControllerDirection = DIRECT) { myOutput = Output; myInput = Input; @@ -72,82 +73,6 @@ bool QuickPID::Compute() { else return false; } -/* AutoTune(...)*************************************************************************** - This function uses the Relay Method to tune the loop without operator intervention. - It determines the ultimate gain (Ku) and ultimate period (Tu) which is used in the - Ziegler-Nichols tuning rules to compute Kp, Ki and Kd. - ******************************************************************************************/ -void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 120) { - - uint32_t t0, t1, t2, t3; - float Ku, Tu, td; - - const int Rule[10][3] = - { //ckp, cki, ckd x 1000 - { 450, 540, 0 }, // ZIEGLER_NICHOLS_PI - { 600, 1176, 75 }, // ZIEGLER_NICHOLS_PID - { 313, 142, 0 }, // TYREUS_LUYBEN_PI - { 454, 206, 72 }, // TYREUS_LUYBEN_PID - { 303, 1212, 0 }, // CIANCONE_MARLIN_PI - { 303, 1333, 37 }, // CIANCONE_MARLIN_PID - { 0, 0, 0 }, // AMIGOF_PID - { 700, 1750, 105 }, // PESSEN_INTEGRAL_PID - { 333, 667, 111 }, // SOME_OVERSHOOT_PID - { 200, 400, 67 }, // NO_OVERSHOOT_PID - }; - const byte ckp = 0, cki = 1, ckd = 2; //c = column - peakHigh = atSetpoint; - peakLow = atSetpoint; - timeout *= 1000; - - if (Print == 1) Serial.print("Stabilizing (33%) →"); - QuickPID::Stabilize(inputPin, outputPin, timeout); - if (Print == 1) Serial.print(" Running AutoTune ↑"); - t0 = micros(); - - analogWrite (outputPin, (atOutput + outputStep)); // step up output, wait for input to reach +'ve hysteresis - while ((analogReadAvg(inputPin) < (atSetpoint + 0.2)) && (millis() <= timeout)) QuickPID::CheckPeak(inputPin); - t1 = micros(); - - while ((analogReadAvg(inputPin) < (atSetpoint + hysteresis)) && (millis() <= timeout)) QuickPID::CheckPeak(inputPin); - t2 = micros(); - - if (Print == 1) Serial.print(" ↓"); - analogWrite (outputPin, (atOutput - outputStep)); // step down output, wait for input to reach -'ve hysteresis - while ((analogReadAvg(inputPin) > (atSetpoint - hysteresis)) && (millis() <= timeout)) QuickPID::CheckPeak(inputPin); - - if (Print == 1) Serial.print(" ↑"); - analogWrite (outputPin, (atOutput + outputStep)); // step up output, wait for input to reach +'ve hysteresis - while ((analogReadAvg(inputPin) < (atSetpoint + hysteresis)) && (millis() <= timeout)) QuickPID::CheckPeak(inputPin); - t3 = micros(); - if (Print == 1) Serial.println(" Done."); - - td = (float)(t1 - t0) / 1000000.0; // dead time (seconds) - dispTd = td; - - Tu = (float)(t3 - t2) / 1000000.0; // ultimate period (seconds) - dispTu = Tu; - - Ku = (float)(4 * outputStep * 2) / (float)(3.14159 * sqrt (sq (peakHigh - peakLow) - sq (hysteresis))); // ultimate gain - dispKu = Ku; - - if (tuningRule == 6) { //AMIGO_PID - (td < readPeriod) ? td = readPeriod : td = td; - kp = (0.2 + 0.45 * (Tu / td)) / Ku; - float Ti = (((0.4 * td) + (0.8 * Tu)) / (td + (0.1 * Tu)) * td); - float Td = (0.5 * td * Tu) / ((0.3 * td) + Tu); - ki = kp / Ti; - kd = Td * kp; - } else { //other rules - kp = Rule[tuningRule][ckp] / 1000.0 * Ku; - ki = Rule[tuningRule][cki] / 1000.0 * Ku / Tu; - kd = Rule[tuningRule][ckd] / 1000.0 * Ku * Tu; - } - dispKp = kp; - dispKi = ki; - dispKd = kd; -} - /* SetTunings(....)************************************************************ This function allows the controller's dynamic performance to be adjusted. it's called automatically from the constructor, but tunings can also @@ -245,15 +170,7 @@ float QuickPID::GetKi() { float QuickPID::GetKd() { return dispKd; } -float QuickPID::GetKu() { - return dispKu; -} -float QuickPID::GetTu() { - return dispTu; -} -float QuickPID::GetTd() { - return dispTd; -} + QuickPID::mode_t QuickPID::GetMode() { return inAuto ? QuickPID::AUTOMATIC : QuickPID::MANUAL; } @@ -262,7 +179,7 @@ QuickPID::direction_t QuickPID::GetDirection() { return controllerDirection; } -// Utility functions ********************************************************** +/* Other Functions************************************************************/ int QuickPID::analogReadFast(int ADCpin) { #if defined(__AVR_ATmega328P__) @@ -287,40 +204,3 @@ int QuickPID::analogReadFast(int ADCpin) { return analogRead(ADCpin); # endif } - -float QuickPID::analogReadAvg(int ADCpin) { - static int arrDat[16]; - static int dat; - static int pos; - static long sum; - dat = QuickPID::analogReadFast(ADCpin); - pos++; - if (pos >= 16) pos = 0; - sum = sum - arrDat[pos] + dat; - arrDat[pos] = dat; - return (float)sum / 16.0; -} - -void QuickPID::CheckPeak(int inputPin) { - float rdAvg = analogReadAvg(inputPin); - if (rdAvg > peakHigh) peakHigh = rdAvg; - if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; - delayMicroseconds(readPeriod); -} - -void QuickPID::Stabilize(int inputPin, int outputPin, uint32_t timeout) { - // initial reading - for (int i = 0; i <= 16; i++) analogReadAvg(inputPin); - // coarse adjust - analogWrite (outputPin, 0); - while ((analogReadAvg(inputPin) > (atSetpoint - hysteresis)) && (millis() <= timeout)); - analogWrite(outputPin, atOutput + 20); - while ((analogReadAvg(inputPin) < atSetpoint) && (millis() <= timeout)); - - // fine adjust - analogWrite (outputPin, atOutput - outputStep); - while ((analogReadAvg(inputPin) > atSetpoint) && (millis() <= timeout)); - analogWrite(outputPin, atOutput + outputStep); - while ((analogReadAvg(inputPin) < atSetpoint) && (millis() <= timeout)); - analogWrite(outputPin, atOutput); -} diff --git a/src/QuickPID.h b/src/QuickPID.h index e6bf678..7fec512 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -6,8 +6,6 @@ class QuickPID { public: - // enumerated types - // controller mode typedef enum { MANUAL = 0, AUTOMATIC = 1 } mode_t; @@ -48,45 +46,35 @@ class QuickPID { // REVERSE means the output will decrease when the error is positive. void SetControllerDirection(direction_t ControllerDirection); - // Sets the sample time in milliseconds with which each PID calculation is performed. Default is 100. + // Sets the sample time in microseconds with which each PID calculation is performed. Default is 100000 µs. void SetSampleTimeUs(uint32_t NewSampleTimeUs); //Display functions ****************************************************************************************** float GetKp(); // These functions query the pid for interal values. They were created mainly for float GetKi(); // the pid front-end, where it's important to know what is actually inside the PID. float GetKd(); - float GetKu(); // Ultimate Gain - float GetTu(); // Ultimate Period - float GetTd(); // Dead Time mode_t GetMode(); direction_t GetDirection(); - - // Utility functions ****************************************************************************************** int analogReadFast(int ADCpin); - float analogReadAvg(int ADCpin); private: + void Initialize(); - void CheckPeak(int inputPin); - void Stabilize(int inputPin, int outputPin, uint32_t timeout); - float dispKp; // tuning parameters for display purposes. + float dispKp; // tuning parameters for display purposes. float dispKi; float dispKd; - float dispKu; - float dispTu; - float dispTd; - float pOn; // proportional mode (0-1) default = 1, 100% Proportional on Error - float kp; // (P)roportional Tuning Parameter - float ki; // (I)ntegral Tuning Parameter - float kd; // (D)erivative Tuning Parameter - float kpi; // proportional on error amount - float kpd; // proportional on measurement amount + float pOn; // proportional mode (0-1) default = 1 (100% Proportional on Error) + float kp; // (P)roportional Tuning Parameter + float ki; // (I)ntegral Tuning Parameter + float kd; // (D)erivative Tuning Parameter + float kpi; // proportional on error amount + float kpd; // proportional on measurement amount - float *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a - float *myOutput; // hard link between the variables and the PID, freeing the user from having - float *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. + float *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a + float *myOutput; // hard link between the variables and the PID, freeing the user from having + float *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. direction_t controllerDirection; uint32_t sampleTimeUs, lastTime; @@ -98,24 +86,15 @@ class QuickPID { return (a * 256.0); // float to fixed point } inline int32_t FX_MUL(int32_t a, int32_t b) { - return ((a * b) >> 8); // fixed point multiply + return ((a * b) >> 8); // fixed point multiply } inline int32_t CONSTRAIN(int32_t x, int32_t lower, int32_t upper) { return ((x) < (lower) ? (lower) : ((x) > (upper) ? (upper) : (x))); } - // AutoTune - float peakHigh, peakLow; - const word readPeriod = 1667; - const byte outputStep = 1; - const byte hysteresis = 1; - const int atSetpoint = 341; // 1/3 of 10-bit ADC matches 8-bit PWM value of 85 for symetrical waveform - const int atOutput = 85; - -}; +}; // class QuickPID #if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) #include "analogWrite.h" #endif - #endif // QuickPID.h From eccafaa7e861ab2f41314cb0b7c194b219ab768a Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 15 May 2021 15:39:15 -0400 Subject: [PATCH 036/101] Update1 --- .../AutoTune_Filter_DIRECT.ino | 76 +++++++ .../AutoTune_Filter_REVERSE.ino | 77 +++++++ .../AutoTune_RC_Filter/AutoTune_RC_Filter.ino | 205 ------------------ src/QuickPID.cpp | 182 +++++++++++++++- src/QuickPID.h | 54 ++++- 5 files changed, 383 insertions(+), 211 deletions(-) create mode 100644 examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino create mode 100644 examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino delete mode 100644 examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino new file mode 100644 index 0000000..489c7f2 --- /dev/null +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -0,0 +1,76 @@ +/****************************************************************************** + AutoTune Filter DIRECT Example + Reference: https://github.com/Dlloydev/QuickPID/wiki/AutoTune + Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter + ******************************************************************************/ + +#include "QuickPID.h" + +const byte inputPin = 0; +const byte outputPin = 3; + +const int outputMax = 255; +const int outputMin = 0; + +float POn = 1.0; // mix of PonE to PonM (0.0-1.0) + +bool printOrPlotter = 0; // on(1) monitor, off(0) plotter +byte tuningRule = 1; // see reference link +byte outputStep = 1; +byte hysteresis = 1; +int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform +int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform + +float Input, Output, Setpoint; +float Kp = 0, Ki = 0, Kd = 0; + +QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); + +void setup() { + Serial.begin(115200); + Serial.println(); + if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { + Serial.println(F("AutoTune test exceeds outMax limit, check output, hysteresis and outputStep values")); + while (1); + } + _myPID.AutoTune(tuningRule); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, 85, printOrPlotter); +} + +void loop() { + + if (_myPID.autoTune->autoTuneLoop() != _myPID.autoTune->RUN_PID) { // running autotune + Input = avg(_myPID.analogReadFast(inputPin)); // filtered input + analogWrite(outputPin, Output); + } + + if (_myPID.autoTune->autoTuneLoop() == _myPID.autoTune->NEW_TUNINGS) { // get new tunings + _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings + _myPID.clearAutoTune(); // releases memory used by AutoTune object + _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID + _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + Setpoint = 500; + } + + if (_myPID.autoTune->autoTuneLoop() == _myPID.autoTune->RUN_PID) { // running PID + if (printOrPlotter == 0) { // plotter + Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); + Serial.print("Input:"); Serial.print(Input); Serial.print(","); + Serial.print("Output:"); Serial.print(Output); Serial.println(); + } + Input = _myPID.analogReadFast(inputPin); + _myPID.Compute(); + analogWrite(outputPin, Output); + } +} + +float avg(int inputVal) { + static int arrDat[16]; + static int pos; + static long sum; + pos++; + if (pos >= 16) pos = 0; + sum = sum - arrDat[pos] + inputVal; + arrDat[pos] = inputVal; + return (float)sum / 16.0; +} diff --git a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino new file mode 100644 index 0000000..ff9ba7c --- /dev/null +++ b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino @@ -0,0 +1,77 @@ +/****************************************************************************** + AutoTune Filter REVERSE Example + Reference: https://github.com/Dlloydev/QuickPID/wiki/AutoTune + Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter + ******************************************************************************/ + +#include "QuickPID.h" + +const byte inputPin = 0; +const byte outputPin = 3; + +const int inputMax = 1023; +const int outputMax = 255; +const int outputMin = 0; + +float POn = 1.0; // mix of PonE to PonM (0.0-1.0) + +bool printOrPlotter = 0; // on(1) monitor, off(0) plotter +byte tuningRule = 1; // see reference link +byte outputStep = 1; +byte hysteresis = 1; +int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform +int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform + +float Input, Output, Setpoint; +float Kp = 0, Ki = 0, Kd = 0; + +QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::REVERSE); + +void setup() { + Serial.begin(115200); + Serial.println(); + if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { + Serial.println(F("AutoTune test exceeds outMax limit, check output, hysteresis and outputStep values")); + while (1); + } + _myPID.AutoTune(tuningRule); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, inputMax - setpoint, output, QuickPID::REVERSE, printOrPlotter); +} + +void loop() { + + if (_myPID.autoTune->autoTuneLoop() != _myPID.autoTune->RUN_PID) { // running autotune + Input = inputMax - avg(_myPID.analogReadFast(inputPin)); // filtered input (reverse acting) + analogWrite(outputPin, Output); + } + + if (_myPID.autoTune->autoTuneLoop() == _myPID.autoTune->NEW_TUNINGS) { // get new tunings + _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings + _myPID.clearAutoTune(); // releases memory used by AutoTune object + _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID + _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + Setpoint = 500; + } + + if (_myPID.autoTune->autoTuneLoop() == _myPID.autoTune->RUN_PID) { // running PID + if (printOrPlotter == 0) { // plotter + Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); + Serial.print("Input:"); Serial.print(Input); Serial.print(","); + Serial.print("Output:"); Serial.print(Output); Serial.println(); + } + Input = inputMax - _myPID.analogReadFast(inputPin); // reverse acting + _myPID.Compute(); + analogWrite(outputPin, Output); + } +} + +float avg(int inputVal) { + static int arrDat[16]; + static int pos; + static long sum; + pos++; + if (pos >= 16) pos = 0; + sum = sum - arrDat[pos] + inputVal; + arrDat[pos] = inputVal; + return (float)sum / 16.0; +} diff --git a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino b/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino deleted file mode 100644 index 5ade2f4..0000000 --- a/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino +++ /dev/null @@ -1,205 +0,0 @@ -/****************************************************************************** - AutoTune RC Filter Example - Use Serial Monitor and Serial Plotter to view results. - Reference: https://github.com/Dlloydev/QuickPID/wiki/AutoTune - Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter - - TUNING RULE RECOMMENED FOR - 0 ZIEGLER_NICHOLS_PI Good noise and disturbance rejection - 1 ZIEGLER_NICHOLS_PID Good noise and disturbance rejection - 2 TYREUS_LUYBEN_PI Time-constant (lag) dominant processes (conservative) - 3 TYREUS_LUYBEN_PID Time-constant (lag) dominant processes (conservative) - 4 CIANCONE_MARLIN_PI Delay (dead-time) dominant processes - 5 CIANCONE_MARLIN_PID Delay (dead-time) dominant processes - 6 AMIGO_PID More universal than ZN_PID (uses a dead time dependancy) - 7 PESSEN_INTEGRAL_PID Similar to ZN_PID but with better dynamic response - 8 SOME_OVERSHOOT_PID ZN_PID with lower proportional and integral gain - 9 NO_OVERSHOOT_PID ZN_PID with much lower P,I,D gain settings - ******************************************************************************/ - -#include "QuickPID.h" - -const byte inputPin = 0; -const byte outputPin = 3; - -int Print = 0; // on(1) monitor, off(0) plotter -int tuningRule = 1; // see above table -float POn = 1.0; // mix of PonE to PonM (0.0-1.0) -byte aTune = 0; // autoTune status, done = 10 - -float Input, Output, Setpoint; -float Kp = 0, Ki = 0, Kd = 0; - -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); - -void setup() { - Serial.begin(115200); - Serial.println(); -} - -void loop() { - Input = avg(myQuickPID.analogReadFast(inputPin)); - if (aTune < 10) autoTune(); - if (aTune == 9) { // apply new tunings - myQuickPID.SetTunings(Kp, Ki, Kd); - myQuickPID.SetMode(QuickPID::AUTOMATIC); - Setpoint = 700; - } - analogWrite(outputPin, Output); - if (aTune == 10) { // compute - if (Print == 0) { // serial plotter - Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); - Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.print(","); - Serial.println(","); - } - Input = avg(myQuickPID.analogReadFast(inputPin)); - myQuickPID.Compute();; - analogWrite(outputPin, Output); - } -} - -void autoTune() { - const int Rule[10][3] = - { //ckp, cki, ckd x 1000 - { 450, 540, 0 }, // ZIEGLER_NICHOLS_PI - { 600, 176, 75 }, // ZIEGLER_NICHOLS_PID - { 313, 142, 0 }, // TYREUS_LUYBEN_PI - { 454, 206, 72 }, // TYREUS_LUYBEN_PID - { 303, 1212, 0 }, // CIANCONE_MARLIN_PI - { 303, 1333, 37 }, // CIANCONE_MARLIN_PID - { 0, 0, 0 }, // AMIGOF_PID - { 700, 1750, 105 }, // PESSEN_INTEGRAL_PID - { 333, 667, 111 }, // SOME_OVERSHOOT_PID - { 200, 400, 67 }, // NO_OVERSHOOT_PID - }; - const byte ckp = 0, cki = 1, ckd = 2; //c = column - - const byte outputStep = 1; - const byte hysteresis = 1; - const int atSetpoint = 341; // 1/3 of 10-bit ADC matches 8-bit PWM value of 85 for symetrical waveform - const int atOutput = 85; - - static uint32_t t0, t1, t2, t3; - static float Ku, Tu, td, kp, ki, kd, rdAvg, peakHigh, peakLow; - - switch (aTune) { - case 0: - peakHigh = atSetpoint; - peakLow = atSetpoint; - if (Print == 1) Serial.print("Stabilizing (33%) →"); - for (int i = 0; i < 16; i++) avg(Input); // initialize - Output = 0; - aTune++; - break; - case 1: // start coarse adjust - if (avg(Input) < (atSetpoint - hysteresis)) { - Output = atOutput + 20; - aTune++; - } - break; - case 2: // start fine adjust - if (avg(Input) > atSetpoint) { - Output = atOutput - outputStep; - aTune++; - } - break; - case 3: // run AutoTune - if (avg(Input) < atSetpoint) { - if (Print == 1) Serial.print(" Running AutoTune"); - Output = atOutput + outputStep; - aTune++; - } - break; - case 4: // get t0 - if (avg(Input) > atSetpoint) { - t0 = micros(); - if (Print == 1) Serial.print(" ↑"); - aTune++; - } - break; - case 5: // get t1 - if (avg(Input) > atSetpoint + 0.2) { - t1 = micros(); - aTune++; - } - break; - case 6: // get t2 - rdAvg = avg(Input); - if (rdAvg > peakHigh) peakHigh = rdAvg; - if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; - - if (rdAvg > atSetpoint + hysteresis) { - t2 = micros(); - if (Print == 1) Serial.print(" ↓"); - Output = atOutput - outputStep; - aTune++; - } - break; - case 7: // begin t3 - rdAvg = avg(Input); - if (rdAvg > peakHigh) peakHigh = rdAvg; - if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; - if (rdAvg < atSetpoint - hysteresis) { - if (Print == 1) Serial.print(" ↑"); - Output = atOutput + outputStep; - aTune++; - } - break; - case 8: // get t3 - rdAvg = avg(Input); - if (rdAvg > peakHigh) peakHigh = rdAvg; - if ((rdAvg < peakLow) && (peakHigh >= (atSetpoint + hysteresis))) peakLow = rdAvg; - if (rdAvg > atSetpoint + hysteresis) { - t3 = micros(); - if (Print == 1) Serial.println(" Done."); - aTune++; - td = (float)(t1 - t0) / 1000000.0; // dead time (seconds) - Tu = (float)(t3 - t2) / 1000000.0; // ultimate period (seconds) - Ku = (float)(4 * outputStep * 2) / (float)(3.14159 * sqrt (sq (peakHigh - peakLow) - sq (hysteresis))); // ultimate gain - if (tuningRule == 6) { //AMIGO_PID - (td < 10) ? td = 10 : td = td; // 10µs minimum - kp = (0.2 + 0.45 * (Tu / td)) / Ku; - float Ti = (((0.4 * td) + (0.8 * Tu)) / (td + (0.1 * Tu)) * td); - float Td = (0.5 * td * Tu) / ((0.3 * td) + Tu); - ki = kp / Ti; - kd = Td * kp; - } else { //other rules - kp = Rule[tuningRule][ckp] / 1000.0 * Ku; - ki = Rule[tuningRule][cki] / 1000.0 * Ku / Tu; - kd = Rule[tuningRule][ckd] / 1000.0 * Ku * Tu; - } - Kp = kp; - Ki = ki; - Kd = kd; - if (Print == 1) { - // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png - if ((Tu / td + 0.0001) > 0.75) Serial.println("This process is easy to control."); - else if ((Tu / td + 0.0001) > 0.25) Serial.println("This process has average controllability."); - else Serial.println("This process is difficult to control."); - Serial.print("Tu: "); Serial.print(Tu); // Ultimate Period (sec) - Serial.print(" td: "); Serial.print(td); // Dead Time (sec) - Serial.print(" Ku: "); Serial.print(Ku); // Ultimate Gain - Serial.print(" Kp: "); Serial.print(Kp); - Serial.print(" Ki: "); Serial.print(Ki); - Serial.print(" Kd: "); Serial.println(Kd); - Serial.println(); - } - } - break; - case 9: // ready to set tunings - aTune++; - break; - } -} - -float avg(int inputVal) { - static int arrDat[16]; - static int pos; - static long sum; - pos++; - if (pos >= 16) pos = 0; - sum = sum - arrDat[pos] + inputVal; - arrDat[pos] = inputVal; - return (float)sum / 16.0; -} diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 894dbd8..68007b8 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -53,15 +53,15 @@ bool QuickPID::Compute() { uint32_t timeChange = (now - lastTime); if (timeChange >= sampleTimeUs) { // Compute the working error variables - int input = *myInput; - int dInput = input - lastInput; + float input = *myInput; + float dInput = input - lastInput; error = *mySetpoint - input; if (controllerDirection == REVERSE) { error = -error; dInput = -dInput; } - if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); // fixed point - else outputSum += (kpi * error) - (kpd * dInput); // floating-point + if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), (int)dInput); // fixed point + else outputSum += (kpi * error) - (kpd * dInput); // floating-point outputSum = CONSTRAIN(outputSum, outMin, outMax); *myOutput = outputSum; @@ -204,3 +204,177 @@ int QuickPID::analogReadFast(int ADCpin) { return analogRead(ADCpin); # endif } + +void QuickPID::AutoTune(uint8_t tuningRule) +{ + autoTune = new AutoTunePID(myInput, myOutput, (uint8_t)tuningRule); +} + +void QuickPID::clearAutoTune() +{ + if (autoTune) + delete autoTune; +} + +AutoTunePID::AutoTunePID() +{ + _input = nullptr; + _output = nullptr; + reset(); +} + +AutoTunePID::AutoTunePID(float *input, float *output, uint8_t tuningRule) +{ + AutoTunePID(); + _input = input; + _output = output; + _tuningRule = tuningRule; +} + +void AutoTunePID::reset() +{ + _t0 = 0; + _t1 = 0; + _t2 = 0; + _t3 = 0; + _Ku = 0.0; + _Tu = 0.0; + _td = 0.0; + _kp = 0.0; + _ki = 0.0; + _kd = 0.0; + _rdAvg = 0.0; + _peakHigh = 0.0; + _peakLow = 0.0; +} + +void AutoTunePID::autoTuneConfig(const byte outputStep, const byte hysteresis, const int atSetpoint, const int atOutput, const bool dir, const bool printOrPlotter) +{ + _outputStep = outputStep; + _hysteresis = hysteresis; + _atSetpoint = atSetpoint; + _atOutput = atOutput; + _direction = dir; + _printOrPlotter = printOrPlotter; +} + +byte AutoTunePID::autoTuneLoop() +{ + switch (_autoTuneStage) { + case STABILIZING: + _peakHigh = _atSetpoint; + _peakLow = _atSetpoint; + if (_printOrPlotter == 1) Serial.print(F("Stabilizing →")); + *_output = 0; + _autoTuneStage++; + break; + case COARSE: // coarse adjust + if (*_input < (_atSetpoint - _hysteresis)) { + if (_printOrPlotter == 1) Serial.print(F(" coarse →")); + (!_direction) ? *_output = _atOutput + _outputStep + 5 : *_output = _atOutput - _outputStep - 5; + _autoTuneStage++; + } + break; + case FINE: // fine adjust + if (*_input > _atSetpoint) { + if (_printOrPlotter == 1) Serial.print(F(" fine →")); + (!_direction) ? *_output = _atOutput - _outputStep : *_output = _atOutput + _outputStep; + _autoTuneStage++; + } + break; + case AUTOTUNE: // run AutoTune relay method + if (*_input < _atSetpoint) { + if (_printOrPlotter == 1) Serial.print(F(" autotune →")); + (!_direction) ? *_output = _atOutput + _outputStep : *_output = _atOutput - _outputStep; + _autoTuneStage++; + } + break; + case T0: // get t0 + if (*_input > _atSetpoint) { + _t0 = micros(); + if (_printOrPlotter == 1) Serial.print(F(" t0 →")); + _autoTuneStage++; + } + break; + case T1: // get t1 + if (*_input > _atSetpoint + 0.2) { + _t1 = micros(); + if (_printOrPlotter == 1) Serial.print(F(" t1 →")); + _autoTuneStage++; + } + break; + case T2: // get t2 + _rdAvg = *_input; + if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; + if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; + + if (_rdAvg > _atSetpoint + _hysteresis) { + _t2 = micros(); + if (_printOrPlotter == 1) Serial.print(F(" t2 →")); + (!_direction) ? *_output = _atOutput - _outputStep : *_output = _atOutput + _outputStep; + _autoTuneStage++; + } + break; + case T3: // get t3 + _rdAvg = *_input; + if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; + if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; + if (_rdAvg < _atSetpoint - _hysteresis) { + if (_printOrPlotter == 1) Serial.print(F(" t3 →")); + (!_direction) ? *_output = _atOutput + _outputStep : *_output = _atOutput - _outputStep; + _autoTuneStage++; + } + break; + case DONE: // calculate everything + _rdAvg = *_input; + if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; + if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; + if (_rdAvg > _atSetpoint + _hysteresis) { + _t3 = micros(); + if (_printOrPlotter == 1) Serial.println(F(" done.")); + _autoTuneStage++; + _td = (float)(_t1 - _t0) / 1000000.0; // dead time (seconds) + _Tu = (float)(_t3 - _t2) / 1000000.0; // ultimate period (seconds) + _Ku = (float)(4 * _outputStep * 2) / (float)(3.14159 * sqrt (sq (_peakHigh - _peakLow) - sq (_hysteresis))); // ultimate gain + if (_tuningRule == 6) { //AMIGO_PID + (_td < 100) ? _td = 100 : _td = _td; // 100µs minimum + _kp = (0.2 + 0.45 * (_Tu / _td)) / _Ku; + float Ti = (((0.4 * _td) + (0.8 * _Tu)) / (_td + (0.1 * _Tu)) * _td); + float Td = (0.5 * _td * _Tu) / ((0.3 * _td) + _Tu); + _ki = _kp / Ti; + _kd = Td * _kp; + } else { //other rules + _kp = RulesContants[_tuningRule][0] / 1000.0 * _Ku; + _ki = RulesContants[_tuningRule][1] / 1000.0 * _Ku / _Tu; + _kd = RulesContants[_tuningRule][2] / 1000.0 * _Ku * _Tu; + } + if (_printOrPlotter == 1) { + // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png + if ((_Tu / _td + 0.0001) > 0.75) Serial.println(F("This process is easy to control.")); + else if ((_Tu / _td + 0.0001) > 0.25) Serial.println(F("This process has average controllability.")); + else Serial.println(F("This process is difficult to control.")); + Serial.print(F("Tu: ")); Serial.print(_Tu); // Ultimate Period (sec) + Serial.print(F(" td: ")); Serial.print(_td); // Dead Time (sec) + Serial.print(F(" Ku: ")); Serial.print(_Ku); // Ultimate Gain + Serial.print(F(" Kp: ")); Serial.print(_kp); + Serial.print(F(" Ki: ")); Serial.print(_ki); + Serial.print(F(" Kd: ")); Serial.println(_kd); + Serial.println(); + } + } + break; + case NEW_TUNINGS: // ready to apply tunings + *_output = 0; + _autoTuneStage++; + return NEW_TUNINGS; + break; + } + return RUN_PID; +} + +void AutoTunePID::setAutoTuneConstants(float* kp, float* ki, float* kd) +{ + *kp = _kp; + *ki = _ki; + *kd = _kd; +} diff --git a/src/QuickPID.h b/src/QuickPID.h index 7fec512..0aade92 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -2,6 +2,51 @@ #ifndef QuickPID_h #define QuickPID_h +class AutoTunePID { + public: + AutoTunePID(); + AutoTunePID(float *input, float *output, uint8_t tuningRule); + ~AutoTunePID() {}; + + void reset(); + void autoTuneConfig(const byte outputStep, const byte hysteresis, const int setpoint, const int output, + const bool dir = 0, const bool printOrPlotter = false); + byte autoTuneLoop(); + void setAutoTuneConstants(float* kp, float* ki, float* kd); + enum atStage : byte { STABILIZING, COARSE, FINE, AUTOTUNE, T0, T1, T2, T3, DONE, NEW_TUNINGS, RUN_PID }; + + private: + float *_input = nullptr; // Pointers to the Input, Output, and Setpoint variables. This creates a + float *_output = nullptr; // hard link between the variables and the PID, freeing the user from having + + byte _autoTuneStage = 0; + byte _tuningRule = 0; + byte _outputStep; + byte _hysteresis; + int _atSetpoint; // 1/3 of 10-bit ADC range for symetrical waveform + int _atOutput; + bool _direction = false; + bool _printOrPlotter = false; + + uint32_t _t0, _t1, _t2, _t3; + float _Ku, _Tu, _td, _kp, _ki, _kd, _rdAvg, _peakHigh, _peakLow; + + const uint16_t RulesContants[10][3] = + { //ckp, cki, ckd x 1000 + { 450, 540, 0 }, // ZIEGLER_NICHOLS_PI + { 600, 176, 75 }, // ZIEGLER_NICHOLS_PID + { 313, 142, 0 }, // TYREUS_LUYBEN_PI + { 454, 206, 72 }, // TYREUS_LUYBEN_PID + { 303, 1212, 0 }, // CIANCONE_MARLIN_PI + { 303, 1333, 37 }, // CIANCONE_MARLIN_PID + { 0, 0, 0 }, // AMIGOF_PID + { 700, 1750, 105 }, // PESSEN_INTEGRAL_PID + { 333, 667, 111 }, // SOME_OVERSHOOT_PID + { 200, 400, 67 }, // NO_OVERSHOOT_PID + }; + +}; // class AutoTunePID + class QuickPID { public: @@ -28,7 +73,9 @@ class QuickPID { bool Compute(); // Automatically determines and sets the tuning parameters Kp, Ki and Kd. Use this in the setup loop. - void AutoTune(int inputPin, int outputPin, int tuningRule, int Print, uint32_t timeout); + // void AutoTune(int inputPin, int outputPin, int tuningRule, int Print, uint32_t timeout); + void AutoTune(uint8_t tuningRule); + void clearAutoTune(); // Sets and clamps the output to a specific range (0-255 by default). void SetOutputLimits(int Min, int Max); @@ -57,6 +104,8 @@ class QuickPID { direction_t GetDirection(); int analogReadFast(int ADCpin); + AutoTunePID *autoTune; + private: void Initialize(); @@ -79,7 +128,8 @@ class QuickPID { direction_t controllerDirection; uint32_t sampleTimeUs, lastTime; int outMin, outMax, error; - int lastInput, outputSum; + int outputSum; + float lastInput; bool inAuto; inline int32_t FL_FX(float a) { From f5c92c5110c129d7539ed3f760485c21ba592907 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 15 May 2021 16:32:21 -0400 Subject: [PATCH 037/101] Update --- examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino | 2 +- src/QuickPID.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index 489c7f2..5ccf9bc 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -34,7 +34,7 @@ void setup() { while (1); } _myPID.AutoTune(tuningRule); - _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, 85, printOrPlotter); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, printOrPlotter); } void loop() { diff --git a/src/QuickPID.h b/src/QuickPID.h index 0aade92..676f3bb 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -10,7 +10,7 @@ class AutoTunePID { void reset(); void autoTuneConfig(const byte outputStep, const byte hysteresis, const int setpoint, const int output, - const bool dir = 0, const bool printOrPlotter = false); + const bool dir = false, const bool printOrPlotter = false); byte autoTuneLoop(); void setAutoTuneConstants(float* kp, float* ki, float* kd); enum atStage : byte { STABILIZING, COARSE, FINE, AUTOTUNE, T0, T1, T2, T3, DONE, NEW_TUNINGS, RUN_PID }; From f846c614a5f7529eb2f7d88a19ab4c04d9a3799a Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 16 May 2021 00:28:15 -0400 Subject: [PATCH 038/101] Update --- README.md | 54 ++++++------------- .../AutoTune_Filter_DIRECT.ino | 6 ++- .../AutoTune_Filter_REVERSE.ino | 2 +- library.json | 4 +- library.properties | 4 +- src/QuickPID.cpp | 7 +-- 6 files changed, 30 insertions(+), 47 deletions(-) diff --git a/README.md b/README.md index 103165c..48fd380 100644 --- a/README.md +++ b/README.md @@ -1,36 +1,30 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) function. This controller can automatically determine and set parameters (Kp, Ki, Kd). Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and controllability of the process are determined. There are 9 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. +QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used. This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. ### About -This PID controller provides a faster *read-compute-write* cycle than alternatives as it has a more efficient and reduced algorithm that avoids time consuming partial calculations, it takes advantage of fixed point math and has a faster analog read function. The `Ki` and `Kd` are scaled by time (µs) and the new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required in `Compute()`. +This PID controller provides a shortened *read-compute-write* cycle by using a reduced PID algorithm, taking advantage of fixed point math and using a fast analog read function. The `Ki` and `Kd` are scaled by time (µs) and the new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function resulting in only two multiply operations required in `Compute()`. ### Features Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the change log and below: -- Quicker hybrid fixed/floating point math in compute function -- Built-in `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. and also ultimate gain `Ku` and ultimate period `Tu` of the control system. There are 9 tuning rules to choose from -- [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) uses a precise and low control effort sequence that gets results quickly. It also determines how difficult the process is to control. -- `POn` parameter controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement -- Reorganized and more efficient PID algorithm, faster analog read function, micros() timing resolution -- Runs a complete PID cycle (*read-compute-write*) faster than just an `analogRead()` command in Arduino -- Includes a complete [analogWrite function for ESP32](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) boards. This controls up to 8 independent PWM pins and 2 DAC pins. +- Quicker hybrid fixed/floating point math, efficient PID algorithm and micros() timing resolution. +- Built-in `AutoTunePID` class as a dynamic object. [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) automatically determines and applies `Kp`, `Ki` and `Kd` tuning parameters and has 10 tuning rules to choose from. +- Variable `POn` parameter controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement. +- Includes [analogWrite](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) for ESP32 boards. -### Performance +### Performance (16MHz ATmega328P) -| `Compute()` | Kp | Ki | Kd | Step Time (µS) | -| :----------------------------------------------------------- | ---- | ---- | ---- | -------------- | -| QuickPID | 2.0 | 15.0 | 0.05 | 72 | -| Arduino PID | 2.0 | 15.0 | 0.05 | 104 | -| **Full PID cycle:** **`analogRead(), Compute(), analogWrite()`** | | | | | -| QuickPID using `analogReadFast()` | 2.0 | 5.0 | 0.2 | 96 | -| Arduino PID using `analogRead()` | 2.0 | 5.0 | 0.2 | 224 | +| `Compute()` | Kp | Ki | Kd | Step Time (µS) | +| :---------- | ---- | ---- | ---- | -------------- | +| QuickPID | 2.0 | 5.0 | 1.0 | 72 | +| Arduino PID | 2.0 | 5.0 | 1.0 | 104 | ### [AutoTune RC Filter](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) -This example allows you to experiment with the AutoTune, various tuning rules and the POn control on an RC filter. It automatically determines and sets the tuning parameters. +This example allows you to experiment with the AutoTunePID class, various tuning rules and the POn control using ADC and PWM with RC filter. It automatically determines and sets the tuning parameters and works with both DIRECT and REVERSE acting controllers. #### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) @@ -40,16 +34,16 @@ This example allows you to experiment with the AutoTune, various tuning rules an outputSum += (kpi * error) - (kpd * dInput); ``` -The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required. The pOn variable controls the setpoint weighting of Proportional on Error and Proportional on Measurement. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated as follows: +The `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated as follows: ```c++ kpi = kp * pOn + ki; kpd = kp * (1 - pOn) + kd; ``` -### Controller Action +### Direct and Reverse Controller Action -If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). Since the PWM and ADC peripherals on microcontrollers only operate with positive values, QuickPID only uses positive values for Input, Output and Setpoint. When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). See the example [AutoTune_RC_Filter.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino) for details. +If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). See the examples `AutoTune_Filter_DIRECT.ino` and `AutoTune_Filter_REVERSE.ino` for details. ### Functions @@ -64,7 +58,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. - `POn` is the Proportional on Error weighting value (0.0-1.0). This controls the mix of Proportional on Error (PonE) and Proportional on Measurement (PonM) that's used in the compute algorithm. Note that POn controls the PonE amount, where the remainder (1-PonE) is the PonM amount. Also, the default POn is 1 -POn +![image](https://user-images.githubusercontent.com/63488701/118383726-986b6e80-b5ce-11eb-94b8-fdbddd4c914e.png) - `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. DIRECT is most common. @@ -85,19 +79,7 @@ This function contains the PID algorithm and it should be called once every loop #### [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) -```c++ -void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 30); -``` - -The `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. It also determines the critical gain `Ku` and critical period `Tu` of the control system. - -`int Print = 0; // on(1), off(0)` - -When using Serial Monitor, turn on serial print output to view the AutoTune status and results. When using the Serial Plotter, turn off the AutoTune print output to prevent plot labels from being overwritten. - -`uint32_t timeout = 120` - -Sets the AutoTune timeout where the default is 120 seconds. +For use of AutoTune, refer to the examples `AutoTune_Filter_DIRECT.ino` and `AutoTune_Filter_REVERSE.ino` #### SetTunings @@ -182,8 +164,6 @@ Use this link for reference. Note that if you're using QuickPID, there's no need #### Change Log -#### [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) - #### Version 2.2.8 - AutoTune is now independent of the QuickPID library and can be run from a sketch. AutoTune is now non-blocking, no timeouts are required and it uses Input and Output variables directly. See the example [AutoTune_RC_Filter.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino) for details. diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index 5ccf9bc..6fa8d2a 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -30,16 +30,18 @@ void setup() { Serial.begin(115200); Serial.println(); if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { - Serial.println(F("AutoTune test exceeds outMax limit, check output, hysteresis and outputStep values")); + Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); while (1); } _myPID.AutoTune(tuningRule); - _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, printOrPlotter); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter); } void loop() { if (_myPID.autoTune->autoTuneLoop() != _myPID.autoTune->RUN_PID) { // running autotune + Serial.println(F("I'm here!")); + Input = avg(_myPID.analogReadFast(inputPin)); // filtered input analogWrite(outputPin, Output); } diff --git a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino index ff9ba7c..2e889ee 100644 --- a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino +++ b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino @@ -31,7 +31,7 @@ void setup() { Serial.begin(115200); Serial.println(); if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { - Serial.println(F("AutoTune test exceeds outMax limit, check output, hysteresis and outputStep values")); + Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); while (1); } _myPID.AutoTune(tuningRule); diff --git a/library.json b/library.json index 5457652..8329d5c 100644 --- a/library.json +++ b/library.json @@ -1,9 +1,9 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards.", + "description": "A fast fixed/floating point PID controller with AutoTune and 10 tuning rules to choose from. This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards.", "license": "MIT", - "version": "2.2.8", + "version": "2.3.0", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index f047789..209c75d 100644 --- a/library.properties +++ b/library.properties @@ -1,8 +1,8 @@ name=QuickPID -version=2.2.8 +version=2.3.0 author=David Lloyd maintainer=David Lloyd -sentence=A fast fixed/floating point PID controller with AutoTune and 9 tuning rules to choose from. +sentence=A fast fixed/floating point PID controller with AutoTune and 10 tuning rules to choose from. paragraph=This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 68007b8..2dd5257 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,7 +1,8 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.2.8 + QuickPID Library for Arduino - Version 2.3.0 by dlloydev https://github.com/Dlloydev/QuickPID - Based on the Arduino PID Library, licensed under the MIT License + Based on the Arduino PID Library and work on AutoTunePID class + by gnalbandian (Gonzalo). Licensed under the MIT License **********************************************************************************/ #if ARDUINO >= 100 @@ -337,7 +338,7 @@ byte AutoTunePID::autoTuneLoop() _Tu = (float)(_t3 - _t2) / 1000000.0; // ultimate period (seconds) _Ku = (float)(4 * _outputStep * 2) / (float)(3.14159 * sqrt (sq (_peakHigh - _peakLow) - sq (_hysteresis))); // ultimate gain if (_tuningRule == 6) { //AMIGO_PID - (_td < 100) ? _td = 100 : _td = _td; // 100µs minimum + (_td < 0.1) ? _td = 0.1 : _td = _td; _kp = (0.2 + 0.45 * (_Tu / _td)) / _Ku; float Ti = (((0.4 * _td) + (0.8 * _Tu)) / (_td + (0.1 * _Tu)) * _td); float Td = (0.5 * _td * _Tu) / ((0.3 * _td) + _Tu); From 5acd87024fa349ef4f3a0bbd167d9c91ca2fa31a Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 16 May 2021 10:52:21 -0400 Subject: [PATCH 039/101] Update AutoTune_Filter_DIRECT.ino --- examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index 6fa8d2a..f5ebde7 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -40,7 +40,6 @@ void setup() { void loop() { if (_myPID.autoTune->autoTuneLoop() != _myPID.autoTune->RUN_PID) { // running autotune - Serial.println(F("I'm here!")); Input = avg(_myPID.analogReadFast(inputPin)); // filtered input analogWrite(outputPin, Output); From 8b465d4bf486f45885305cdae90f9a543bdf62c9 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 16 May 2021 11:01:26 -0400 Subject: [PATCH 040/101] Update README.md --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 48fd380..5397165 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ This PID controller provides a shortened *read-compute-write* cycle by using a r Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the change log and below: - Quicker hybrid fixed/floating point math, efficient PID algorithm and micros() timing resolution. -- Built-in `AutoTunePID` class as a dynamic object. [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) automatically determines and applies `Kp`, `Ki` and `Kd` tuning parameters and has 10 tuning rules to choose from. +- Built-in `AutoTunePID` class as a dynamic object. AutoTune automatically determines and applies `Kp`, `Ki` and `Kd` tuning parameters and has 10 tuning rules to choose from. - Variable `POn` parameter controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement. - Includes [analogWrite](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) for ESP32 boards. @@ -166,7 +166,7 @@ Use this link for reference. Note that if you're using QuickPID, there's no need #### Version 2.2.8 -- AutoTune is now independent of the QuickPID library and can be run from a sketch. AutoTune is now non-blocking, no timeouts are required and it uses Input and Output variables directly. See the example [AutoTune_RC_Filter.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_RC_Filter/AutoTune_RC_Filter.ino) for details. +- AutoTune is now independent of the QuickPID library and can be run from a sketch. AutoTune is now non-blocking, no timeouts are required and it uses Input and Output variables directly. #### Version 2.2.7 @@ -217,7 +217,7 @@ Use this link for reference. Note that if you're using QuickPID, there's no need #### Version 2.0.3 -- Initial Version with modifications as listed in [features.](#Features) +- Initial Version ------ From 779ad8ce350d8404ad0dac1f8be9f7efe164f092 Mon Sep 17 00:00:00 2001 From: Gonzalo Date: Tue, 18 May 2021 18:25:13 -0300 Subject: [PATCH 041/101] Changed return value for the current tunning stage Please, check example suggested in the other PR. --- src/QuickPID.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 2dd5257..b675257 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -367,10 +367,18 @@ byte AutoTunePID::autoTuneLoop() case NEW_TUNINGS: // ready to apply tunings *_output = 0; _autoTuneStage++; - return NEW_TUNINGS; + //return NEW_TUNINGS; break; + + case RUN_PID: // ready to apply tunings + return RUN_PID; + break; } - return RUN_PID; + + if(_autoTuneStage < 1) // safety measure to avoid overflow of _autoTuneStage variable if its value is 0, which shouldn't happen never. Nonetheless... + return 0 + else + return _autoTuneStage - 1; } void AutoTunePID::setAutoTuneConstants(float* kp, float* ki, float* kd) From d7e4e31acf79351ac4b1e22b90cdf9d5e117ef0e Mon Sep 17 00:00:00 2001 From: Gonzalo Date: Tue, 18 May 2021 18:27:17 -0300 Subject: [PATCH 042/101] Please consider this aproach Just some safety deference checks and better segmentation of the code. Just to better understand what is running at each loop cycle. --- .../AutoTune_Filter_DIRECT.ino | 54 +++++++++++-------- 1 file changed, 32 insertions(+), 22 deletions(-) diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index f5ebde7..04cbcb9 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -39,30 +39,40 @@ void setup() { void loop() { - if (_myPID.autoTune->autoTuneLoop() != _myPID.autoTune->RUN_PID) { // running autotune - - Input = avg(_myPID.analogReadFast(inputPin)); // filtered input - analogWrite(outputPin, Output); - } - - if (_myPID.autoTune->autoTuneLoop() == _myPID.autoTune->NEW_TUNINGS) { // get new tunings - _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings - _myPID.clearAutoTune(); // releases memory used by AutoTune object - _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID - _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID - Setpoint = 500; - } - - if (_myPID.autoTune->autoTuneLoop() == _myPID.autoTune->RUN_PID) { // running PID - if (printOrPlotter == 0) { // plotter - Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); - Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.println(); - } - Input = _myPID.analogReadFast(inputPin); +if (_myPID.autoTune) // Avoid deferencing nullptr after _myPID.clearAutoTune() +{ + uint8_t autoTuningCurrentStage = autoTuneLoop(); + if(autoTuningCurrentStage < _myPID.autoTune->RUN_PID) + { + Input = avg(_myPID.analogReadFast(inputPin)); // filtered input + analogWrite(outputPin, Output); + + if (autoTuningCurrentStage == _myPID.autoTune->NEW_TUNINGS) // get new tunings + { + _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings + _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID + _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + Setpoint = 500; + } + } + else // RUN_PID stage + { + if (printOrPlotter == 0) // plotter + { + _myPID.clearAutoTune(); // releases memory used by AutoTune object + Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); + Serial.print("Input:"); Serial.print(Input); Serial.print(","); + Serial.print("Output:"); Serial.print(Output); Serial.println(); + } + } + +} +else // Autotune already done (or not created) +{ + Input = _myPID.analogReadFast(inputPin); _myPID.Compute(); analogWrite(outputPin, Output); - } +} } float avg(int inputVal) { From 2ab38f4f7163b13af6ea746b681928a4304ed40c Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 18 May 2021 22:40:47 -0400 Subject: [PATCH 043/101] Update analogWrite.cpp --- src/analogWrite.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/analogWrite.cpp b/src/analogWrite.cpp index 9cec264..ec3e653 100644 --- a/src/analogWrite.cpp +++ b/src/analogWrite.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 2.0.6 + AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 2.0.8 by dlloydev https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite This Library is licensed under the MIT License **********************************************************************************/ @@ -236,12 +236,14 @@ int32_t analogWriteResolution(int8_t pin, uint8_t resolution) { if (ch >= 0) { if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; if (aw::pinsStatus[ch / aw::chd].resolution != resolution) { + ledcDetachPin(pin); aw::awLedcSetup(ch, aw::pinsStatus[ch / aw::chd].frequency, resolution & 0xF); + ledcAttachPin(pin, ch); ledcWrite(ch, aw::pinsStatus[ch / aw::chd].value); aw::pinsStatus[ch / aw::chd].resolution = resolution & 0xF; } } - return 1 << resolution & 0xF; + return 1 << (resolution & 0xF); } void setPinsStatusDefaults(int32_t value, float frequency, uint8_t resolution, uint32_t phase) { From 323d52e3801884cf58ffe5479fb9b3e23b1873f0 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Thu, 20 May 2021 10:26:47 -0400 Subject: [PATCH 044/101] Major update --- README.md | 10 +- .../AutoTune_Filter_DIRECT.ino | 68 +++--- .../AutoTune_Filter_REVERSE.ino | 39 ++-- src/QuickPID.cpp | 197 ++++++++++-------- src/QuickPID.h | 35 +++- 5 files changed, 202 insertions(+), 147 deletions(-) diff --git a/README.md b/README.md index 5397165..115fb1c 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used. This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. +QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. ### About @@ -164,6 +164,14 @@ Use this link for reference. Note that if you're using QuickPID, there's no need #### Change Log +#### Version 2.3.0 + +- New AutoTune class added as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). +- AutoTune now works for a reverse acting controller. +- AutoTune configuration parameters include outputStep, hysteresis, setpoint, output, direction and printOrPlotter. +- Defined tuningMethod as an enum. +- Updated AnalogWrite methods for ESP32/ESP32-S2. + #### Version 2.2.8 - AutoTune is now independent of the QuickPID library and can be run from a sketch. AutoTune is now non-blocking, no timeouts are required and it uses Input and Output variables directly. diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index 04cbcb9..d4723e2 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -23,6 +23,7 @@ int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; +bool pidLoop = false; QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); @@ -33,46 +34,47 @@ void setup() { Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); while (1); } - _myPID.AutoTune(tuningRule); + _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter); } void loop() { -if (_myPID.autoTune) // Avoid deferencing nullptr after _myPID.clearAutoTune() -{ - uint8_t autoTuningCurrentStage = autoTuneLoop(); - if(autoTuningCurrentStage < _myPID.autoTune->RUN_PID) - { - Input = avg(_myPID.analogReadFast(inputPin)); // filtered input - analogWrite(outputPin, Output); - - if (autoTuningCurrentStage == _myPID.autoTune->NEW_TUNINGS) // get new tunings - { - _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings - _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID - _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID - Setpoint = 500; - } - } - else // RUN_PID stage - { - if (printOrPlotter == 0) // plotter - { - _myPID.clearAutoTune(); // releases memory used by AutoTune object - Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); - Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.println(); - } - } - -} -else // Autotune already done (or not created) -{ - Input = _myPID.analogReadFast(inputPin); + if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() + { + switch (_myPID.autoTune->autoTuneLoop()) { + case _myPID.autoTune->AUTOTUNE: + Input = avg(_myPID.analogReadFast(inputPin)); + analogWrite(outputPin, Output); + break; + + case _myPID.autoTune->TUNINGS: + _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings + _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID + _myPID.SetSampleTimeUs(10000); // 10ms + _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + Setpoint = 500; + break; + + case _myPID.autoTune->CLR: + if (!pidLoop) { + _myPID.clearAutoTune(); // releases memory used by AutoTune object + pidLoop = true; + } + break; + } + } + if (pidLoop) { + if (printOrPlotter == 0) { // plotter + Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); + Serial.print("Input:"); Serial.print(Input); Serial.print(","); + Serial.print("Output:"); Serial.print(Output); Serial.println(); + } + Input = _myPID.analogReadFast(inputPin); _myPID.Compute(); analogWrite(outputPin, Output); -} + } + //delay(1); // adjust loop speed } float avg(int inputVal) { diff --git a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino index 2e889ee..9fc6110 100644 --- a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino +++ b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino @@ -24,6 +24,7 @@ int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; +bool pidLoop = false; QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::REVERSE); @@ -34,26 +35,37 @@ void setup() { Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); while (1); } - _myPID.AutoTune(tuningRule); + _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, inputMax - setpoint, output, QuickPID::REVERSE, printOrPlotter); } void loop() { - if (_myPID.autoTune->autoTuneLoop() != _myPID.autoTune->RUN_PID) { // running autotune - Input = inputMax - avg(_myPID.analogReadFast(inputPin)); // filtered input (reverse acting) - analogWrite(outputPin, Output); - } + if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() + { + switch (_myPID.autoTune->autoTuneLoop()) { + case _myPID.autoTune->AUTOTUNE: + Input = inputMax - avg(_myPID.analogReadFast(inputPin)); // filtered, reverse acting + analogWrite(outputPin, Output); + break; - if (_myPID.autoTune->autoTuneLoop() == _myPID.autoTune->NEW_TUNINGS) { // get new tunings - _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings - _myPID.clearAutoTune(); // releases memory used by AutoTune object - _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID - _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID - Setpoint = 500; - } + case _myPID.autoTune->TUNINGS: + _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings + _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID + _myPID.SetSampleTimeUs(10000); // 10ms + _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + Setpoint = 500; + break; - if (_myPID.autoTune->autoTuneLoop() == _myPID.autoTune->RUN_PID) { // running PID + case _myPID.autoTune->CLR: + if (!pidLoop) { + _myPID.clearAutoTune(); // releases memory used by AutoTune object + pidLoop = true; + } + break; + } + } + if (pidLoop) { if (printOrPlotter == 0) { // plotter Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); Serial.print("Input:"); Serial.print(Input); Serial.print(","); @@ -63,6 +75,7 @@ void loop() { _myPID.Compute(); analogWrite(outputPin, Output); } + //delay(1); // adjust loop speed } float avg(int inputVal) { diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index b675257..7dbb8f0 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -180,35 +180,11 @@ QuickPID::direction_t QuickPID::GetDirection() { return controllerDirection; } -/* Other Functions************************************************************/ +/* AutoTune Functions************************************************************/ -int QuickPID::analogReadFast(int ADCpin) { -#if defined(__AVR_ATmega328P__) - byte ADCregOriginal = ADCSRA; - ADCSRA = (ADCSRA & B11111000) | 5; // 32 prescaler - int adc = analogRead(ADCpin); - ADCSRA = ADCregOriginal; - return adc; -#elif defined(__AVR_ATtiny_Zero_One__) || defined(__AVR_ATmega_Zero__) - byte ADCregOriginal = ADC0_CTRLC; - ADC0_CTRLC = 0x54; // reduced cap, Vdd ref, 32 prescaler - int adc = analogRead(ADCpin); - ADC0_CTRLC = ADCregOriginal; - return adc; -#elif defined(__AVR_DA__) - byte ADCregOriginal = ADC0.CTRLC; - ADC0.CTRLC = ADC_PRESC_DIV32_gc; // 32 prescaler - int adc = analogRead(ADCpin); - ADC0.CTRLC = ADCregOriginal; - return adc; -#else - return analogRead(ADCpin); -# endif -} - -void QuickPID::AutoTune(uint8_t tuningRule) +void QuickPID::AutoTune(tuningMethod tuningRule) { - autoTune = new AutoTunePID(myInput, myOutput, (uint8_t)tuningRule); + autoTune = new AutoTunePID(myInput, myOutput, tuningRule); } void QuickPID::clearAutoTune() @@ -221,10 +197,11 @@ AutoTunePID::AutoTunePID() { _input = nullptr; _output = nullptr; + reset(); } -AutoTunePID::AutoTunePID(float *input, float *output, uint8_t tuningRule) +AutoTunePID::AutoTunePID(float *input, float *output, tuningMethod tuningRule) { AutoTunePID(); _input = input; @@ -234,6 +211,7 @@ AutoTunePID::AutoTunePID(float *input, float *output, uint8_t tuningRule) void AutoTunePID::reset() { + _t0 = 0; _t1 = 0; _t2 = 0; @@ -247,9 +225,11 @@ void AutoTunePID::reset() _rdAvg = 0.0; _peakHigh = 0.0; _peakLow = 0.0; + _autoTuneStage = 0; } -void AutoTunePID::autoTuneConfig(const byte outputStep, const byte hysteresis, const int atSetpoint, const int atOutput, const bool dir, const bool printOrPlotter) +void AutoTunePID::autoTuneConfig(const byte outputStep, const byte hysteresis, const int atSetpoint, + const int atOutput, const bool dir, const bool printOrPlotter) { _outputStep = outputStep; _hysteresis = hysteresis; @@ -257,128 +237,139 @@ void AutoTunePID::autoTuneConfig(const byte outputStep, const byte hysteresis, c _atOutput = atOutput; _direction = dir; _printOrPlotter = printOrPlotter; + _autoTuneStage = STABILIZING; } byte AutoTunePID::autoTuneLoop() { switch (_autoTuneStage) { + case AUTOTUNE: + return AUTOTUNE; + break; case STABILIZING: + if (_printOrPlotter == 1) Serial.print(F("Stabilizing →")); _peakHigh = _atSetpoint; _peakLow = _atSetpoint; - if (_printOrPlotter == 1) Serial.print(F("Stabilizing →")); - *_output = 0; - _autoTuneStage++; + (!_direction) ? *_output = 0 : *_output = _atOutput + _outputStep + 5; + _autoTuneStage = COARSE; + return AUTOTUNE; break; case COARSE: // coarse adjust + delay(2000); if (*_input < (_atSetpoint - _hysteresis)) { - if (_printOrPlotter == 1) Serial.print(F(" coarse →")); (!_direction) ? *_output = _atOutput + _outputStep + 5 : *_output = _atOutput - _outputStep - 5; - _autoTuneStage++; + _autoTuneStage = FINE; } + return AUTOTUNE; break; case FINE: // fine adjust if (*_input > _atSetpoint) { - if (_printOrPlotter == 1) Serial.print(F(" fine →")); (!_direction) ? *_output = _atOutput - _outputStep : *_output = _atOutput + _outputStep; - _autoTuneStage++; + _autoTuneStage = TEST; } + return AUTOTUNE; break; - case AUTOTUNE: // run AutoTune relay method + case TEST: // run AutoTune relay method if (*_input < _atSetpoint) { - if (_printOrPlotter == 1) Serial.print(F(" autotune →")); + if (_printOrPlotter == 1) Serial.print(F(" AutoTune →")); (!_direction) ? *_output = _atOutput + _outputStep : *_output = _atOutput - _outputStep; - _autoTuneStage++; + _autoTuneStage = T0; } + return AUTOTUNE; break; case T0: // get t0 if (*_input > _atSetpoint) { _t0 = micros(); if (_printOrPlotter == 1) Serial.print(F(" t0 →")); - _autoTuneStage++; + _autoTuneStage = T1; } + return AUTOTUNE; break; case T1: // get t1 if (*_input > _atSetpoint + 0.2) { _t1 = micros(); if (_printOrPlotter == 1) Serial.print(F(" t1 →")); - _autoTuneStage++; + _autoTuneStage = T2; } + return AUTOTUNE; break; case T2: // get t2 _rdAvg = *_input; if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; - if (_rdAvg > _atSetpoint + _hysteresis) { _t2 = micros(); if (_printOrPlotter == 1) Serial.print(F(" t2 →")); (!_direction) ? *_output = _atOutput - _outputStep : *_output = _atOutput + _outputStep; - _autoTuneStage++; + _autoTuneStage = T3L; } + return AUTOTUNE; break; - case T3: // get t3 + case T3L: // t3 low cycle _rdAvg = *_input; if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; if (_rdAvg < _atSetpoint - _hysteresis) { - if (_printOrPlotter == 1) Serial.print(F(" t3 →")); (!_direction) ? *_output = _atOutput + _outputStep : *_output = _atOutput - _outputStep; - _autoTuneStage++; + _autoTuneStage = T3H; } + return AUTOTUNE; break; - case DONE: // calculate everything + case T3H: // t3 high cycle, relay test done _rdAvg = *_input; if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; if (_rdAvg > _atSetpoint + _hysteresis) { _t3 = micros(); - if (_printOrPlotter == 1) Serial.println(F(" done.")); - _autoTuneStage++; - _td = (float)(_t1 - _t0) / 1000000.0; // dead time (seconds) - _Tu = (float)(_t3 - _t2) / 1000000.0; // ultimate period (seconds) - _Ku = (float)(4 * _outputStep * 2) / (float)(3.14159 * sqrt (sq (_peakHigh - _peakLow) - sq (_hysteresis))); // ultimate gain - if (_tuningRule == 6) { //AMIGO_PID - (_td < 0.1) ? _td = 0.1 : _td = _td; - _kp = (0.2 + 0.45 * (_Tu / _td)) / _Ku; - float Ti = (((0.4 * _td) + (0.8 * _Tu)) / (_td + (0.1 * _Tu)) * _td); - float Td = (0.5 * _td * _Tu) / ((0.3 * _td) + _Tu); - _ki = _kp / Ti; - _kd = Td * _kp; - } else { //other rules - _kp = RulesContants[_tuningRule][0] / 1000.0 * _Ku; - _ki = RulesContants[_tuningRule][1] / 1000.0 * _Ku / _Tu; - _kd = RulesContants[_tuningRule][2] / 1000.0 * _Ku * _Tu; - } - if (_printOrPlotter == 1) { - // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png - if ((_Tu / _td + 0.0001) > 0.75) Serial.println(F("This process is easy to control.")); - else if ((_Tu / _td + 0.0001) > 0.25) Serial.println(F("This process has average controllability.")); - else Serial.println(F("This process is difficult to control.")); - Serial.print(F("Tu: ")); Serial.print(_Tu); // Ultimate Period (sec) - Serial.print(F(" td: ")); Serial.print(_td); // Dead Time (sec) - Serial.print(F(" Ku: ")); Serial.print(_Ku); // Ultimate Gain - Serial.print(F(" Kp: ")); Serial.print(_kp); - Serial.print(F(" Ki: ")); Serial.print(_ki); - Serial.print(F(" Kd: ")); Serial.println(_kd); - Serial.println(); - } + if (_printOrPlotter == 1) Serial.println(F(" t3 → done.")); + _autoTuneStage = CALC; } + return AUTOTUNE; break; - case NEW_TUNINGS: // ready to apply tunings - *_output = 0; - _autoTuneStage++; - //return NEW_TUNINGS; + case CALC: // calculations + _td = (float)(_t1 - _t0) / 1000000.0; // dead time (seconds) + _Tu = (float)(_t3 - _t2) / 1000000.0; // ultimate period (seconds) + _Ku = (float)(4 * _outputStep * 2) / (float)(3.14159 * sqrt (sq (_peakHigh - _peakLow) - sq (_hysteresis))); // ultimate gain + if (_tuningRule == tuningMethod::AMIGOF_PID) { + (_td < 0.1) ? _td = 0.1 : _td = _td; + _kp = (0.2 + 0.45 * (_Tu / _td)) / _Ku; + float Ti = (((0.4 * _td) + (0.8 * _Tu)) / (_td + (0.1 * _Tu)) * _td); + float Td = (0.5 * _td * _Tu) / ((0.3 * _td) + _Tu); + _ki = _kp / Ti; + _kd = Td * _kp; + } else { //other rules + _kp = RulesContants[(int)_tuningRule][0] / 1000.0 * _Ku; + _ki = RulesContants[(int)_tuningRule][1] / 1000.0 * _Ku / _Tu; + _kd = RulesContants[(int)_tuningRule][2] / 1000.0 * _Ku * _Tu; + } + if (_printOrPlotter == 1) { + // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png + if ((_Tu / _td + 0.0001) > 0.75) Serial.println(F("This process is easy to control.")); + else if ((_Tu / _td + 0.0001) > 0.25) Serial.println(F("This process has average controllability.")); + else Serial.println(F("This process is difficult to control.")); + Serial.print(F("Tu: ")); Serial.print(_Tu); // Ultimate Period (sec) + Serial.print(F(" td: ")); Serial.print(_td); // Dead Time (sec) + Serial.print(F(" Ku: ")); Serial.print(_Ku); // Ultimate Gain + Serial.print(F(" Kp: ")); Serial.print(_kp); + Serial.print(F(" Ki: ")); Serial.print(_ki); + Serial.print(F(" Kd: ")); Serial.println(_kd); + Serial.println(); + } + _autoTuneStage = TUNINGS; + return AUTOTUNE; + break; + case TUNINGS: + _autoTuneStage = CLR; + return TUNINGS; + break; + case CLR: + return CLR; + break; + default: + return CLR; break; - - case RUN_PID: // ready to apply tunings - return RUN_PID; - break; } - - if(_autoTuneStage < 1) // safety measure to avoid overflow of _autoTuneStage variable if its value is 0, which shouldn't happen never. Nonetheless... - return 0 - else - return _autoTuneStage - 1; + return CLR; } void AutoTunePID::setAutoTuneConstants(float* kp, float* ki, float* kd) @@ -387,3 +378,29 @@ void AutoTunePID::setAutoTuneConstants(float* kp, float* ki, float* kd) *ki = _ki; *kd = _kd; } + +/* Other Functions************************************************************/ + +int QuickPID::analogReadFast(int ADCpin) { +#if defined(__AVR_ATmega328P__) + byte ADCregOriginal = ADCSRA; + ADCSRA = (ADCSRA & B11111000) | 5; // 32 prescaler + int adc = analogRead(ADCpin); + ADCSRA = ADCregOriginal; + return adc; +#elif defined(__AVR_ATtiny_Zero_One__) || defined(__AVR_ATmega_Zero__) + byte ADCregOriginal = ADC0_CTRLC; + ADC0_CTRLC = 0x54; // reduced cap, Vdd ref, 32 prescaler + int adc = analogRead(ADCpin); + ADC0_CTRLC = ADCregOriginal; + return adc; +#elif defined(__AVR_DA__) + byte ADCregOriginal = ADC0.CTRLC; + ADC0.CTRLC = ADC_PRESC_DIV32_gc; // 32 prescaler + int adc = analogRead(ADCpin); + ADC0.CTRLC = ADCregOriginal; + return adc; +#else + return analogRead(ADCpin); +# endif +} diff --git a/src/QuickPID.h b/src/QuickPID.h index 676f3bb..d4b5b64 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -2,28 +2,44 @@ #ifndef QuickPID_h #define QuickPID_h +enum class tuningMethod : uint8_t +{ + ZIEGLER_NICHOLS_PI, + ZIEGLER_NICHOLS_PID, + TYREUS_LUYBEN_PI, + TYREUS_LUYBEN_PID, + CIANCONE_MARLIN_PI, + CIANCONE_MARLIN_PID, + AMIGOF_PID, + PESSEN_INTEGRAL_PID, + SOME_OVERSHOOT_PID, + NO_OVERSHOOT_PID +}; + class AutoTunePID { public: AutoTunePID(); - AutoTunePID(float *input, float *output, uint8_t tuningRule); + AutoTunePID(float *input, float *output, tuningMethod tuningRule); ~AutoTunePID() {}; void reset(); void autoTuneConfig(const byte outputStep, const byte hysteresis, const int setpoint, const int output, - const bool dir = false, const bool printOrPlotter = false); + const bool dir = false, const bool printOrPlotter = false); byte autoTuneLoop(); void setAutoTuneConstants(float* kp, float* ki, float* kd); - enum atStage : byte { STABILIZING, COARSE, FINE, AUTOTUNE, T0, T1, T2, T3, DONE, NEW_TUNINGS, RUN_PID }; + enum atStage : byte { AUTOTUNE, STABILIZING, COARSE, FINE, TEST, T0, T1, T2, T3L, T3H, CALC, TUNINGS, CLR }; private: - float *_input = nullptr; // Pointers to the Input, Output, and Setpoint variables. This creates a - float *_output = nullptr; // hard link between the variables and the PID, freeing the user from having - byte _autoTuneStage = 0; - byte _tuningRule = 0; + float *_input = nullptr; // Pointers to the Input, Output, and Setpoint variables. This creates a + float *_output = nullptr; // hard link between the variables and the PID, freeing the user from having + // float *mySetpoint = nullptr; // to constantly tell us what these values are. With pointers we'll just know. + + byte _autoTuneStage = 1; + tuningMethod _tuningRule; byte _outputStep; byte _hysteresis; - int _atSetpoint; // 1/3 of 10-bit ADC range for symetrical waveform + int _atSetpoint; // 1/3 of 10-bit ADC range for symetrical waveform int _atOutput; bool _direction = false; bool _printOrPlotter = false; @@ -73,8 +89,7 @@ class QuickPID { bool Compute(); // Automatically determines and sets the tuning parameters Kp, Ki and Kd. Use this in the setup loop. - // void AutoTune(int inputPin, int outputPin, int tuningRule, int Print, uint32_t timeout); - void AutoTune(uint8_t tuningRule); + void AutoTune(tuningMethod tuningRule); void clearAutoTune(); // Sets and clamps the output to a specific range (0-255 by default). From 1ff4d46d85b62c82790ba012f46b21478e64bf76 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Thu, 20 May 2021 22:33:06 -0400 Subject: [PATCH 045/101] replace delay with timer --- examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino | 2 +- .../AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino | 2 +- src/QuickPID.cpp | 6 +++++- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index d4723e2..de72fee 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -74,7 +74,7 @@ void loop() { _myPID.Compute(); analogWrite(outputPin, Output); } - //delay(1); // adjust loop speed + delay(1); // adjust loop speed } float avg(int inputVal) { diff --git a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino index 9fc6110..f44d499 100644 --- a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino +++ b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino @@ -75,7 +75,7 @@ void loop() { _myPID.Compute(); analogWrite(outputPin, Output); } - //delay(1); // adjust loop speed + delay(1); // adjust loop speed } float avg(int inputVal) { diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 7dbb8f0..6c131b2 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -248,6 +248,7 @@ byte AutoTunePID::autoTuneLoop() break; case STABILIZING: if (_printOrPlotter == 1) Serial.print(F("Stabilizing →")); + _t0 = millis(); _peakHigh = _atSetpoint; _peakLow = _atSetpoint; (!_direction) ? *_output = 0 : *_output = _atOutput + _outputStep + 5; @@ -255,7 +256,10 @@ byte AutoTunePID::autoTuneLoop() return AUTOTUNE; break; case COARSE: // coarse adjust - delay(2000); + if (millis() - _t0 < 2000) { + return AUTOTUNE; + break; + } if (*_input < (_atSetpoint - _hysteresis)) { (!_direction) ? *_output = _atOutput + _outputStep + 5 : *_output = _atOutput - _outputStep - 5; _autoTuneStage = FINE; From 3fa06f2bbaf42ad9132b045efd0ea30aca914ea9 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 22 May 2021 13:51:49 -0400 Subject: [PATCH 046/101] Update - Resolved `Kp` windup as noted in issue #6. Algorithm reverts to upstream library, but with fixed point math option and newer controller direction method maintained. - Updated AutoTune examples and documentation. --- README.md | 39 +++++-------------- .../AutoTune_Filter_DIRECT.ino | 15 +++++-- .../AutoTune_Filter_REVERSE.ino | 15 +++++-- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 30 ++++++++++---- src/QuickPID.h | 4 +- 7 files changed, 60 insertions(+), 47 deletions(-) diff --git a/README.md b/README.md index 115fb1c..7f3fba3 100644 --- a/README.md +++ b/README.md @@ -1,26 +1,14 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. +QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. ### About -This PID controller provides a shortened *read-compute-write* cycle by using a reduced PID algorithm, taking advantage of fixed point math and using a fast analog read function. The `Ki` and `Kd` are scaled by time (µs) and the new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function resulting in only two multiply operations required in `Compute()`. +This PID controller provides a shortened *read-compute-write* cycle by taking advantage of fixed point math and using a fast analog read function. ### Features -Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the change log and below: - -- Quicker hybrid fixed/floating point math, efficient PID algorithm and micros() timing resolution. -- Built-in `AutoTunePID` class as a dynamic object. AutoTune automatically determines and applies `Kp`, `Ki` and `Kd` tuning parameters and has 10 tuning rules to choose from. -- Variable `POn` parameter controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement. -- Includes [analogWrite](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) for ESP32 boards. - -### Performance (16MHz ATmega328P) - -| `Compute()` | Kp | Ki | Kd | Step Time (µS) | -| :---------- | ---- | ---- | ---- | -------------- | -| QuickPID | 2.0 | 5.0 | 1.0 | 72 | -| Arduino PID | 2.0 | 5.0 | 1.0 | 104 | +Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the change log. ### [AutoTune RC Filter](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) @@ -28,19 +16,6 @@ This example allows you to experiment with the AutoTunePID class, various tuning #### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) -### Simplified PID Algorithm - -```c++ -outputSum += (kpi * error) - (kpd * dInput); -``` - -The `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated as follows: - -```c++ - kpi = kp * pOn + ki; - kpd = kp * (1 - pOn) + kd; -``` - ### Direct and Reverse Controller Action If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). See the examples `AutoTune_Filter_DIRECT.ino` and `AutoTune_Filter_REVERSE.ino` for details. @@ -79,7 +54,7 @@ This function contains the PID algorithm and it should be called once every loop #### [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) -For use of AutoTune, refer to the examples `AutoTune_Filter_DIRECT.ino` and `AutoTune_Filter_REVERSE.ino` +For use of AutoTune, refer to the examples [AutoTune_Filter_DIRECT.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino) and [AutoTune_Filter_REVERSE.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino) #### SetTunings @@ -164,6 +139,12 @@ Use this link for reference. Note that if you're using QuickPID, there's no need #### Change Log +#### Version 2.3.1 + +- Resolved `Kp` windup as noted in issue #6. Algorithm reverts to upstream library, but with fixed point math option and newer controller direction method maintained. +- Updated AutoTune examples and documentation. +- Default AutoTune `outputStep` value in examples (and documentation) is now 5. + #### Version 2.3.0 - New AutoTune class added as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index de72fee..358f020 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -1,6 +1,5 @@ /****************************************************************************** AutoTune Filter DIRECT Example - Reference: https://github.com/Dlloydev/QuickPID/wiki/AutoTune Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter ******************************************************************************/ @@ -15,8 +14,7 @@ const int outputMin = 0; float POn = 1.0; // mix of PonE to PonM (0.0-1.0) bool printOrPlotter = 0; // on(1) monitor, off(0) plotter -byte tuningRule = 1; // see reference link -byte outputStep = 1; +byte outputStep = 5; byte hysteresis = 1; int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform @@ -34,7 +32,18 @@ void setup() { Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); while (1); } + // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki + //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); + //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); + //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); + //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); + //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); + //_myPID.AutoTune(tuningMethod::AMIGOF_PID); + //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); + //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); + //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter); } diff --git a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino index f44d499..d38b9c1 100644 --- a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino +++ b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino @@ -1,6 +1,5 @@ /****************************************************************************** AutoTune Filter REVERSE Example - Reference: https://github.com/Dlloydev/QuickPID/wiki/AutoTune Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter ******************************************************************************/ @@ -16,8 +15,7 @@ const int outputMin = 0; float POn = 1.0; // mix of PonE to PonM (0.0-1.0) bool printOrPlotter = 0; // on(1) monitor, off(0) plotter -byte tuningRule = 1; // see reference link -byte outputStep = 1; +byte outputStep = 5; byte hysteresis = 1; int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform @@ -35,7 +33,18 @@ void setup() { Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); while (1); } + // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki + //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); + //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); + //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); + //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); + //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); + //_myPID.AutoTune(tuningMethod::AMIGOF_PID); + //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); + //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); + //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, inputMax - setpoint, output, QuickPID::REVERSE, printOrPlotter); } diff --git a/library.json b/library.json index 8329d5c..d107c27 100644 --- a/library.json +++ b/library.json @@ -3,7 +3,7 @@ "keywords": "PID, controller, signal", "description": "A fast fixed/floating point PID controller with AutoTune and 10 tuning rules to choose from. This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards.", "license": "MIT", - "version": "2.3.0", + "version": "2.3.1", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 209c75d..2bc7258 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.3.0 +version=2.3.1 author=David Lloyd maintainer=David Lloyd sentence=A fast fixed/floating point PID controller with AutoTune and 10 tuning rules to choose from. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 6c131b2..5c5ecc3 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.3.0 + QuickPID Library for Arduino - Version 2.3.1 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License @@ -53,7 +53,7 @@ bool QuickPID::Compute() { uint32_t now = micros(); uint32_t timeChange = (now - lastTime); - if (timeChange >= sampleTimeUs) { // Compute the working error variables + if (timeChange >= sampleTimeUs) { // compute the working errors float input = *myInput; float dInput = input - lastInput; error = *mySetpoint - input; @@ -61,12 +61,27 @@ bool QuickPID::Compute() { error = -error; dInput = -dInput; } - if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), (int)dInput); // fixed point - else outputSum += (kpi * error) - (kpd * dInput); // floating-point + // add integral error amount + if (ki < 31) outputSum += FX_MUL(FL_FX(ki), error); + else outputSum += (ki * error); + // proportional on measurement amount + if (kpm < 31) outputSum -= FX_MUL(FL_FX(kpm), dInput); + else outputSum -= (kpm * dInput); outputSum = CONSTRAIN(outputSum, outMin, outMax); - *myOutput = outputSum; + // proportional on error amount + float output; + if (kpe < 31) output = FX_MUL(FL_FX(kpe), error); + else output = (kpe * error); + + // derivative amount + if (kd < 31) output += outputSum - FX_MUL(FL_FX(kd), dInput); + else output += outputSum - kd * dInput; + output = CONSTRAIN(output, outMin, outMax); + *myOutput = output; + + // remember some variables for next time lastInput = input; lastTime = now; return true; @@ -88,8 +103,8 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) { kp = Kp; ki = Ki * SampleTimeSec; kd = Kd / SampleTimeSec; - kpi = (kp * pOn) + ki; - kpd = (kp * (1 - pOn)) + kd; + kpe = kp * pOn; + kpm = kp * (1 - pOn); } /* SetTunings(...)************************************************************* @@ -211,7 +226,6 @@ AutoTunePID::AutoTunePID(float *input, float *output, tuningMethod tuningRule) void AutoTunePID::reset() { - _t0 = 0; _t1 = 0; _t2 = 0; diff --git a/src/QuickPID.h b/src/QuickPID.h index d4b5b64..844bc54 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -133,8 +133,8 @@ class QuickPID { float kp; // (P)roportional Tuning Parameter float ki; // (I)ntegral Tuning Parameter float kd; // (D)erivative Tuning Parameter - float kpi; // proportional on error amount - float kpd; // proportional on measurement amount + float kpe; // proportional on error amount + float kpm; // proportional on measurement amount float *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a float *myOutput; // hard link between the variables and the PID, freeing the user from having From c601c20448c7dd28a36fdf1d26d48c0c63ec759d Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 23 May 2021 21:41:20 -0400 Subject: [PATCH 047/101] Update #### Version 2.3.2 - Removed fixed point calculations as the speed benefit was very minimal. - Prevent integral windup if output exceeds limits. - Added the following new functions that return the P, I and D terms of the calculation. ```c++ float GetPeTerm(); // proportional on error component of output float GetPmTerm(); // proportional on measurement component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output ``` #### --- README.md | 113 +++++++++++---------------------------------- keywords.txt | 7 +-- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 96 +++++++++++++++++++------------------- src/QuickPID.h | 28 ++++++----- 6 files changed, 96 insertions(+), 152 deletions(-) diff --git a/README.md b/README.md index 7f3fba3..1797d39 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,6 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. - -### About - -This PID controller provides a shortened *read-compute-write* cycle by taking advantage of fixed point math and using a fast analog read function. +QuickPID is an updated implementation of the Arduino PID library with a built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. ### Features @@ -18,7 +14,7 @@ This example allows you to experiment with the AutoTunePID class, various tuning ### Direct and Reverse Controller Action -If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). See the examples `AutoTune_Filter_DIRECT.ino` and `AutoTune_Filter_REVERSE.ino` for details. +If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). See the examples [AutoTune_Filter_DIRECT.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino) and [AutoTune_Filter_REVERSE.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino) for details. ### Functions @@ -31,11 +27,11 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. -- `POn` is the Proportional on Error weighting value (0.0-1.0). This controls the mix of Proportional on Error (PonE) and Proportional on Measurement (PonM) that's used in the compute algorithm. Note that POn controls the PonE amount, where the remainder (1-PonE) is the PonM amount. Also, the default POn is 1 +- `POn` is the Proportional on Error weighting value with range 0.0-1.0 and default 1.0 (100% Proportional on Error). This controls the mix of Proportional on Error to Proportional on Measurement. ![image](https://user-images.githubusercontent.com/63488701/118383726-986b6e80-b5ce-11eb-94b8-fdbddd4c914e.png) -- `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. DIRECT is most common. +- `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. ```c++ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, @@ -110,22 +106,23 @@ void QuickPID::SetControllerDirection(uint8_t Direction) The PID will either be connected to a DIRECT acting process (+Output leads to +Input) or a REVERSE acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. -#### Display_Functions +#### PID Query Functions ```c++ -float QuickPID::GetKp(); -float QuickPID::GetKi(); -float QuickPID::GetKd(); -float QuickPID::GetKu(); -float QuickPID::GetTu(); -float QuickPID::GetTd(); -uint8_t QuickPID::GetMode(); -uint8_t QuickPID::GetDirection(); + float GetKp(); // proportional gain + float GetKi(); // integral gain + float GetKd(); // derivative gain + float GetPeTerm(); // proportional on error component of output + float GetPmTerm(); // proportional on measurement component of output + float GetIterm(); // integral component of output + float GetDterm(); // derivative component of output + mode_t GetMode(); // MANUAL (0) or AUTOMATIC (1) + direction_t GetDirection(); // DIRECT (0) or REVERSE (1) ``` -These functions query the internal state of the PID. They're here for display purposes. +These functions query the internal state of the PID. -#### Utility_Functions +#### Utility Functions ```c++ int QuickPID::analogReadFast(int ADCpin) @@ -137,76 +134,20 @@ A faster configuration of `analogRead()`where a preset of 32 is used. If the ar Use this link for reference. Note that if you're using QuickPID, there's no need to install the AnalogWrite library as this feature is already included. -#### Change Log - -#### Version 2.3.1 - -- Resolved `Kp` windup as noted in issue #6. Algorithm reverts to upstream library, but with fixed point math option and newer controller direction method maintained. -- Updated AutoTune examples and documentation. -- Default AutoTune `outputStep` value in examples (and documentation) is now 5. - -#### Version 2.3.0 - -- New AutoTune class added as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). -- AutoTune now works for a reverse acting controller. -- AutoTune configuration parameters include outputStep, hysteresis, setpoint, output, direction and printOrPlotter. -- Defined tuningMethod as an enum. -- Updated AnalogWrite methods for ESP32/ESP32-S2. - -#### Version 2.2.8 - -- AutoTune is now independent of the QuickPID library and can be run from a sketch. AutoTune is now non-blocking, no timeouts are required and it uses Input and Output variables directly. - -#### Version 2.2.7 - -- Fixed REVERSE acting controller mode. -- now using src folder for source code -- replaced defines with enumerated types and inline functions - -#### Version 2.2.6 - -- Changed Input, Output and Setpoint parameters to float. -- Updated compatibility with the ESP32 AnalogWrite - -#### Version 2.2.2 +### [Change Log](https://github.com/Dlloydev/QuickPID/wiki/Change-Log) -- Added compatibility with the ESP32 Arduino framework -- Added full featured AnalogWrite methods for ESP32 and ESP32S2 +#### Latest Version 2.3.2 -#### Version 2.2.1 +- Removed fixed point calculations as the speed benefit was very minimal. +- Prevent integral windup if output exceeds limits. +- Added the following new functions that return the P, I and D terms of the calculation. -- Even faster AutoTune function -- AutoTune now determines the controllability of the process -- Added AMIGO_PID tuning rule -- Added `GetTd()` function to display dead time - -#### Version 2.2.0 - -- Improved AutoTune function -- Added 8 tuning rules -- Added `GetKu()` function to display ultimate gain -- Added `GetTu()` function to display ultimate period (time constant) -- Updated example and documentation - -#### Version 2.1.0 - -- Added AutoTune function and documentation - -- Added AutoTune_RC_Filter example and documentation - -#### Version 2.0.5 - -- Added MIT license text file -- POn defaults to 1 - -#### Version 2.0.4 - -- Added `QuickPID_AdaptiveTunings.ino`, `QuickPID_Basic.ino`, `QuickPID_PonM.ino` and `QuickPID_RelayOutput.ino` to the examples folder. -- `QuickPID_RelayOutput.ino` has the added feature of `minWindow` setting that sets the minimum on time for the relay. - -#### Version 2.0.3 - -- Initial Version +```c++ + float GetPeTerm(); // proportional on error component of output + float GetPmTerm(); // proportional on measurement component of output + float GetIterm(); // integral component of output + float GetDterm(); // derivative component of output +``` ------ diff --git a/keywords.txt b/keywords.txt index 01aa409..8c71e6a 100644 --- a/keywords.txt +++ b/keywords.txt @@ -23,9 +23,10 @@ SetSampleTimeUs KEYWORD2 GetKp KEYWORD2 GetKi KEYWORD2 GetKd KEYWORD2 -GetKu KEYWORD2 -GetTu KEYWORD2 -GetTd KEYWORD2 +GetPeTerm KEYWORD2 +GetPmTerm KEYWORD2 +GetIterm KEYWORD2 +GetDterm KEYWORD2 GetMode KEYWORD2 GetDirection KEYWORD2 analogReadFast KEYWORD2 diff --git a/library.json b/library.json index d107c27..52db86d 100644 --- a/library.json +++ b/library.json @@ -3,7 +3,7 @@ "keywords": "PID, controller, signal", "description": "A fast fixed/floating point PID controller with AutoTune and 10 tuning rules to choose from. This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards.", "license": "MIT", - "version": "2.3.1", + "version": "2.3.2", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 2bc7258..1c4bf99 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.3.1 +version=2.3.2 author=David Lloyd maintainer=David Lloyd sentence=A fast fixed/floating point PID controller with AutoTune and 10 tuning rules to choose from. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 5c5ecc3..76d7eeb 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.3.1 + QuickPID Library for Arduino - Version 2.3.2 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License @@ -34,16 +34,15 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, lastTime = micros() - sampleTimeUs; } -/* Constructor ******************************************************************** - To allow backwards compatability for v1.1, or for people that just want - to use Proportional on Error without explicitly saying so. +/* Constructor ********************************************************************* + To allow using Proportional on Error without explicitly saying so. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, direction_t ControllerDirection = DIRECT) : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = DIRECT) { } -/* Compute() ********************************************************************** +/* Compute() *********************************************************************** This function should be called every time "void loop()" executes. The function will decide whether a new PID Output needs to be computed. Returns true when the output is computed, false when nothing has been done. @@ -52,8 +51,7 @@ bool QuickPID::Compute() { if (!inAuto) return false; uint32_t now = micros(); uint32_t timeChange = (now - lastTime); - - if (timeChange >= sampleTimeUs) { // compute the working errors + if (timeChange >= sampleTimeUs) { float input = *myInput; float dInput = input - lastInput; error = *mySetpoint - input; @@ -61,27 +59,24 @@ bool QuickPID::Compute() { error = -error; dInput = -dInput; } - // add integral error amount - if (ki < 31) outputSum += FX_MUL(FL_FX(ki), error); - else outputSum += (ki * error); + iTerm = ki * error; + outputSum += iTerm; // add integral error amount + if (outputSum > outMax) iTerm -= outputSum - outMax; // improve integral windup + else if (outputSum < outMin) iTerm += outMin - outputSum; - // proportional on measurement amount - if (kpm < 31) outputSum -= FX_MUL(FL_FX(kpm), dInput); - else outputSum -= (kpm * dInput); + pmTerm = kpm * dInput; + outputSum -= (kpm * dInput); // subtract proportional on measurement amount outputSum = CONSTRAIN(outputSum, outMin, outMax); - - // proportional on error amount float output; - if (kpe < 31) output = FX_MUL(FL_FX(kpe), error); - else output = (kpe * error); - // derivative amount - if (kd < 31) output += outputSum - FX_MUL(FL_FX(kd), dInput); - else output += outputSum - kd * dInput; + peTerm = kpe * error; + output = peTerm; // add proportional on error amount + + dTerm = kd * dInput; + output += outputSum - dTerm; // add derivative amount output = CONSTRAIN(output, outMin, outMax); - *myOutput = output; - // remember some variables for next time + *myOutput = output; lastInput = input; lastTime = now; return true; @@ -107,14 +102,14 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) { kpm = kp * (1 - pOn); } -/* SetTunings(...)************************************************************* +/* SetTunings(...)************************************************************ Set Tunings using the last remembered POn setting. ******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { SetTunings(Kp, Ki, Kd, pOn); } -/* SetSampleTime(...) ********************************************************* +/* SetSampleTime(.)*********************************************************** Sets the period, in microseconds, at which the calculation is performed ******************************************************************************/ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) { @@ -126,7 +121,7 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) { } } -/* SetOutputLimits(...)******************************************************** +/* SetOutputLimits(..)******************************************************** The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. ******************************************************************************/ @@ -141,7 +136,7 @@ void QuickPID::SetOutputLimits(int Min, int Max) { } } -/* SetMode(...)**************************************************************** +/* SetMode(.)***************************************************************** Allows the controller Mode to be set to manual (0) or Automatic (non-zero) when the transition from manual to auto occurs, the controller is automatically initialized @@ -164,7 +159,7 @@ void QuickPID::Initialize() { outputSum = CONSTRAIN(outputSum, outMin, outMax); } -/* SetControllerDirection(...)************************************************* +/* SetControllerDirection(.)************************************************** The PID will either be connected to a DIRECT acting process (+Output leads to +Input) or a REVERSE acting process(+Output leads to -Input.) ******************************************************************************/ @@ -173,18 +168,29 @@ void QuickPID::SetControllerDirection(direction_t ControllerDirection) { } /* Status Functions************************************************************ - Just because you set the Kp=-1 doesn't mean it actually happened. These - functions query the internal state of the PID. They're here for display + These functions query the internal state of the PID. They're here for display purposes. These are the functions the PID Front-end uses for example. ******************************************************************************/ float QuickPID::GetKp() { - return dispKp; + return dispKp; } float QuickPID::GetKi() { - return dispKi; + return dispKi; } float QuickPID::GetKd() { - return dispKd; + return dispKd; +} +float QuickPID::GetPeTerm() { + return peTerm; +} +float QuickPID::GetPmTerm() { + return pmTerm; +} +float QuickPID::GetIterm() { + return iTerm; +} +float QuickPID::GetDterm() { + return dTerm; } QuickPID::mode_t QuickPID::GetMode() { @@ -195,37 +201,31 @@ QuickPID::direction_t QuickPID::GetDirection() { return controllerDirection; } -/* AutoTune Functions************************************************************/ +/* AutoTune Functions*********************************************************/ -void QuickPID::AutoTune(tuningMethod tuningRule) -{ +void QuickPID::AutoTune(tuningMethod tuningRule) { autoTune = new AutoTunePID(myInput, myOutput, tuningRule); } -void QuickPID::clearAutoTune() -{ +void QuickPID::clearAutoTune() { if (autoTune) delete autoTune; } -AutoTunePID::AutoTunePID() -{ +AutoTunePID::AutoTunePID() { _input = nullptr; _output = nullptr; - reset(); } -AutoTunePID::AutoTunePID(float *input, float *output, tuningMethod tuningRule) -{ +AutoTunePID::AutoTunePID(float *input, float *output, tuningMethod tuningRule) { AutoTunePID(); _input = input; _output = output; _tuningRule = tuningRule; } -void AutoTunePID::reset() -{ +void AutoTunePID::reset() { _t0 = 0; _t1 = 0; _t2 = 0; @@ -254,8 +254,7 @@ void AutoTunePID::autoTuneConfig(const byte outputStep, const byte hysteresis, c _autoTuneStage = STABILIZING; } -byte AutoTunePID::autoTuneLoop() -{ +byte AutoTunePID::autoTuneLoop() { switch (_autoTuneStage) { case AUTOTUNE: return AUTOTUNE; @@ -390,14 +389,13 @@ byte AutoTunePID::autoTuneLoop() return CLR; } -void AutoTunePID::setAutoTuneConstants(float* kp, float* ki, float* kd) -{ +void AutoTunePID::setAutoTuneConstants(float* kp, float* ki, float* kd) { *kp = _kp; *ki = _ki; *kd = _kd; } -/* Other Functions************************************************************/ +/* Utility************************************************************/ int QuickPID::analogReadFast(int ADCpin) { #if defined(__AVR_ATmega328P__) diff --git a/src/QuickPID.h b/src/QuickPID.h index 844bc54..67fce80 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -111,12 +111,17 @@ class QuickPID { // Sets the sample time in microseconds with which each PID calculation is performed. Default is 100000 µs. void SetSampleTimeUs(uint32_t NewSampleTimeUs); - //Display functions ****************************************************************************************** - float GetKp(); // These functions query the pid for interal values. They were created mainly for - float GetKi(); // the pid front-end, where it's important to know what is actually inside the PID. - float GetKd(); - mode_t GetMode(); - direction_t GetDirection(); + // PID Query functions *********************************************************************************** + float GetKp(); // proportional gain + float GetKi(); // integral gain + float GetKd(); // derivative gain + float GetPeTerm(); // proportional on error component of output + float GetPmTerm(); // proportional on measurement component of output + float GetIterm(); // integral component of output + float GetDterm(); // derivative component of output + mode_t GetMode(); // MANUAL (0) or AUTOMATIC (1) + direction_t GetDirection(); // DIRECT (0) or REVERSE (1) + int analogReadFast(int ADCpin); AutoTunePID *autoTune; @@ -128,6 +133,11 @@ class QuickPID { float dispKp; // tuning parameters for display purposes. float dispKi; float dispKd; + float peTerm; + float pmTerm; + float iTerm; + float dTerm; + float pOn; // proportional mode (0-1) default = 1 (100% Proportional on Error) float kp; // (P)roportional Tuning Parameter @@ -147,12 +157,6 @@ class QuickPID { float lastInput; bool inAuto; - inline int32_t FL_FX(float a) { - return (a * 256.0); // float to fixed point - } - inline int32_t FX_MUL(int32_t a, int32_t b) { - return ((a * b) >> 8); // fixed point multiply - } inline int32_t CONSTRAIN(int32_t x, int32_t lower, int32_t upper) { return ((x) < (lower) ? (lower) : ((x) > (upper) ? (upper) : (x))); } From 45681465873dd6b1dd0aab56bdca0042e8af5d59 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 24 May 2021 09:57:21 -0400 Subject: [PATCH 048/101] Update Readme --- README.md | 34 ++++++++++++++++------------------ src/QuickPID.cpp | 2 +- 2 files changed, 17 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index 1797d39..5928de1 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,22 @@ QuickPID is an updated implementation of the Arduino PID library with a built-in ### Features -Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the change log. +Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the [change log](https://github.com/Dlloydev/QuickPID/wiki/Change-Log). + +#### New feature Summary + +- [x] `analogReadFast()` support for AVR (4x faster) +- [x] `analogWrite()` support for ESP32 and ESP32-S2 +- [x] Variable Proportional on Error Proportional on Measurement parameter `POn` +- [x] Integral windup prevention when output exceeds limits +- [x] New PID query functions that return the P, I and D terms of the calculation +- [x] New AutoTune class added as a dynamic object and includes 10 tuning methods +- [x] AutoTune is compatible with reverse acting controllers +- [x] AutoTune's fast, non-blocking tuning sequence completes in only 1.5 oscillations +- [x] AutoTune determines how easy the process is to control +- [x] AutoTune determines ultimate period `Tu`, dead time `td`, ultimate gain `Ku`, and tuning parameters `Kp, Ki, Kd` +- [x] New REVERSE mode only changes sign of `error` and `dInput` +- [x] Uses `float` instead of `double` ### [AutoTune RC Filter](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) @@ -134,23 +149,6 @@ A faster configuration of `analogRead()`where a preset of 32 is used. If the ar Use this link for reference. Note that if you're using QuickPID, there's no need to install the AnalogWrite library as this feature is already included. -### [Change Log](https://github.com/Dlloydev/QuickPID/wiki/Change-Log) - -#### Latest Version 2.3.2 - -- Removed fixed point calculations as the speed benefit was very minimal. -- Prevent integral windup if output exceeds limits. -- Added the following new functions that return the P, I and D terms of the calculation. - -```c++ - float GetPeTerm(); // proportional on error component of output - float GetPmTerm(); // proportional on measurement component of output - float GetIterm(); // integral component of output - float GetDterm(); // derivative component of output -``` - ------- - ### Original README (Arduino PID) ``` diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 76d7eeb..036ac4c 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -61,7 +61,7 @@ bool QuickPID::Compute() { } iTerm = ki * error; outputSum += iTerm; // add integral error amount - if (outputSum > outMax) iTerm -= outputSum - outMax; // improve integral windup + if (outputSum > outMax) iTerm -= outputSum - outMax; // prevents integral windup else if (outputSum < outMin) iTerm += outMin - outputSum; pmTerm = kpm * dInput; From ca599e0d723c84897c0b8ca90327438e6fec72f1 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 24 May 2021 21:10:43 -0400 Subject: [PATCH 049/101] New TIMER mode #### QuickPID 2.3.3 - Added new `TIMER` mode which is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See AutoTune_`TIMER` mode example [AutoTune_Filter_TIMER_Mode.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino) --- README.md | 7 +- .../AutoTune_Filter_TIMER_Mode.ino | 109 ++++++++++++++++++ keywords.txt | 1 + library.json | 4 +- library.properties | 4 +- src/QuickPID.cpp | 37 +++--- src/QuickPID.h | 19 +-- 7 files changed, 146 insertions(+), 35 deletions(-) create mode 100644 examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino diff --git a/README.md b/README.md index 5928de1..3bfb265 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) +# QuickPID ![arduino-library-badge](https://camo.githubusercontent.com/989057908f34abd0c8bc2a8d762f86ccebbe377ed9ffef8c3dfdf27a09c6dac9/68747470733a2f2f7777772e617264752d62616467652e636f6d2f62616467652f517569636b5049442e7376673f) QuickPID is an updated implementation of the Arduino PID library with a built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. @@ -8,6 +8,7 @@ Development began with a fork of the Arduino PID Library. Modifications and new #### New feature Summary +- [x] `TIMER` mode for calling PID compute by an external timer function or ISR - [x] `analogReadFast()` support for AVR (4x faster) - [x] `analogWrite()` support for ESP32 and ESP32-S2 - [x] Variable Proportional on Error Proportional on Measurement parameter `POn` @@ -103,7 +104,9 @@ The PID controller is designed to vary its output within a given range. By defa void QuickPID::SetMode(uint8_t Mode); ``` -Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (non-zero). when the transition from manual to automatic occurs, the controller is automatically initialized. +Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (1) or `TIMER` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. + +Timer mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See example [AutoTune_Filter_TIMER_Mode.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino) #### Initialize diff --git a/examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino b/examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino new file mode 100644 index 0000000..46ffa3e --- /dev/null +++ b/examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino @@ -0,0 +1,109 @@ +/****************************************************************************** + AutoTune Filter TIMER Mode Example + Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter + ******************************************************************************/ +#include "NoDelay.h" // https://github.com/M-tech-Creations/NoDelay +#include "QuickPID.h" +void runPid(); // declare function before noDelay +noDelay LEDtime(10, runPid); // creates a noDelay varible set to 10ms, will call runPid function + +const byte inputPin = 0; +const byte outputPin = 3; + +const int outputMax = 255; +const int outputMin = 0; + +float POn = 1.0; // mix of PonE to PonM (0.0-1.0) +bool printOrPlotter = 0; // on(1) monitor, off(0) plotter +byte outputStep = 5; +byte hysteresis = 1; +int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform +int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform + +float Input, Output, Setpoint; +float Kp = 0, Ki = 0, Kd = 0; +bool pidLoop = false; +static boolean computeNow = false; + +QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); + +void setup() { + Serial.begin(115200); + Serial.println(); + if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { + Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); + while (1); + } + // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki + //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); + _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); + //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); + //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); + //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); + //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); + //_myPID.AutoTune(tuningMethod::AMIGOF_PID); + //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); + //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); + //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); + + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter); +} + +void loop() { + LEDtime.update();//will check if set time has past and if so will run set function + + if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() + { + switch (_myPID.autoTune->autoTuneLoop()) { + case _myPID.autoTune->AUTOTUNE: + Input = avg(_myPID.analogReadFast(inputPin)); + analogWrite(outputPin, Output); + break; + + case _myPID.autoTune->TUNINGS: + _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings + _myPID.SetMode(QuickPID::TIMER); // setup PID + _myPID.SetSampleTimeUs(10000); // 10ms + _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + Setpoint = 500; + break; + + case _myPID.autoTune->CLR: + if (!pidLoop) { + _myPID.clearAutoTune(); // releases memory used by AutoTune object + pidLoop = true; + } + break; + } + } + if (pidLoop) { + if (printOrPlotter == 0) { // plotter + Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); + Serial.print("Input:"); Serial.print(Input); Serial.print(","); + Serial.print("Output:"); Serial.print(Output); Serial.print(","); + Serial.print("iTerm:"); Serial.print(_myPID.GetDterm()); Serial.println(); + } + if (computeNow) { + Input = _myPID.analogReadFast(inputPin); + _myPID.Compute(); + analogWrite(outputPin, Output); + computeNow = false; + } + } + delay(1); // adjust loop speed +} + +void runPid() { + computeNow = true; +} + +float avg(int inputVal) { + static int arrDat[16]; + static int pos; + static long sum; + pos++; + if (pos >= 16) pos = 0; + sum = sum - arrDat[pos] + inputVal; + arrDat[pos] = inputVal; + return (float)sum / 16.0; +} diff --git a/keywords.txt b/keywords.txt index 8c71e6a..f7f398e 100644 --- a/keywords.txt +++ b/keywords.txt @@ -41,6 +41,7 @@ analogWriteResolution KEYWORD2 AUTOMATIC LITERAL1 MANUAL LITERAL1 +TIMER LITERAL1 DIRECT LITERAL1 REVERSE LITERAL1 mode_t LITERAL1 diff --git a/library.json b/library.json index 52db86d..257f37d 100644 --- a/library.json +++ b/library.json @@ -1,9 +1,9 @@ { "name": "QuickPID", "keywords": "PID, controller, signal", - "description": "A fast fixed/floating point PID controller with AutoTune and 10 tuning rules to choose from. This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards.", + "description": "A fast PID controller with AutoTune and 10 tuning rules. Compatible with most Arduino and ESP32 boards.", "license": "MIT", - "version": "2.3.2", + "version": "2.3.3", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 1c4bf99..f45599b 100644 --- a/library.properties +++ b/library.properties @@ -1,8 +1,8 @@ name=QuickPID -version=2.3.2 +version=2.3.3 author=David Lloyd maintainer=David Lloyd -sentence=A fast fixed/floating point PID controller with AutoTune and 10 tuning rules to choose from. +sentence=A fast PID controller with AutoTune and 10 tuning rules. paragraph=This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 036ac4c..d5e3349 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,8 +1,8 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.3.2 + QuickPID Library for Arduino - Version 2.3.3 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class - by gnalbandian (Gonzalo). Licensed under the MIT License + by gnalbandian (Gonzalo). Licensed under the MIT License. **********************************************************************************/ #if ARDUINO >= 100 @@ -24,7 +24,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, myOutput = Output; myInput = Input; mySetpoint = Setpoint; - inAuto = false; + mode = MANUAL; QuickPID::SetOutputLimits(0, 255); // same default as Arduino PWM limit sampleTimeUs = 100000; // 0.1 sec default @@ -48,10 +48,10 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, when the output is computed, false when nothing has been done. **********************************************************************************/ bool QuickPID::Compute() { - if (!inAuto) return false; - uint32_t now = micros(); - uint32_t timeChange = (now - lastTime); - if (timeChange >= sampleTimeUs) { + if (mode == MANUAL) return false; + uint32_t now = micros(); + uint32_t timeChange = (now - lastTime); + if (mode == TIMER || timeChange >= sampleTimeUs) { float input = *myInput; float dInput = input - lastInput; error = *mySetpoint - input; @@ -87,7 +87,7 @@ bool QuickPID::Compute() { /* SetTunings(....)************************************************************ This function allows the controller's dynamic performance to be adjusted. it's called automatically from the constructor, but tunings can also - be adjusted on the fly during normal operation + be adjusted on the fly during normal operation. ******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) { if (Kp < 0 || Ki < 0 || Kd < 0) return; @@ -110,7 +110,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { } /* SetSampleTime(.)*********************************************************** - Sets the period, in microseconds, at which the calculation is performed + Sets the period, in microseconds, at which the calculation is performed. ******************************************************************************/ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) { if (NewSampleTimeUs > 0) { @@ -130,23 +130,22 @@ void QuickPID::SetOutputLimits(int Min, int Max) { outMin = Min; outMax = Max; - if (inAuto) { + if (mode != MANUAL) { *myOutput = CONSTRAIN(*myOutput, outMin, outMax); outputSum = CONSTRAIN(outputSum, outMin, outMax); } } /* SetMode(.)***************************************************************** - Allows the controller Mode to be set to manual (0) or Automatic (non-zero) - when the transition from manual to auto occurs, the controller is - automatically initialized + Sets the controller mode to MANUAL (0), AUTOMATIC (1) or TIMER (2) + when the transition from MANUAL to AUTOMATIC or TIMER occurs, the + controller is automatically initialized. ******************************************************************************/ void QuickPID::SetMode(mode_t Mode) { - bool newAuto = (Mode == AUTOMATIC); - if (newAuto && !inAuto) { //we just went from manual to auto + if (mode == MANUAL && Mode != MANUAL) { // just went from MANUAL to AUTOMATIC or TIMER QuickPID::Initialize(); } - inAuto = newAuto; + mode = Mode; } /* Initialize()**************************************************************** @@ -161,7 +160,7 @@ void QuickPID::Initialize() { /* SetControllerDirection(.)************************************************** The PID will either be connected to a DIRECT acting process (+Output leads - to +Input) or a REVERSE acting process(+Output leads to -Input.) + to +Input) or a REVERSE acting process(+Output leads to -Input). ******************************************************************************/ void QuickPID::SetControllerDirection(direction_t ControllerDirection) { controllerDirection = ControllerDirection; @@ -192,11 +191,9 @@ float QuickPID::GetIterm() { float QuickPID::GetDterm() { return dTerm; } - QuickPID::mode_t QuickPID::GetMode() { - return inAuto ? QuickPID::AUTOMATIC : QuickPID::MANUAL; + return mode; } - QuickPID::direction_t QuickPID::GetDirection() { return controllerDirection; } diff --git a/src/QuickPID.h b/src/QuickPID.h index 67fce80..7bf85d3 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -68,7 +68,7 @@ class QuickPID { public: // controller mode - typedef enum { MANUAL = 0, AUTOMATIC = 1 } mode_t; + typedef enum { MANUAL = 0, AUTOMATIC = 1, TIMER = 2 } mode_t; // DIRECT: intput increases when the error is positive. REVERSE: intput decreases when the error is positive. typedef enum { DIRECT = 0, REVERSE = 1 } direction_t; @@ -78,13 +78,13 @@ class QuickPID { // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, float POn, direction_t ControllerDirection); - // Overload constructor with proportional mode. Links the PID to Input, Output, Setpoint and Tuning Parameters. + // Overload constructor with proportional ratio. Links the PID to Input, Output, Setpoint and Tuning Parameters. QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, direction_t ControllerDirection); - // Sets PID to either Manual (0) or Auto (non-0). + // Sets PID mode to MANUAL (0), AUTOMATIC (1) or TIMER (2). void SetMode(mode_t Mode); - // Performs the PID calculation. It should be called every time loop() cycles. ON/OFF and calculation frequency + // Performs the PID calculation. It should be called every time loop() cycles ON/OFF and calculation frequency // can be set using SetMode and SetSampleTime respectively. bool Compute(); @@ -101,7 +101,7 @@ class QuickPID { // changing tunings during runtime for Adaptive control. void SetTunings(float Kp, float Ki, float Kd); - // Overload for specifying proportional mode. + // Overload for specifying proportional ratio. void SetTunings(float Kp, float Ki, float Kd, float POn); // Sets the controller Direction or Action. DIRECT means the output will increase when the error is positive. @@ -115,11 +115,11 @@ class QuickPID { float GetKp(); // proportional gain float GetKi(); // integral gain float GetKd(); // derivative gain - float GetPeTerm(); // proportional on error component of output + float GetPeTerm(); // proportional on error component of output float GetPmTerm(); // proportional on measurement component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output - mode_t GetMode(); // MANUAL (0) or AUTOMATIC (1) + mode_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) direction_t GetDirection(); // DIRECT (0) or REVERSE (1) int analogReadFast(int ADCpin); @@ -137,9 +137,9 @@ class QuickPID { float pmTerm; float iTerm; float dTerm; - - float pOn; // proportional mode (0-1) default = 1 (100% Proportional on Error) + + float pOn; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 float kp; // (P)roportional Tuning Parameter float ki; // (I)ntegral Tuning Parameter float kd; // (D)erivative Tuning Parameter @@ -150,6 +150,7 @@ class QuickPID { float *myOutput; // hard link between the variables and the PID, freeing the user from having float *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. + mode_t mode = MANUAL; direction_t controllerDirection; uint32_t sampleTimeUs, lastTime; int outMin, outMax, error; From daa7cb68ee632c85f70d503239965b2c6029062b Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Thu, 27 May 2021 18:00:22 -0400 Subject: [PATCH 050/101] Update New TIMER mode examples #12 --- README.md | 9 +- .../AutoTune_Filter_DIRECT.ino | 9 +- .../AutoTune_Filter_REVERSE.ino | 9 +- .../AutoTune_Interrupt_TIMER.ino} | 25 ++--- .../AutoTune_Software_TIMER.ino | 104 ++++++++++++++++++ .../Basic_Interrupt_TIMER.ino | 44 ++++++++ .../Basic_Software_TIMER.ino | 46 ++++++++ .../QuickPID_AdaptiveTunings.ino | 2 +- examples/QuickPID_Basic/QuickPID_Basic.ino | 2 +- examples/QuickPID_PonM/QuickPID_PonM.ino | 2 +- .../QuickPID_RelayOutput.ino | 4 +- src/QuickPID.cpp | 22 ++-- src/QuickPID.h | 10 +- 13 files changed, 238 insertions(+), 50 deletions(-) rename examples/{AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino => AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino} (85%) create mode 100644 examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino create mode 100644 examples/Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino create mode 100644 examples/Basic_Software_TIMER/Basic_Software_TIMER.ino diff --git a/README.md b/README.md index 3bfb265..c776222 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ This function contains the PID algorithm and it should be called once every loop #### [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) -For use of AutoTune, refer to the examples [AutoTune_Filter_DIRECT.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino) and [AutoTune_Filter_REVERSE.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino) +For examples using AutoTune, please refer to the examples folder. #### SetTunings @@ -106,7 +106,12 @@ void QuickPID::SetMode(uint8_t Mode); Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (1) or `TIMER` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. -Timer mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See example [AutoTune_Filter_TIMER_Mode.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino) +`TIMER` mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See examples: + +- [AutoTune_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino) +- [AutoTune_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino) +- [Basic_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino) +- [Basic_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/Basic_Software_TIMER/Basic_Software_TIMER.ino) #### Initialize diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index 358f020..bbb6dfb 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -5,9 +5,9 @@ #include "QuickPID.h" +const uint32_t sampleTimeUs = 10000; // 10ms const byte inputPin = 0; const byte outputPin = 3; - const int outputMax = 255; const int outputMin = 0; @@ -44,7 +44,7 @@ void setup() { //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); - _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); } void loop() { @@ -60,7 +60,7 @@ void loop() { case _myPID.autoTune->TUNINGS: _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID - _myPID.SetSampleTimeUs(10000); // 10ms + _myPID.SetSampleTimeUs(sampleTimeUs); _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID Setpoint = 500; break; @@ -77,13 +77,12 @@ void loop() { if (printOrPlotter == 0) { // plotter Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.println(); + Serial.print("Output:"); Serial.print(Output); Serial.println(","); } Input = _myPID.analogReadFast(inputPin); _myPID.Compute(); analogWrite(outputPin, Output); } - delay(1); // adjust loop speed } float avg(int inputVal) { diff --git a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino index d38b9c1..6ab3c2b 100644 --- a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino +++ b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino @@ -5,9 +5,9 @@ #include "QuickPID.h" +const uint32_t sampleTimeUs = 10000; // 10ms const byte inputPin = 0; const byte outputPin = 3; - const int inputMax = 1023; const int outputMax = 255; const int outputMin = 0; @@ -45,7 +45,7 @@ void setup() { //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); - _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, inputMax - setpoint, output, QuickPID::REVERSE, printOrPlotter); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, inputMax - setpoint, output, QuickPID::REVERSE, printOrPlotter, sampleTimeUs); } void loop() { @@ -61,7 +61,7 @@ void loop() { case _myPID.autoTune->TUNINGS: _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID - _myPID.SetSampleTimeUs(10000); // 10ms + _myPID.SetSampleTimeUs(sampleTimeUs); _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID Setpoint = 500; break; @@ -78,13 +78,12 @@ void loop() { if (printOrPlotter == 0) { // plotter Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.println(); + Serial.print("Output:"); Serial.print(Output); Serial.println(","); } Input = inputMax - _myPID.analogReadFast(inputPin); // reverse acting _myPID.Compute(); analogWrite(outputPin, Output); } - delay(1); // adjust loop speed } float avg(int inputVal) { diff --git a/examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino b/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino similarity index 85% rename from examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino rename to examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino index 46ffa3e..ad426e6 100644 --- a/examples/AutoTune_Filter_TIMER_Mode/AutoTune_Filter_TIMER_Mode.ino +++ b/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino @@ -1,15 +1,14 @@ /****************************************************************************** - AutoTune Filter TIMER Mode Example + AutoTune Interrupt TIMER Example Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter ******************************************************************************/ -#include "NoDelay.h" // https://github.com/M-tech-Creations/NoDelay +#include "TimerOne.h" // https://github.com/PaulStoffregen/TimerOne #include "QuickPID.h" -void runPid(); // declare function before noDelay -noDelay LEDtime(10, runPid); // creates a noDelay varible set to 10ms, will call runPid function +void runPid(); +const uint32_t sampleTimeUs = 10000; // 10ms const byte inputPin = 0; const byte outputPin = 3; - const int outputMax = 255; const int outputMin = 0; @@ -28,6 +27,9 @@ static boolean computeNow = false; QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); void setup() { + Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval + Timer1.attachInterrupt(runPid); // attaches runPid() as a timer overflow interrupt + Serial.begin(115200); Serial.println(); if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { @@ -45,13 +47,10 @@ void setup() { //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); - - _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); } void loop() { - LEDtime.update();//will check if set time has past and if so will run set function - if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() { switch (_myPID.autoTune->autoTuneLoop()) { @@ -59,15 +58,13 @@ void loop() { Input = avg(_myPID.analogReadFast(inputPin)); analogWrite(outputPin, Output); break; - case _myPID.autoTune->TUNINGS: _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings _myPID.SetMode(QuickPID::TIMER); // setup PID - _myPID.SetSampleTimeUs(10000); // 10ms + _myPID.SetSampleTimeUs(sampleTimeUs); _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID Setpoint = 500; break; - case _myPID.autoTune->CLR: if (!pidLoop) { _myPID.clearAutoTune(); // releases memory used by AutoTune object @@ -80,8 +77,7 @@ void loop() { if (printOrPlotter == 0) { // plotter Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.print(","); - Serial.print("iTerm:"); Serial.print(_myPID.GetDterm()); Serial.println(); + Serial.print("Output:"); Serial.print(Output); Serial.println(","); } if (computeNow) { Input = _myPID.analogReadFast(inputPin); @@ -90,7 +86,6 @@ void loop() { computeNow = false; } } - delay(1); // adjust loop speed } void runPid() { diff --git a/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino b/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino new file mode 100644 index 0000000..f8577ef --- /dev/null +++ b/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino @@ -0,0 +1,104 @@ +/****************************************************************************** + AutoTune Software TIMER Example + Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter + ******************************************************************************/ +#include "Ticker.h" // https://github.com/sstaub/Ticker +#include "QuickPID.h" +void runPid(); + +const uint32_t sampleTimeUs = 10000; // 10ms +const byte inputPin = 0; +const byte outputPin = 3; +const int outputMax = 255; +const int outputMin = 0; + +float POn = 1.0; // mix of PonE to PonM (0.0-1.0) +bool printOrPlotter = 0; // on(1) monitor, off(0) plotter +byte outputStep = 5; +byte hysteresis = 1; +int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform +int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform + +float Input, Output, Setpoint; +float Kp = 0, Ki = 0, Kd = 0; +bool pidLoop = false; +static boolean computeNow = false; + +Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); +QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); + +void setup() { + timer1.start(); + Serial.begin(115200); + Serial.println(); + if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { + Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); + while (1); + } + // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki + //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); + _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); + //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); + //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); + //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); + //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); + //_myPID.AutoTune(tuningMethod::AMIGOF_PID); + //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); + //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); + //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); +} + +void loop() { + timer1.update(); + if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() + { + switch (_myPID.autoTune->autoTuneLoop()) { + case _myPID.autoTune->AUTOTUNE: + Input = avg(_myPID.analogReadFast(inputPin)); + analogWrite(outputPin, Output); + break; + case _myPID.autoTune->TUNINGS: + _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings + _myPID.SetMode(QuickPID::TIMER); // setup PID + _myPID.SetSampleTimeUs(sampleTimeUs); + _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + Setpoint = 500; + break; + case _myPID.autoTune->CLR: + if (!pidLoop) { + _myPID.clearAutoTune(); // releases memory used by AutoTune object + pidLoop = true; + } + break; + } + } + if (pidLoop) { + if (printOrPlotter == 0) { // plotter + Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); + Serial.print("Input:"); Serial.print(Input); Serial.print(","); + Serial.print("Output:"); Serial.print(Output); Serial.println(","); + } + if (computeNow) { + Input = _myPID.analogReadFast(inputPin); + _myPID.Compute(); + analogWrite(outputPin, Output); + computeNow = false; + } + } +} + +void runPid() { + computeNow = true; +} + +float avg(int inputVal) { + static int arrDat[16]; + static int pos; + static long sum; + pos++; + if (pos >= 16) pos = 0; + sum = sum - arrDat[pos] + inputVal; + arrDat[pos] = inputVal; + return (float)sum / 16.0; +} diff --git a/examples/Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino b/examples/Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino new file mode 100644 index 0000000..8a5643f --- /dev/null +++ b/examples/Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino @@ -0,0 +1,44 @@ +/******************************************************** + Basic Interrupt TIMER Example + Reading analog input 0 to control analog PWM output 3 + ********************************************************/ +#include "TimerOne.h" // https://github.com/PaulStoffregen/TimerOne +#include "QuickPID.h" + +#define PIN_INPUT 0 +#define PIN_OUTPUT 3 +const uint32_t sampleTimeUs = 100000; // 100ms +static boolean computeNow = false; + +//Define Variables we'll be connecting to +float Setpoint, Input, Output; + +//Specify the links and initial tuning parameters +float Kp = 2, Ki = 5, Kd = 1; + +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); + +void setup() { + Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval + Timer1.attachInterrupt(runPid); // attaches runPid() as a timer overflow interrupt + + //initialize the variables we're linked to + Input = myQuickPID.analogReadFast(PIN_INPUT); + Setpoint = 100; + + //turn the PID on + myQuickPID.SetMode(QuickPID::AUTOMATIC); +} + +void loop() { + if (computeNow) { + Input = myQuickPID.analogReadFast(PIN_INPUT); + myQuickPID.Compute(); + analogWrite(PIN_OUTPUT, Output); + computeNow = false; + } +} + +void runPid() { + computeNow = true; +} diff --git a/examples/Basic_Software_TIMER/Basic_Software_TIMER.ino b/examples/Basic_Software_TIMER/Basic_Software_TIMER.ino new file mode 100644 index 0000000..33e2550 --- /dev/null +++ b/examples/Basic_Software_TIMER/Basic_Software_TIMER.ino @@ -0,0 +1,46 @@ +/******************************************************** + Basic Software TIMER Example + Reading analog input 0 to control analog PWM output 3 + ********************************************************/ +#include "Ticker.h" // https://github.com/sstaub/Ticker +#include "QuickPID.h" +void runPid(); + +#define PIN_INPUT 0 +#define PIN_OUTPUT 3 +const uint32_t sampleTimeUs = 100000; // 100ms +static boolean computeNow = false; + +//Define Variables we'll be connecting to +float Setpoint, Input, Output; + +//Specify the links and initial tuning parameters +float Kp = 2, Ki = 5, Kd = 1; + +Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); + +void setup() { + timer1.start(); + + //initialize the variables we're linked to + Input = myQuickPID.analogReadFast(PIN_INPUT); + Setpoint = 100; + + //turn the PID on + myQuickPID.SetMode(QuickPID::AUTOMATIC); +} + +void loop() { + timer1.update(); + if (computeNow) { + Input = myQuickPID.analogReadFast(PIN_INPUT); + myQuickPID.Compute(); + analogWrite(PIN_OUTPUT, Output); + computeNow = false; + } +} + +void runPid() { + computeNow = true; +} diff --git a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino index 83c31e1..f2e7e25 100644 --- a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino +++ b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino @@ -1,5 +1,5 @@ /******************************************************** - PID Adaptive Tuning Example + QuickPID Adaptive Tuning Example One of the benefits of the PID library is that you can change the tuning parameters at any time. this can be helpful if we want the controller to be agressive at some diff --git a/examples/QuickPID_Basic/QuickPID_Basic.ino b/examples/QuickPID_Basic/QuickPID_Basic.ino index 1aa4ede..99df1c1 100644 --- a/examples/QuickPID_Basic/QuickPID_Basic.ino +++ b/examples/QuickPID_Basic/QuickPID_Basic.ino @@ -1,5 +1,5 @@ /******************************************************** - PID Basic Example + QuickPID Basic Example Reading analog input 0 to control analog PWM output 3 ********************************************************/ diff --git a/examples/QuickPID_PonM/QuickPID_PonM.ino b/examples/QuickPID_PonM/QuickPID_PonM.ino index febf186..c66b601 100644 --- a/examples/QuickPID_PonM/QuickPID_PonM.ino +++ b/examples/QuickPID_PonM/QuickPID_PonM.ino @@ -1,5 +1,5 @@ /******************************************************** - PID Proportional on measurement Example + QuickPID Proportional on measurement Example Setting the PID to use Proportional on measurement will make the output move more smoothly when the setpoint is changed. In addition, it can eliminate overshoot diff --git a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino index 6c13323..6f3f019 100644 --- a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino +++ b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino @@ -1,5 +1,5 @@ /******************************************************** - PID RelayOutput Example + QuickPID RelayOutput Example Same as basic example, except that this time, the output is going to a digital pin which (we presume) is controlling a relay. the pid is designed to Output an analog value, @@ -56,7 +56,7 @@ void loop() /************************************************ turn the output pin on/off based on pid output ************************************************/ - if (millis() - windowStartTime > WindowSize) + if (millis() - windowStartTime >= WindowSize) { //time to shift the Relay Window windowStartTime += WindowSize; myQuickPID.Compute(); diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index d5e3349..8b94809 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -49,8 +49,8 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, **********************************************************************************/ bool QuickPID::Compute() { if (mode == MANUAL) return false; - uint32_t now = micros(); - uint32_t timeChange = (now - lastTime); + uint32_t now = micros(); + uint32_t timeChange = (now - lastTime); if (mode == TIMER || timeChange >= sampleTimeUs) { float input = *myInput; float dInput = input - lastInput; @@ -66,15 +66,15 @@ bool QuickPID::Compute() { pmTerm = kpm * dInput; outputSum -= (kpm * dInput); // subtract proportional on measurement amount - outputSum = CONSTRAIN(outputSum, outMin, outMax); + outputSum = constrain(outputSum, outMin, outMax); float output; peTerm = kpe * error; output = peTerm; // add proportional on error amount dTerm = kd * dInput; - output += outputSum - dTerm; // add derivative amount - output = CONSTRAIN(output, outMin, outMax); + output += outputSum - dTerm; // subtract derivative on input amount + output = constrain(output, outMin, outMax); *myOutput = output; lastInput = input; @@ -131,8 +131,8 @@ void QuickPID::SetOutputLimits(int Min, int Max) { outMax = Max; if (mode != MANUAL) { - *myOutput = CONSTRAIN(*myOutput, outMin, outMax); - outputSum = CONSTRAIN(outputSum, outMin, outMax); + *myOutput = constrain(*myOutput, outMin, outMax); + outputSum = constrain(outputSum, outMin, outMax); } } @@ -155,7 +155,7 @@ void QuickPID::SetMode(mode_t Mode) { void QuickPID::Initialize() { outputSum = *myOutput; lastInput = *myInput; - outputSum = CONSTRAIN(outputSum, outMin, outMax); + outputSum = constrain(outputSum, outMin, outMax); } /* SetControllerDirection(.)************************************************** @@ -240,7 +240,7 @@ void AutoTunePID::reset() { } void AutoTunePID::autoTuneConfig(const byte outputStep, const byte hysteresis, const int atSetpoint, - const int atOutput, const bool dir, const bool printOrPlotter) + const int atOutput, const bool dir, const bool printOrPlotter, uint32_t sampleTimeUs) { _outputStep = outputStep; _hysteresis = hysteresis; @@ -248,10 +248,12 @@ void AutoTunePID::autoTuneConfig(const byte outputStep, const byte hysteresis, c _atOutput = atOutput; _direction = dir; _printOrPlotter = printOrPlotter; + _tLoop = constrain((sampleTimeUs / 16), 500, 10000); _autoTuneStage = STABILIZING; } byte AutoTunePID::autoTuneLoop() { + delayMicroseconds(_tLoop); // small delay improves results (0.5-10ms) switch (_autoTuneStage) { case AUTOTUNE: return AUTOTUNE; @@ -386,7 +388,7 @@ byte AutoTunePID::autoTuneLoop() { return CLR; } -void AutoTunePID::setAutoTuneConstants(float* kp, float* ki, float* kd) { +void AutoTunePID::setAutoTuneConstants(float * kp, float * ki, float * kd) { *kp = _kp; *ki = _ki; *kd = _kd; diff --git a/src/QuickPID.h b/src/QuickPID.h index 7bf85d3..a24591a 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -24,7 +24,7 @@ class AutoTunePID { void reset(); void autoTuneConfig(const byte outputStep, const byte hysteresis, const int setpoint, const int output, - const bool dir = false, const bool printOrPlotter = false); + const bool dir = false, const bool printOrPlotter = false, uint32_t sampleTimeUs = 10000); byte autoTuneLoop(); void setAutoTuneConstants(float* kp, float* ki, float* kd); enum atStage : byte { AUTOTUNE, STABILIZING, COARSE, FINE, TEST, T0, T1, T2, T3L, T3H, CALC, TUNINGS, CLR }; @@ -43,8 +43,7 @@ class AutoTunePID { int _atOutput; bool _direction = false; bool _printOrPlotter = false; - - uint32_t _t0, _t1, _t2, _t3; + uint32_t _tLoop, _t0, _t1, _t2, _t3; float _Ku, _Tu, _td, _kp, _ki, _kd, _rdAvg, _peakHigh, _peakLow; const uint16_t RulesContants[10][3] = @@ -138,7 +137,6 @@ class QuickPID { float iTerm; float dTerm; - float pOn; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 float kp; // (P)roportional Tuning Parameter float ki; // (I)ntegral Tuning Parameter @@ -158,10 +156,6 @@ class QuickPID { float lastInput; bool inAuto; - inline int32_t CONSTRAIN(int32_t x, int32_t lower, int32_t upper) { - return ((x) < (lower) ? (lower) : ((x) > (upper) ? (upper) : (x))); - } - }; // class QuickPID #if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) From 20784219edbb6968ce1dd30a1c051a16306a4452 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 28 May 2021 11:11:41 -0400 Subject: [PATCH 051/101] Update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Faster PID read-compute-write cycle (Arduino UNO): QuickPID = 51µs, PID_v1 = 128µs Updated compute() function New Variable Derivative on Error to Derivative on Measurement parameter DOn Updated PID Query Functions: GetPterm(); GetIterm(); GetDterm(); Updated all examples and documentation --- README.md | 39 ++++++++------- .../AutoTune_Filter_DIRECT.ino | 13 ++--- .../AutoTune_Filter_REVERSE.ino | 13 ++--- .../AutoTune_Interrupt_TIMER.ino | 13 +++-- .../AutoTune_Software_TIMER.ino | 13 +++-- .../QuickPID_AdaptiveTunings.ino | 12 +++-- examples/QuickPID_PDonM/QuickPID_PDonM.ino | 34 ++++++++++++++ examples/QuickPID_PonM/QuickPID_PonM.ino | 33 ------------- .../QuickPID_RelayOutput.ino | 28 +++++------ keywords.txt | 3 +- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 47 +++++++++---------- src/QuickPID.h | 13 +++-- 14 files changed, 141 insertions(+), 124 deletions(-) create mode 100644 examples/QuickPID_PDonM/QuickPID_PDonM.ino delete mode 100644 examples/QuickPID_PonM/QuickPID_PonM.ino diff --git a/README.md b/README.md index c776222..318fd2d 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # QuickPID ![arduino-library-badge](https://camo.githubusercontent.com/989057908f34abd0c8bc2a8d762f86ccebbe377ed9ffef8c3dfdf27a09c6dac9/68747470733a2f2f7777772e617264752d62616467652e636f6d2f62616467652f517569636b5049442e7376673f) -QuickPID is an updated implementation of the Arduino PID library with a built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available is a POn setting that controls the mix of Proportional on Error to Proportional on Measurement. +QuickPID is an updated implementation of the Arduino PID library with a built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available are POn and DOn settings where POn controls the mix of Proportional on Error to Proportional on Measurement and DOn controls the mix of Derivative on Error to Derivative on Measurement. ### Features @@ -8,23 +8,28 @@ Development began with a fork of the Arduino PID Library. Modifications and new #### New feature Summary +- [x] Fast PID read-compute-write cycle (Arduino UNO): QuickPID = **51µs**, PID_v1 = **128µs** - [x] `TIMER` mode for calling PID compute by an external timer function or ISR - [x] `analogReadFast()` support for AVR (4x faster) - [x] `analogWrite()` support for ESP32 and ESP32-S2 -- [x] Variable Proportional on Error Proportional on Measurement parameter `POn` -- [x] Integral windup prevention when output exceeds limits -- [x] New PID query functions that return the P, I and D terms of the calculation -- [x] New AutoTune class added as a dynamic object and includes 10 tuning methods -- [x] AutoTune is compatible with reverse acting controllers -- [x] AutoTune's fast, non-blocking tuning sequence completes in only 1.5 oscillations -- [x] AutoTune determines how easy the process is to control -- [x] AutoTune determines ultimate period `Tu`, dead time `td`, ultimate gain `Ku`, and tuning parameters `Kp, Ki, Kd` +- [x] Variable Proportional on Error to Proportional on Measurement parameter `POn` +- [x] Variable Derivative on Error to Derivative on Measurement parameter `DOn` +- [x] New PID Query Functions: `GetPterm();` `GetIterm();` `GetDterm();` +- [x] Integral windup prevention when output exceeds limits - [x] New REVERSE mode only changes sign of `error` and `dInput` - [x] Uses `float` instead of `double` +#### AutoTune Features + +- [x] New AutoTune class added as a dynamic object and includes 10 tuning methods +- [x] Compatible with reverse acting controllers +- [x] Fast, non-blocking tuning sequence completes in only 1.5 oscillations +- [x] Determines how easy the process is to control +- [x] Determines ultimate period `Tu`, dead time `td`, ultimate gain `Ku`, and tuning parameters `Kp, Ki, Kd` + ### [AutoTune RC Filter](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) -This example allows you to experiment with the AutoTunePID class, various tuning rules and the POn control using ADC and PWM with RC filter. It automatically determines and sets the tuning parameters and works with both DIRECT and REVERSE acting controllers. +This example allows you to experiment with the AutoTunePID class, various tuning rules and the POn and DOn controls using ADC and PWM with RC filter. It automatically determines and sets the tuning parameters and works with both DIRECT and REVERSE acting controllers. #### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) @@ -38,14 +43,15 @@ If a positive error increases the controller's output, the controller is said to ```c++ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, float POn, uint8_t ControllerDirection); + float Kp, float Ki, float Kd, float POn, float DOn, uint8_t ControllerDirection); ``` - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. -- `POn` is the Proportional on Error weighting value with range 0.0-1.0 and default 1.0 (100% Proportional on Error). This controls the mix of Proportional on Error to Proportional on Measurement. +- `POn` controls the mix of Proportional on Error to Proportional on Measurement. Range is 0.0-1.0, default = 1.0 +- `DOn` controls the mix of Derivative on Error to Derivative on Measurement. Range is 0.0-1.0, default = 0.0 -![image](https://user-images.githubusercontent.com/63488701/118383726-986b6e80-b5ce-11eb-94b8-fdbddd4c914e.png) +![POnDOn](https://user-images.githubusercontent.com/63488701/120000053-68de3e00-bfa0-11eb-9db2-04c2f4be76a2.png) - `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. @@ -71,7 +77,7 @@ For examples using AutoTune, please refer to the examples folder. #### SetTunings ```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn); +void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn, float DOn); ``` This function allows the controller's dynamic performance to be adjusted. It's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. The parameters are as described in the constructor. @@ -80,7 +86,7 @@ This function allows the controller's dynamic performance to be adjusted. It's c void QuickPID::SetTunings(float Kp, float Ki, float Kd); ``` -Set Tunings using the last remembered POn setting. +Set Tunings using the last remembered POn and DOn settings. See example [QuickPID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino) #### SetSampleTime @@ -135,8 +141,7 @@ The PID will either be connected to a DIRECT acting process (+Output leads to +I float GetKp(); // proportional gain float GetKi(); // integral gain float GetKd(); // derivative gain - float GetPeTerm(); // proportional on error component of output - float GetPmTerm(); // proportional on measurement component of output + float GetPterm(); // proportional component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output mode_t GetMode(); // MANUAL (0) or AUTOMATIC (1) diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index bbb6dfb..4105908 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -11,19 +11,20 @@ const byte outputPin = 3; const int outputMax = 255; const int outputMin = 0; -float POn = 1.0; // mix of PonE to PonM (0.0-1.0) - bool printOrPlotter = 0; // on(1) monitor, off(0) plotter +float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 +float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 + byte outputStep = 5; byte hysteresis = 1; -int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform -int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform +int setpoint = 341; // 1/3 of range for symetrical waveform +int output = 85; // 1/3 of range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; bool pidLoop = false; -QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); +QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); void setup() { Serial.begin(115200); @@ -61,7 +62,7 @@ void loop() { _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID _myPID.SetSampleTimeUs(sampleTimeUs); - _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID Setpoint = 500; break; diff --git a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino index 6ab3c2b..21cf9a6 100644 --- a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino +++ b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino @@ -12,19 +12,20 @@ const int inputMax = 1023; const int outputMax = 255; const int outputMin = 0; -float POn = 1.0; // mix of PonE to PonM (0.0-1.0) - bool printOrPlotter = 0; // on(1) monitor, off(0) plotter +float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 +float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 + byte outputStep = 5; byte hysteresis = 1; -int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform -int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform +int setpoint = 341; // 1/3 of range for symetrical waveform +int output = 85; // 1/3 of range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; bool pidLoop = false; -QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::REVERSE); +QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::REVERSE); void setup() { Serial.begin(115200); @@ -62,7 +63,7 @@ void loop() { _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID _myPID.SetSampleTimeUs(sampleTimeUs); - _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID Setpoint = 500; break; diff --git a/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino b/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino index ad426e6..93d3248 100644 --- a/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino +++ b/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino @@ -12,19 +12,21 @@ const byte outputPin = 3; const int outputMax = 255; const int outputMin = 0; -float POn = 1.0; // mix of PonE to PonM (0.0-1.0) bool printOrPlotter = 0; // on(1) monitor, off(0) plotter +float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 +float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 + byte outputStep = 5; byte hysteresis = 1; -int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform -int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform +int setpoint = 341; // 1/3 of range for symetrical waveform +int output = 85; // 1/3 of range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; bool pidLoop = false; static boolean computeNow = false; -QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); +QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); void setup() { Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval @@ -47,6 +49,7 @@ void setup() { //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); } @@ -62,7 +65,7 @@ void loop() { _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings _myPID.SetMode(QuickPID::TIMER); // setup PID _myPID.SetSampleTimeUs(sampleTimeUs); - _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID Setpoint = 500; break; case _myPID.autoTune->CLR: diff --git a/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino b/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino index f8577ef..f366469 100644 --- a/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino +++ b/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino @@ -12,12 +12,14 @@ const byte outputPin = 3; const int outputMax = 255; const int outputMin = 0; -float POn = 1.0; // mix of PonE to PonM (0.0-1.0) bool printOrPlotter = 0; // on(1) monitor, off(0) plotter +float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 +float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 + byte outputStep = 5; byte hysteresis = 1; -int setpoint = 341; // 1/3 of 10-bit ADC range for symetrical waveform -int output = 85; // 1/3 of 8-bit PWM range for symetrical waveform +int setpoint = 341; // 1/3 of range for symetrical waveform +int output = 85; // 1/3 of range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; @@ -25,7 +27,7 @@ bool pidLoop = false; static boolean computeNow = false; Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); -QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); +QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); void setup() { timer1.start(); @@ -46,6 +48,7 @@ void setup() { //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); + _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); } @@ -62,7 +65,7 @@ void loop() { _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings _myPID.SetMode(QuickPID::TIMER); // setup PID _myPID.SetSampleTimeUs(sampleTimeUs); - _myPID.SetTunings(Kp, Ki, Kd, POn); // apply new tunings to PID + _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID Setpoint = 500; break; case _myPID.autoTune->CLR: diff --git a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino index f2e7e25..9399d10 100644 --- a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino +++ b/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino @@ -20,11 +20,13 @@ float Setpoint, Input, Output; //Define the aggressive and conservative and POn Tuning Parameters float aggKp = 4, aggKi = 0.2, aggKd = 1; float consKp = 1, consKi = 0.05, consKd = 0.25; -float aggPOn = 1.0; // Range is 0.0 to 1.0 (1.0 is 100% P on Error, 0% P on Measurement) -float consPOn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) +float aggPOn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0) +float consPOn = 0.0; // proportional on Error to Measurement ratio (0.0-1.0) +float aggDOn = 1.0; // derivative on Error to Measurement ratio (0.0-1.0) +float consDOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0) //Specify the links and initial tuning parameters -QuickPID myQuickPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, aggPOn, QuickPID::DIRECT); +QuickPID myQuickPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, aggPOn, consDOn, QuickPID::DIRECT); void setup() { @@ -42,10 +44,10 @@ void loop() float gap = abs(Setpoint - Input); //distance away from setpoint if (gap < 10) { //we're close to setpoint, use conservative tuning parameters - myQuickPID.SetTunings(consKp, consKi, consKd, consPOn); + myQuickPID.SetTunings(consKp, consKi, consKd, consPOn, consDOn); } else { //we're far from setpoint, use aggressive tuning parameters - myQuickPID.SetTunings(aggKp, aggKi, aggKd, aggPOn); + myQuickPID.SetTunings(aggKp, aggKi, aggKd, aggPOn, aggDOn); } myQuickPID.Compute(); analogWrite(PIN_OUTPUT, Output); diff --git a/examples/QuickPID_PDonM/QuickPID_PDonM.ino b/examples/QuickPID_PDonM/QuickPID_PDonM.ino new file mode 100644 index 0000000..7b770d2 --- /dev/null +++ b/examples/QuickPID_PDonM/QuickPID_PDonM.ino @@ -0,0 +1,34 @@ +/************************************************************************************** + QuickPID Proportional-Derivative on Measurement Example + Increasing the proportional on measurement setting will make the output move more + smoothly when the setpoint is changed. Also, it can eliminate overshoot. + Decreasing the derivative on measurement adds more derivative on error. This reduces + reduce overshoot but may increase output spikes. Adjust to suit your requirements. + **************************************************************************************/ + +#include "QuickPID.h" + +//Define Variables we'll be connecting to +float Setpoint, Input, Output; +float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 +float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 + +//Specify the links and initial tuning parameters +QuickPID myQuickPID(&Input, &Output, &Setpoint, 2, 5, 1, POn, DOn, QuickPID::DIRECT); + +void setup() +{ + //initialize the variables we're linked to + Input = myQuickPID.analogReadFast(0); + Setpoint = 100; + + //turn the PID on + myQuickPID.SetMode(QuickPID::AUTOMATIC); +} + +void loop() +{ + Input = myQuickPID.analogReadFast(0); + myQuickPID.Compute(); + analogWrite(3, Output); +} diff --git a/examples/QuickPID_PonM/QuickPID_PonM.ino b/examples/QuickPID_PonM/QuickPID_PonM.ino deleted file mode 100644 index c66b601..0000000 --- a/examples/QuickPID_PonM/QuickPID_PonM.ino +++ /dev/null @@ -1,33 +0,0 @@ -/******************************************************** - QuickPID Proportional on measurement Example - Setting the PID to use Proportional on measurement will - make the output move more smoothly when the setpoint - is changed. In addition, it can eliminate overshoot - in certain processes like sous-vides. - ********************************************************/ - -#include "QuickPID.h" - -//Define Variables we'll be connecting to -float Setpoint, Input, Output; -float POn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) - -//Specify the links and initial tuning parameters -QuickPID myQuickPID(&Input, &Output, &Setpoint, 2, 5, 1, POn, QuickPID::DIRECT); - -void setup() -{ - //initialize the variables we're linked to - Input = myQuickPID.analogReadFast(0); - Setpoint = 100; - - //turn the PID on - myQuickPID.SetMode(QuickPID::AUTOMATIC); -} - -void loop() -{ - Input = myQuickPID.analogReadFast(0); - myQuickPID.Compute(); - analogWrite(3, Output); -} diff --git a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino index 6f3f019..c3836b1 100644 --- a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino +++ b/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino @@ -1,20 +1,19 @@ -/******************************************************** +/************************************************************* QuickPID RelayOutput Example Same as basic example, except that this time, the output is going to a digital pin which (we presume) is controlling - a relay. the pid is designed to Output an analog value, + a relay. The pid is designed to Output an analog value, but the relay can only be On/Off. - to connect them together we use "time proportioning - control" it's essentially a really slow version of PWM. - first we decide on a window size (5000mS say.) we then - set the pid to adjust its output between 0 and that window - size. lastly, we add some logic that translates the PID - output into "Relay On Time" with the remainder of the - window being "Relay Off Time" - The minWindow setting is a floor so that the relay would - be on for a minimum amount of time. - ********************************************************/ + To connect them together we use "time proportioning + control", essentially a really slow version of PWM. + First we decide on a window size (5000mS for example). + We then set the pid to adjust its output between 0 and that + window size. Lastly, we add some logic that translates the + PID output into "Relay On Time" with the remainder of the + window being "Relay Off Time". The minWindow setting is a + floor (minimum time) the relay would be on. + *************************************************************/ #include "QuickPID.h" @@ -26,9 +25,10 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -float POn = 0.0; // Range is 0.0 to 1.0 (0.0 is 0% P on Error, 100% P on Measurement) +float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 +float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, QuickPID::DIRECT); +QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); unsigned int WindowSize = 5000; unsigned int minWindow = 500; diff --git a/keywords.txt b/keywords.txt index f7f398e..886db57 100644 --- a/keywords.txt +++ b/keywords.txt @@ -23,8 +23,7 @@ SetSampleTimeUs KEYWORD2 GetKp KEYWORD2 GetKi KEYWORD2 GetKd KEYWORD2 -GetPeTerm KEYWORD2 -GetPmTerm KEYWORD2 +GetPterm KEYWORD2 GetIterm KEYWORD2 GetDterm KEYWORD2 GetMode KEYWORD2 diff --git a/library.json b/library.json index 257f37d..9704678 100644 --- a/library.json +++ b/library.json @@ -3,7 +3,7 @@ "keywords": "PID, controller, signal", "description": "A fast PID controller with AutoTune and 10 tuning rules. Compatible with most Arduino and ESP32 boards.", "license": "MIT", - "version": "2.3.3", + "version": "2.4.0", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index f45599b..8f6468f 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.3.3 +version=2.4.0 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with AutoTune and 10 tuning rules. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 8b94809..d743840 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.3.3 + QuickPID Library for Arduino - Version 2.4.0 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. @@ -18,7 +18,7 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, float POn = 1, + float Kp, float Ki, float Kd, float POn = 1.0, float DOn = 0.0, QuickPID::direction_t ControllerDirection = DIRECT) { myOutput = Output; @@ -29,7 +29,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, QuickPID::SetOutputLimits(0, 255); // same default as Arduino PWM limit sampleTimeUs = 100000; // 0.1 sec default QuickPID::SetControllerDirection(ControllerDirection); - QuickPID::SetTunings(Kp, Ki, Kd, POn); + QuickPID::SetTunings(Kp, Ki, Kd, POn, DOn); lastTime = micros() - sampleTimeUs; } @@ -39,7 +39,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, direction_t ControllerDirection = DIRECT) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, ControllerDirection = DIRECT) { + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, dOn = 1, ControllerDirection = DIRECT) { } /* Compute() *********************************************************************** @@ -59,21 +59,21 @@ bool QuickPID::Compute() { error = -error; dInput = -dInput; } + pmTerm = kpm * dInput; + peTerm = kpe * error; iTerm = ki * error; - outputSum += iTerm; // add integral error amount - if (outputSum > outMax) iTerm -= outputSum - outMax; // prevents integral windup + dmTerm = kdm * dInput; + deTerm = -kde * error; + + outputSum += iTerm; + if (outputSum > outMax) iTerm -= outputSum - outMax; // prevent integral windup else if (outputSum < outMin) iTerm += outMin - outputSum; - pmTerm = kpm * dInput; - outputSum -= (kpm * dInput); // subtract proportional on measurement amount + float output = peTerm; + outputSum -= pmTerm; outputSum = constrain(outputSum, outMin, outMax); - float output; - peTerm = kpe * error; - output = peTerm; // add proportional on error amount - - dTerm = kd * dInput; - output += outputSum - dTerm; // subtract derivative on input amount + output += outputSum - deTerm + dmTerm; output = constrain(output, outMin, outMax); *myOutput = output; @@ -89,24 +89,26 @@ bool QuickPID::Compute() { it's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. ******************************************************************************/ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1) { +void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1.0, float DOn = 0.0) { if (Kp < 0 || Ki < 0 || Kd < 0) return; pOn = POn; + dOn = DOn; dispKp = Kp; dispKi = Ki; dispKd = Kd; - float SampleTimeSec = (float)sampleTimeUs / 1000000; kp = Kp; ki = Ki * SampleTimeSec; kd = Kd / SampleTimeSec; kpe = kp * pOn; kpm = kp * (1 - pOn); + kde = kp * dOn; + kdm = kp * (1 - dOn); } /* SetTunings(...)************************************************************ - Set Tunings using the last remembered POn setting. + Set Tunings using the last remembered POn and DOn setting. ******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { - SetTunings(Kp, Ki, Kd, pOn); + SetTunings(Kp, Ki, Kd, pOn, dOn); } /* SetSampleTime(.)*********************************************************** @@ -179,17 +181,14 @@ float QuickPID::GetKi() { float QuickPID::GetKd() { return dispKd; } -float QuickPID::GetPeTerm() { - return peTerm; -} -float QuickPID::GetPmTerm() { - return pmTerm; +float QuickPID::GetPterm() { + return peTerm + pmTerm; } float QuickPID::GetIterm() { return iTerm; } float QuickPID::GetDterm() { - return dTerm; + return deTerm + dmTerm; } QuickPID::mode_t QuickPID::GetMode() { return mode; diff --git a/src/QuickPID.h b/src/QuickPID.h index a24591a..db5228f 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -75,7 +75,7 @@ class QuickPID { // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, float POn, direction_t ControllerDirection); + QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, float POn, float DOn, direction_t ControllerDirection); // Overload constructor with proportional ratio. Links the PID to Input, Output, Setpoint and Tuning Parameters. QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, direction_t ControllerDirection); @@ -101,7 +101,7 @@ class QuickPID { void SetTunings(float Kp, float Ki, float Kd); // Overload for specifying proportional ratio. - void SetTunings(float Kp, float Ki, float Kd, float POn); + void SetTunings(float Kp, float Ki, float Kd, float POn, float DOn); // Sets the controller Direction or Action. DIRECT means the output will increase when the error is positive. // REVERSE means the output will decrease when the error is positive. @@ -114,8 +114,7 @@ class QuickPID { float GetKp(); // proportional gain float GetKi(); // integral gain float GetKd(); // derivative gain - float GetPeTerm(); // proportional on error component of output - float GetPmTerm(); // proportional on measurement component of output + float GetPterm(); // proportional component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output mode_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) @@ -135,14 +134,18 @@ class QuickPID { float peTerm; float pmTerm; float iTerm; - float dTerm; + float deTerm; + float dmTerm; float pOn; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 + float dOn; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 float kp; // (P)roportional Tuning Parameter float ki; // (I)ntegral Tuning Parameter float kd; // (D)erivative Tuning Parameter float kpe; // proportional on error amount float kpm; // proportional on measurement amount + float kde; // derivative on error amount + float kdm; // derivative on measurement amount float *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a float *myOutput; // hard link between the variables and the PID, freeing the user from having From 383bc2e054c1f50b2a31a54f60cf50ac81d71e1b Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 29 May 2021 16:33:23 -0400 Subject: [PATCH 052/101] Update Minor autoTune update --- README.md | 2 +- src/QuickPID.cpp | 15 ++++++++++----- src/QuickPID.h | 4 ++-- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 318fd2d..b43bf69 100644 --- a/README.md +++ b/README.md @@ -144,7 +144,7 @@ The PID will either be connected to a DIRECT acting process (+Output leads to +I float GetPterm(); // proportional component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output - mode_t GetMode(); // MANUAL (0) or AUTOMATIC (1) + mode_t GetMode(); // MANUAL (0) or AUTOMATIC (1) or TIMER (2) direction_t GetDirection(); // DIRECT (0) or REVERSE (1) ``` diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index d743840..4b1c925 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -222,6 +222,7 @@ AutoTunePID::AutoTunePID(float *input, float *output, tuningMethod tuningRule) { } void AutoTunePID::reset() { + _tLast = 0; _t0 = 0; _t1 = 0; _t2 = 0; @@ -247,16 +248,20 @@ void AutoTunePID::autoTuneConfig(const byte outputStep, const byte hysteresis, c _atOutput = atOutput; _direction = dir; _printOrPlotter = printOrPlotter; - _tLoop = constrain((sampleTimeUs / 16), 500, 10000); + _tLoop = constrain((sampleTimeUs / 8), 500, 16383); _autoTuneStage = STABILIZING; } byte AutoTunePID::autoTuneLoop() { - delayMicroseconds(_tLoop); // small delay improves results (0.5-10ms) + if ((micros() - _tLast) <= _tLoop) return WAIT; + else _tLast = micros(); switch (_autoTuneStage) { case AUTOTUNE: return AUTOTUNE; break; + case WAIT: + return WAIT; + break; case STABILIZING: if (_printOrPlotter == 1) Serial.print(F("Stabilizing →")); _t0 = millis(); @@ -353,9 +358,9 @@ byte AutoTunePID::autoTuneLoop() { _ki = _kp / Ti; _kd = Td * _kp; } else { //other rules - _kp = RulesContants[(int)_tuningRule][0] / 1000.0 * _Ku; - _ki = RulesContants[(int)_tuningRule][1] / 1000.0 * _Ku / _Tu; - _kd = RulesContants[(int)_tuningRule][2] / 1000.0 * _Ku * _Tu; + _kp = RulesContants[static_cast(_tuningRule)][0] / 1000.0 * _Ku; + _ki = RulesContants[static_cast(_tuningRule)][1] / 1000.0 * _Ku / _Tu; + _kd = RulesContants[static_cast(_tuningRule)][2] / 1000.0 * _Ku * _Tu; } if (_printOrPlotter == 1) { // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png diff --git a/src/QuickPID.h b/src/QuickPID.h index db5228f..fe086c2 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -27,7 +27,7 @@ class AutoTunePID { const bool dir = false, const bool printOrPlotter = false, uint32_t sampleTimeUs = 10000); byte autoTuneLoop(); void setAutoTuneConstants(float* kp, float* ki, float* kd); - enum atStage : byte { AUTOTUNE, STABILIZING, COARSE, FINE, TEST, T0, T1, T2, T3L, T3H, CALC, TUNINGS, CLR }; + enum atStage : byte { AUTOTUNE, WAIT, STABILIZING, COARSE, FINE, TEST, T0, T1, T2, T3L, T3H, CALC, TUNINGS, CLR }; private: @@ -43,7 +43,7 @@ class AutoTunePID { int _atOutput; bool _direction = false; bool _printOrPlotter = false; - uint32_t _tLoop, _t0, _t1, _t2, _t3; + uint32_t _tLoop, _tLast, _t0, _t1, _t2, _t3; float _Ku, _Tu, _td, _kp, _ki, _kd, _rdAvg, _peakHigh, _peakLow; const uint16_t RulesContants[10][3] = From 5846cf432044d61313db6d5a44dc5918f57a860a Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 29 May 2021 23:45:54 -0400 Subject: [PATCH 053/101] Update More descriptive example names --- .../AutoTune_AVR_Interrupt_TIMER.ino} | 2 +- .../AutoTune_AVR_Software_TIMER.ino} | 2 +- .../PID_AVR_Basic_Interrupt_TIMER.ino} | 2 +- .../PID_AVR_Basic_Software_TIMER.ino} | 2 +- .../PID_AdaptiveTunings.ino} | 2 +- .../QuickPID_Basic.ino => PID_Basic/PID_Basic.ino} | 2 +- .../PID_POn_DOn_Error_Measurement.ino} | 2 +- .../PID_RelayOutput.ino} | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) rename examples/{AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino => AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino} (98%) rename examples/{AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino => AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino} (98%) rename examples/{Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino => PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino} (96%) rename examples/{Basic_Software_TIMER/Basic_Software_TIMER.ino => PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino} (96%) rename examples/{QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino => PID_AdaptiveTunings/PID_AdaptiveTunings.ino} (98%) rename examples/{QuickPID_Basic/QuickPID_Basic.ino => PID_Basic/PID_Basic.ino} (96%) rename examples/{QuickPID_PDonM/QuickPID_PDonM.ino => PID_POn_DOn_Error_Measurement/PID_POn_DOn_Error_Measurement.ino} (94%) rename examples/{QuickPID_RelayOutput/QuickPID_RelayOutput.ino => PID_RelayOutput/PID_RelayOutput.ino} (98%) diff --git a/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino b/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino similarity index 98% rename from examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino rename to examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino index 93d3248..64edc2d 100644 --- a/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino +++ b/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino @@ -1,5 +1,5 @@ /****************************************************************************** - AutoTune Interrupt TIMER Example + AutoTune AVR Interrupt TIMER Example Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter ******************************************************************************/ #include "TimerOne.h" // https://github.com/PaulStoffregen/TimerOne diff --git a/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino b/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino similarity index 98% rename from examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino rename to examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino index f366469..32f9576 100644 --- a/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino +++ b/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino @@ -1,5 +1,5 @@ /****************************************************************************** - AutoTune Software TIMER Example + AutoTune AVR Software TIMER Example Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter ******************************************************************************/ #include "Ticker.h" // https://github.com/sstaub/Ticker diff --git a/examples/Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino similarity index 96% rename from examples/Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino rename to examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino index 8a5643f..56d8025 100644 --- a/examples/Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino +++ b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino @@ -1,5 +1,5 @@ /******************************************************** - Basic Interrupt TIMER Example + PID AVR Basic Interrupt TIMER Example Reading analog input 0 to control analog PWM output 3 ********************************************************/ #include "TimerOne.h" // https://github.com/PaulStoffregen/TimerOne diff --git a/examples/Basic_Software_TIMER/Basic_Software_TIMER.ino b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino similarity index 96% rename from examples/Basic_Software_TIMER/Basic_Software_TIMER.ino rename to examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino index 33e2550..7c49e50 100644 --- a/examples/Basic_Software_TIMER/Basic_Software_TIMER.ino +++ b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino @@ -1,5 +1,5 @@ /******************************************************** - Basic Software TIMER Example + PID AVR Basic Software TIMER Example Reading analog input 0 to control analog PWM output 3 ********************************************************/ #include "Ticker.h" // https://github.com/sstaub/Ticker diff --git a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino similarity index 98% rename from examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino rename to examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino index 9399d10..c12756b 100644 --- a/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino +++ b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino @@ -1,5 +1,5 @@ /******************************************************** - QuickPID Adaptive Tuning Example + PID Adaptive Tuning Example One of the benefits of the PID library is that you can change the tuning parameters at any time. this can be helpful if we want the controller to be agressive at some diff --git a/examples/QuickPID_Basic/QuickPID_Basic.ino b/examples/PID_Basic/PID_Basic.ino similarity index 96% rename from examples/QuickPID_Basic/QuickPID_Basic.ino rename to examples/PID_Basic/PID_Basic.ino index 99df1c1..1aa4ede 100644 --- a/examples/QuickPID_Basic/QuickPID_Basic.ino +++ b/examples/PID_Basic/PID_Basic.ino @@ -1,5 +1,5 @@ /******************************************************** - QuickPID Basic Example + PID Basic Example Reading analog input 0 to control analog PWM output 3 ********************************************************/ diff --git a/examples/QuickPID_PDonM/QuickPID_PDonM.ino b/examples/PID_POn_DOn_Error_Measurement/PID_POn_DOn_Error_Measurement.ino similarity index 94% rename from examples/QuickPID_PDonM/QuickPID_PDonM.ino rename to examples/PID_POn_DOn_Error_Measurement/PID_POn_DOn_Error_Measurement.ino index 7b770d2..9ff0daa 100644 --- a/examples/QuickPID_PDonM/QuickPID_PDonM.ino +++ b/examples/PID_POn_DOn_Error_Measurement/PID_POn_DOn_Error_Measurement.ino @@ -1,5 +1,5 @@ /************************************************************************************** - QuickPID Proportional-Derivative on Measurement Example + Proportional and Derivative on the ratio of Error to Measurement Example Increasing the proportional on measurement setting will make the output move more smoothly when the setpoint is changed. Also, it can eliminate overshoot. Decreasing the derivative on measurement adds more derivative on error. This reduces diff --git a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino b/examples/PID_RelayOutput/PID_RelayOutput.ino similarity index 98% rename from examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino rename to examples/PID_RelayOutput/PID_RelayOutput.ino index c3836b1..43476f2 100644 --- a/examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino +++ b/examples/PID_RelayOutput/PID_RelayOutput.ino @@ -1,5 +1,5 @@ /************************************************************* - QuickPID RelayOutput Example + PID Relay Output Example Same as basic example, except that this time, the output is going to a digital pin which (we presume) is controlling a relay. The pid is designed to Output an analog value, From 953b3f13b43e7022370351e742e98b0b2b29488b Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 30 May 2021 09:24:06 -0400 Subject: [PATCH 054/101] Update README.md --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index b43bf69..c314ff7 100644 --- a/README.md +++ b/README.md @@ -86,7 +86,7 @@ This function allows the controller's dynamic performance to be adjusted. It's c void QuickPID::SetTunings(float Kp, float Ki, float Kd); ``` -Set Tunings using the last remembered POn and DOn settings. See example [QuickPID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino) +Set Tunings using the last remembered POn and DOn settings. See example [PID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino) #### SetSampleTime @@ -114,10 +114,10 @@ Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (1) or `TIME `TIMER` mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See examples: -- [AutoTune_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino) -- [AutoTune_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino) -- [Basic_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/Basic_Interrupt_TIMER/Basic_Interrupt_TIMER.ino) -- [Basic_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/Basic_Software_TIMER/Basic_Software_TIMER.ino) +- [AutoTune_AVR_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino) +- [AutoTune_AVR_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino) +- [PID_AVR_Basic_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino) +- [PID_AVR_Basic_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino) #### Initialize From bae82995ed4d8a1749326ee3fa4e8a4ede6286ec Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 30 May 2021 11:52:51 -0400 Subject: [PATCH 055/101] Update Version update --- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/library.json b/library.json index 9704678..67d7997 100644 --- a/library.json +++ b/library.json @@ -3,7 +3,7 @@ "keywords": "PID, controller, signal", "description": "A fast PID controller with AutoTune and 10 tuning rules. Compatible with most Arduino and ESP32 boards.", "license": "MIT", - "version": "2.4.0", + "version": "2.4.1", "url": "https://github.com/Dlloydev/QuickPID", "include": "QuickPID", "authors": diff --git a/library.properties b/library.properties index 8f6468f..2209e83 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.4.0 +version=2.4.1 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with AutoTune and 10 tuning rules. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 4b1c925..a94e24e 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.0 + QuickPID Library for Arduino - Version 2.4.1 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. From d264af5aa989f4d3772829267dcfd3c4dc9687d8 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 1 Jun 2021 14:06:31 -0400 Subject: [PATCH 056/101] Update Assorted minor updates --- README.md | 16 +++++------- .../AutoTune_AVR_Interrupt_TIMER.ino | 2 +- .../PID_AVR_Basic_Interrupt_TIMER.ino | 2 +- library.json | 26 ++++++++++++------- library.properties | 8 +++--- src/QuickPID.cpp | 10 +++---- src/QuickPID.h | 2 +- 7 files changed, 35 insertions(+), 31 deletions(-) diff --git a/README.md b/README.md index c314ff7..cd5f6cc 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,8 @@ QuickPID is an updated implementation of the Arduino PID library with a built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available are POn and DOn settings where POn controls the mix of Proportional on Error to Proportional on Measurement and DOn controls the mix of Derivative on Error to Derivative on Measurement. +#### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) + ### Features Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the [change log](https://github.com/Dlloydev/QuickPID/wiki/Change-Log). @@ -27,15 +29,15 @@ Development began with a fork of the Arduino PID Library. Modifications and new - [x] Determines how easy the process is to control - [x] Determines ultimate period `Tu`, dead time `td`, ultimate gain `Ku`, and tuning parameters `Kp, Ki, Kd` -### [AutoTune RC Filter](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) +### [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) -This example allows you to experiment with the AutoTunePID class, various tuning rules and the POn and DOn controls using ADC and PWM with RC filter. It automatically determines and sets the tuning parameters and works with both DIRECT and REVERSE acting controllers. +#### [AutoTune Filter Examples](https://github.com/Dlloydev/QuickPID/wiki/AutoTune-Filter-Examples) -#### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) +The examples [AutoTune_Filter_DIRECT.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino) and [AutoTune_Filter_REVERSE.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino) allow you to experiment with the AutoTunePID class, various tuning rules and the POn and DOn controls using ADC and PWM with RC filter. It automatically determines and sets the tuning parameters and works with both `DIRECT` and `REVERSE` acting controllers. -### Direct and Reverse Controller Action +#### Direct and Reverse Controller Action -If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). See the examples [AutoTune_Filter_DIRECT.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino) and [AutoTune_Filter_REVERSE.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino) for details. +If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). ### Functions @@ -70,10 +72,6 @@ bool QuickPID::Compute(); This function contains the PID algorithm and it should be called once every loop(). Most of the time it will just return false without doing anything. However, at a frequency specified by `SetSampleTime` it will calculate a new Output and return true. -#### [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) - -For examples using AutoTune, please refer to the examples folder. - #### SetTunings ```c++ diff --git a/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino b/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino index 64edc2d..5c6a1f8 100644 --- a/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino +++ b/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino @@ -24,7 +24,7 @@ int output = 85; // 1/3 of range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; bool pidLoop = false; -static boolean computeNow = false; +volatile bool computeNow = false; QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); diff --git a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino index 56d8025..e8632b2 100644 --- a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino +++ b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino @@ -8,7 +8,7 @@ #define PIN_INPUT 0 #define PIN_OUTPUT 3 const uint32_t sampleTimeUs = 100000; // 100ms -static boolean computeNow = false; +volatile bool computeNow = false; //Define Variables we'll be connecting to float Setpoint, Input, Output; diff --git a/library.json b/library.json index 67d7997..59654b5 100644 --- a/library.json +++ b/library.json @@ -1,21 +1,27 @@ { "name": "QuickPID", + "version": "2.4.2", + "description": "A fast PID controller with an AutoTune dynamic object, 10 tunung rules, Integral anti-windup, TIMER Mode, variable Proportional and Derivative on Error to Measurement and full-featured Arduino analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", - "description": "A fast PID controller with AutoTune and 10 tuning rules. Compatible with most Arduino and ESP32 boards.", - "license": "MIT", - "version": "2.4.1", - "url": "https://github.com/Dlloydev/QuickPID", - "include": "QuickPID", + "repository": + { + "type": "git", + "url": "https://github.com/Dlloydev/QuickPID" + }, "authors": [ { "name": "David Lloyd" + "email": "dlloydev@testcor.ca", + "url": "https://github.com/Dlloydev/QuickPID", + "maintainer": true } ], - "repository": - { - "type": "git", - "url": "https://github.com/Dlloydev/QuickPID" + "license": "MIT", + "homepage": "https://github.com/Dlloydev/QuickPID", + "dependencies": { + "QuickPID": "~2.4.2" }, - "frameworks": "arduino" + "frameworks": "*", + "platforms": "*" } diff --git a/library.properties b/library.properties index 2209e83..da23c3f 100644 --- a/library.properties +++ b/library.properties @@ -1,10 +1,10 @@ name=QuickPID -version=2.4.1 +version=2.4.2 author=David Lloyd maintainer=David Lloyd -sentence=A fast PID controller with AutoTune and 10 tuning rules. -paragraph=This controller can automatically determine and set tuning parameters. Compatible with most Arduino and ESP32 boards. +sentence=A fast PID controller with AutoTune, integral anti-windup and variable controls for Proportional and Derivative on Error to Measurement. +paragraph=A fast PID controller with an AutoTune dynamic object, 10 tunung rules, Integral anti-windup, TIMER Mode, variable Proportional and Derivative on Error to Measurement and full-featured Arduino analogWrite compatibility for ESP32 and ESP32-S2. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID architectures=* -includes=QuickPID.h +includes=QuickPID.h,analogWrite.h diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index a94e24e..46788fd 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.1 + QuickPID Library for Arduino - Version 2.4.2 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. @@ -66,7 +66,7 @@ bool QuickPID::Compute() { deTerm = -kde * error; outputSum += iTerm; - if (outputSum > outMax) iTerm -= outputSum - outMax; // prevent integral windup + if (outputSum > outMax) iTerm -= outputSum - outMax; // integral anti-windup else if (outputSum < outMin) iTerm += outMin - outputSum; float output = peTerm; @@ -358,9 +358,9 @@ byte AutoTunePID::autoTuneLoop() { _ki = _kp / Ti; _kd = Td * _kp; } else { //other rules - _kp = RulesContants[static_cast(_tuningRule)][0] / 1000.0 * _Ku; - _ki = RulesContants[static_cast(_tuningRule)][1] / 1000.0 * _Ku / _Tu; - _kd = RulesContants[static_cast(_tuningRule)][2] / 1000.0 * _Ku * _Tu; + _kp = (float)(RulesContants[static_cast(_tuningRule)][0] / 1000.0) * _Ku; + _ki = (float)(RulesContants[static_cast(_tuningRule)][1] / 1000.0) * (_Ku / _Tu); + _kd = (float)(RulesContants[static_cast(_tuningRule)][2] / 1000.0) * (_Ku * _Tu); } if (_printOrPlotter == 1) { // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png diff --git a/src/QuickPID.h b/src/QuickPID.h index fe086c2..96bd6a6 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -57,7 +57,7 @@ class AutoTunePID { { 0, 0, 0 }, // AMIGOF_PID { 700, 1750, 105 }, // PESSEN_INTEGRAL_PID { 333, 667, 111 }, // SOME_OVERSHOOT_PID - { 200, 400, 67 }, // NO_OVERSHOOT_PID + { 333, 100, 67 } // NO_OVERSHOOT_PID }; }; // class AutoTunePID From 1344cfad414dbd502446b077b2de72c18e06cfb4 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 1 Jun 2021 14:54:56 -0400 Subject: [PATCH 057/101] Update library.json --- library.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.json b/library.json index 59654b5..f08d9a4 100644 --- a/library.json +++ b/library.json @@ -11,7 +11,7 @@ "authors": [ { - "name": "David Lloyd" + "name": "David Lloyd", "email": "dlloydev@testcor.ca", "url": "https://github.com/Dlloydev/QuickPID", "maintainer": true From 5817808931e681f1a40ef2dadcf506c1c9ae767a Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 1 Jun 2021 14:59:44 -0400 Subject: [PATCH 058/101] Update Update to include updated library.json --- library.json | 4 ++-- library.properties | 2 +- src/QuickPID.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/library.json b/library.json index f08d9a4..578c5f8 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "2.4.2", + "version": "2.4.3", "description": "A fast PID controller with an AutoTune dynamic object, 10 tunung rules, Integral anti-windup, TIMER Mode, variable Proportional and Derivative on Error to Measurement and full-featured Arduino analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~2.4.2" + "QuickPID": "~2.4.3" }, "frameworks": "*", "platforms": "*" diff --git a/library.properties b/library.properties index da23c3f..ecae974 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.4.2 +version=2.4.3 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with AutoTune, integral anti-windup and variable controls for Proportional and Derivative on Error to Measurement. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 46788fd..a854593 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.2 + QuickPID Library for Arduino - Version 2.4.3 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. From a8cf244565ebd25a93e6b3b75e7a92f2444e7b43 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 7 Jun 2021 23:23:50 -0400 Subject: [PATCH 059/101] Update - Now using 2-stage integral anti-windup strategy - Better organized compute code - Some cleanup and documentation updates --- README.md | 2 +- library.json | 6 +++--- library.properties | 7 +++---- src/QuickPID.cpp | 33 ++++++++------------------------- 4 files changed, 15 insertions(+), 33 deletions(-) diff --git a/README.md b/README.md index cd5f6cc..e6e9b67 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ Development began with a fork of the Arduino PID Library. Modifications and new - [x] Variable Proportional on Error to Proportional on Measurement parameter `POn` - [x] Variable Derivative on Error to Derivative on Measurement parameter `DOn` - [x] New PID Query Functions: `GetPterm();` `GetIterm();` `GetDterm();` -- [x] Integral windup prevention when output exceeds limits +- [x] 2-stage Integral windup prevention when output exceeds limits - [x] New REVERSE mode only changes sign of `error` and `dInput` - [x] Uses `float` instead of `double` diff --git a/library.json b/library.json index 578c5f8..9362daf 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", - "version": "2.4.3", - "description": "A fast PID controller with an AutoTune dynamic object, 10 tunung rules, Integral anti-windup, TIMER Mode, variable Proportional and Derivative on Error to Measurement and full-featured Arduino analogWrite compatibility for ESP32 and ESP32-S2.", + "version": "2.4.4", + "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": { @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~2.4.3" + "QuickPID": "~2.4.4" }, "frameworks": "*", "platforms": "*" diff --git a/library.properties b/library.properties index ecae974..adbce13 100644 --- a/library.properties +++ b/library.properties @@ -1,10 +1,9 @@ name=QuickPID -version=2.4.3 +version=2.4.4 author=David Lloyd maintainer=David Lloyd -sentence=A fast PID controller with AutoTune, integral anti-windup and variable controls for Proportional and Derivative on Error to Measurement. -paragraph=A fast PID controller with an AutoTune dynamic object, 10 tunung rules, Integral anti-windup, TIMER Mode, variable Proportional and Derivative on Error to Measurement and full-featured Arduino analogWrite compatibility for ESP32 and ESP32-S2. +sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. +paragraph=Also includes analogWrite compatibility for ESP32 and ESP32-S2. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID architectures=* -includes=QuickPID.h,analogWrite.h diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index a854593..3948bce 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.3 + QuickPID Library for Arduino - Version 2.4.4 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. @@ -65,18 +65,13 @@ bool QuickPID::Compute() { dmTerm = kdm * dInput; deTerm = -kde * error; - outputSum += iTerm; - if (outputSum > outMax) iTerm -= outputSum - outMax; // integral anti-windup - else if (outputSum < outMin) iTerm += outMin - outputSum; + outputSum += iTerm; // include integral amount + if (outputSum > outMax) outputSum -= outputSum - outMax; // early integral anti-windup at outMax + else if (outputSum < outMin) outputSum += outMin - outputSum; // early integral anti-windup at outMin + outputSum = constrain(outputSum, outMin, outMax); // integral anti-windup clamp + outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp + *myOutput = constrain(outputSum + peTerm + dmTerm - deTerm, outMin, outMax); // totalize, clamp and drive the output - float output = peTerm; - outputSum -= pmTerm; - outputSum = constrain(outputSum, outMin, outMax); - - output += outputSum - deTerm + dmTerm; - output = constrain(output, outMin, outMax); - - *myOutput = output; lastInput = input; lastTime = now; return true; @@ -399,7 +394,7 @@ void AutoTunePID::setAutoTuneConstants(float * kp, float * ki, float * kd) { } /* Utility************************************************************/ - +// https://github.com/avandalen/avdweb_AnalogReadFast int QuickPID::analogReadFast(int ADCpin) { #if defined(__AVR_ATmega328P__) byte ADCregOriginal = ADCSRA; @@ -407,18 +402,6 @@ int QuickPID::analogReadFast(int ADCpin) { int adc = analogRead(ADCpin); ADCSRA = ADCregOriginal; return adc; -#elif defined(__AVR_ATtiny_Zero_One__) || defined(__AVR_ATmega_Zero__) - byte ADCregOriginal = ADC0_CTRLC; - ADC0_CTRLC = 0x54; // reduced cap, Vdd ref, 32 prescaler - int adc = analogRead(ADCpin); - ADC0_CTRLC = ADCregOriginal; - return adc; -#elif defined(__AVR_DA__) - byte ADCregOriginal = ADC0.CTRLC; - ADC0.CTRLC = ADC_PRESC_DIV32_gc; // 32 prescaler - int adc = analogRead(ADCpin); - ADC0.CTRLC = ADCregOriginal; - return adc; #else return analogRead(ADCpin); # endif From 1626eef57f54d00eddb6f55c042adea4837661ad Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 15 Jun 2021 00:04:11 -0400 Subject: [PATCH 060/101] Update - autoTune parameters `outputStep`, `hysteresis`, `Setpoint` and `Output` are now float - minimum `outputStep` is now 0 (previously 5). --- README.md | 2 +- library.json | 4 ++-- library.properties | 2 +- src/QuickPID.cpp | 14 +++++++------- src/QuickPID.h | 7 ++----- 5 files changed, 13 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index e6e9b67..28d3429 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# QuickPID ![arduino-library-badge](https://camo.githubusercontent.com/989057908f34abd0c8bc2a8d762f86ccebbe377ed9ffef8c3dfdf27a09c6dac9/68747470733a2f2f7777772e617264752d62616467652e636f6d2f62616467652f517569636b5049442e7376673f) +# QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) QuickPID is an updated implementation of the Arduino PID library with a built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available are POn and DOn settings where POn controls the mix of Proportional on Error to Proportional on Measurement and DOn controls the mix of Derivative on Error to Derivative on Measurement. diff --git a/library.json b/library.json index 9362daf..5af1625 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "2.4.4", + "version": "2.4.5", "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~2.4.4" + "QuickPID": "~2.4.5" }, "frameworks": "*", "platforms": "*" diff --git a/library.properties b/library.properties index adbce13..8e8be56 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.4.4 +version=2.4.5 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 3948bce..feb0a59 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.4 + QuickPID Library for Arduino - Version 2.4.5 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. @@ -95,8 +95,8 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1.0, float D kd = Kd / SampleTimeSec; kpe = kp * pOn; kpm = kp * (1 - pOn); - kde = kp * dOn; - kdm = kp * (1 - dOn); + kde = kd * dOn; + kdm = kd * (1 - dOn); } /* SetTunings(...)************************************************************ @@ -234,8 +234,8 @@ void AutoTunePID::reset() { _autoTuneStage = 0; } -void AutoTunePID::autoTuneConfig(const byte outputStep, const byte hysteresis, const int atSetpoint, - const int atOutput, const bool dir, const bool printOrPlotter, uint32_t sampleTimeUs) +void AutoTunePID::autoTuneConfig(const float outputStep, const float hysteresis, const float atSetpoint, + const float atOutput, const bool dir, const bool printOrPlotter, uint32_t sampleTimeUs) { _outputStep = outputStep; _hysteresis = hysteresis; @@ -262,7 +262,7 @@ byte AutoTunePID::autoTuneLoop() { _t0 = millis(); _peakHigh = _atSetpoint; _peakLow = _atSetpoint; - (!_direction) ? *_output = 0 : *_output = _atOutput + _outputStep + 5; + (!_direction) ? *_output = 0 : *_output = _atOutput + (_outputStep * 2); _autoTuneStage = COARSE; return AUTOTUNE; break; @@ -272,7 +272,7 @@ byte AutoTunePID::autoTuneLoop() { break; } if (*_input < (_atSetpoint - _hysteresis)) { - (!_direction) ? *_output = _atOutput + _outputStep + 5 : *_output = _atOutput - _outputStep - 5; + (!_direction) ? *_output = _atOutput + (_outputStep * 2) : *_output = _atOutput - (_outputStep * 2); _autoTuneStage = FINE; } return AUTOTUNE; diff --git a/src/QuickPID.h b/src/QuickPID.h index 96bd6a6..5d857dd 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -23,7 +23,7 @@ class AutoTunePID { ~AutoTunePID() {}; void reset(); - void autoTuneConfig(const byte outputStep, const byte hysteresis, const int setpoint, const int output, + void autoTuneConfig(const float outputStep, const float hysteresis, const float setpoint, const float output, const bool dir = false, const bool printOrPlotter = false, uint32_t sampleTimeUs = 10000); byte autoTuneLoop(); void setAutoTuneConstants(float* kp, float* ki, float* kd); @@ -37,13 +37,10 @@ class AutoTunePID { byte _autoTuneStage = 1; tuningMethod _tuningRule; - byte _outputStep; - byte _hysteresis; - int _atSetpoint; // 1/3 of 10-bit ADC range for symetrical waveform - int _atOutput; bool _direction = false; bool _printOrPlotter = false; uint32_t _tLoop, _tLast, _t0, _t1, _t2, _t3; + float _outputStep, _hysteresis, _atSetpoint, _atOutput; float _Ku, _Tu, _td, _kp, _ki, _kd, _rdAvg, _peakHigh, _peakLow; const uint16_t RulesContants[10][3] = From 1c83b4de0d109475ffeb24925ec6180639ffb159 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 15 Jun 2021 10:27:20 -0400 Subject: [PATCH 061/101] tweak autoTune --- src/QuickPID.cpp | 3 ++- src/QuickPID.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index feb0a59..54682d0 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -296,12 +296,13 @@ byte AutoTunePID::autoTuneLoop() { if (*_input > _atSetpoint) { _t0 = micros(); if (_printOrPlotter == 1) Serial.print(F(" t0 →")); + _inputLast = *_input; _autoTuneStage = T1; } return AUTOTUNE; break; case T1: // get t1 - if (*_input > _atSetpoint + 0.2) { + if ((*_input > _atSetpoint) && (*_input > _inputLast)) { _t1 = micros(); if (_printOrPlotter == 1) Serial.print(F(" t1 →")); _autoTuneStage = T2; diff --git a/src/QuickPID.h b/src/QuickPID.h index 5d857dd..08a52d2 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -41,7 +41,7 @@ class AutoTunePID { bool _printOrPlotter = false; uint32_t _tLoop, _tLast, _t0, _t1, _t2, _t3; float _outputStep, _hysteresis, _atSetpoint, _atOutput; - float _Ku, _Tu, _td, _kp, _ki, _kd, _rdAvg, _peakHigh, _peakLow; + float _Ku, _Tu, _td, _kp, _ki, _kd, _rdAvg, _peakHigh, _peakLow, _inputLast; const uint16_t RulesContants[10][3] = { //ckp, cki, ckd x 1000 From 09fd03d3dfce2587439dfc08838a0035301f332b Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 16 Jun 2021 09:16:30 -0400 Subject: [PATCH 062/101] Update update default dOn --- src/QuickPID.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 54682d0..157e1a4 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -39,7 +39,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, direction_t ControllerDirection = DIRECT) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1, dOn = 1, ControllerDirection = DIRECT) { + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1.0, dOn = 0.0, ControllerDirection = DIRECT) { } /* Compute() *********************************************************************** From 11a10e1375e7a3b3824e6af4c23d7aa0bd1266e9 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 19 Jun 2021 13:00:37 -0400 Subject: [PATCH 063/101] Update --- library.json | 4 ++-- library.properties | 2 +- src/QuickPID.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/library.json b/library.json index 5af1625..1244a50 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "2.4.5", + "version": "2.4.6", "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~2.4.5" + "QuickPID": "~2.4.6" }, "frameworks": "*", "platforms": "*" diff --git a/library.properties b/library.properties index 8e8be56..3037caf 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.4.5 +version=2.4.6 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 157e1a4..8600aeb 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.5 + QuickPID Library for Arduino - Version 2.4.6 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. From 91b639fe2e2572c9b2ae7e3ab2f9f8aca1ffb2a8 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 19 Sep 2021 10:50:03 -0400 Subject: [PATCH 064/101] Update Version 2.4.7 Changed outputSum, outMin, outMax and error variables from int to float (issue #27) Update condition (line 64) in PID_RelayOutput example (issue #25) Update analogWrite.cpp and analogWrite.h to version 2.0.9 --- examples/PID_RelayOutput/PID_RelayOutput.ino | 2 +- library.json | 4 +- library.properties | 2 +- src/QuickPID.cpp | 2 +- src/QuickPID.h | 4 +- src/analogWrite.cpp | 93 ++++++++------------ src/analogWrite.h | 2 - 7 files changed, 44 insertions(+), 65 deletions(-) diff --git a/examples/PID_RelayOutput/PID_RelayOutput.ino b/examples/PID_RelayOutput/PID_RelayOutput.ino index 43476f2..f281eb1 100644 --- a/examples/PID_RelayOutput/PID_RelayOutput.ino +++ b/examples/PID_RelayOutput/PID_RelayOutput.ino @@ -61,6 +61,6 @@ void loop() windowStartTime += WindowSize; myQuickPID.Compute(); } - if (((unsigned int)Output > minWindow) && ((unsigned int)Output < (millis() - windowStartTime))) digitalWrite(RELAY_PIN, HIGH); + if (((unsigned int)Output > minWindow) && ((unsigned int)Output > (millis() - windowStartTime))) digitalWrite(RELAY_PIN, HIGH); else digitalWrite(RELAY_PIN, LOW); } diff --git a/library.json b/library.json index 1244a50..a14196e 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "2.4.6", + "version": "2.4.7", "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": @@ -22,6 +22,6 @@ "dependencies": { "QuickPID": "~2.4.6" }, - "frameworks": "*", + "frameworks": "arduino", "platforms": "*" } diff --git a/library.properties b/library.properties index 3037caf..6db1cd9 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.4.6 +version=2.4.7 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 8600aeb..2a0559e 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.6 + QuickPID Library for Arduino - Version 2.4.7 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. diff --git a/src/QuickPID.h b/src/QuickPID.h index 08a52d2..c745a17 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -151,9 +151,7 @@ class QuickPID { mode_t mode = MANUAL; direction_t controllerDirection; uint32_t sampleTimeUs, lastTime; - int outMin, outMax, error; - int outputSum; - float lastInput; + float outputSum, outMin, outMax, error, lastInput; bool inAuto; }; // class QuickPID diff --git a/src/analogWrite.cpp b/src/analogWrite.cpp index ec3e653..1f19088 100644 --- a/src/analogWrite.cpp +++ b/src/analogWrite.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 2.0.8 + AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 2.0.9 by dlloydev https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite This Library is licensed under the MIT License **********************************************************************************/ @@ -29,15 +29,6 @@ pinStatus_t pinsStatus[8] = { const uint8_t chd = 2; #endif -float awLedcSetup(uint8_t ch, double frequency, uint8_t bits) { -#if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) - frequency *= 80; //workaround for issue 5050 - return ledcSetup(ch, frequency, bits) / 80; -#else //ESP32 - return ledcSetup(ch, frequency, bits); -#endif -} - void awDetachPin(uint8_t pin, uint8_t ch) { pinsStatus[ch / chd].pin = -1; pinsStatus[ch / chd].value = 0; @@ -50,14 +41,6 @@ void awDetachPin(uint8_t pin, uint8_t ch) { REG_SET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL, GPIO_MODE_DEF_DISABLE); } -float awLedcReadFreq(uint8_t ch) { -#if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) - return ledcReadFreq(ch) / 80; //workaround for issue 5050 -#else //ESP32 - return ledcReadFreq(ch); -#endif -} - int8_t awGetChannel(int8_t pin) { if (!((pinMask >> pin) & 1)) return -1; //not pwm pin for (int8_t i = 0; i < 8; i++) { @@ -73,7 +56,7 @@ int8_t awGetChannel(int8_t pin) { if (pinsStatus[ch / chd].pin == -1) { //free channel if ((ledcRead(ch) < 1) && (ledcReadFreq(ch) < 1)) { //free timer pinsStatus[ch / chd].pin = pin; - aw::awLedcSetup(ch, pinsStatus[ch / chd].frequency, pinsStatus[ch / chd].resolution); + ledcSetup(ch, pinsStatus[ch / chd].frequency, pinsStatus[ch / chd].resolution); ledcAttachPin(pin, ch); return ch; break; @@ -109,7 +92,7 @@ float analogWrite(int8_t pin, int32_t value) { aw::pinsStatus[ch / aw::chd].value = value; } } - return aw::awLedcReadFreq(ch); + return ledcReadFreq(ch); } return 0; } @@ -128,7 +111,7 @@ float analogWrite(int8_t pin, int32_t value, float frequency) { if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high if (aw::pinsStatus[ch / aw::chd].frequency != frequency) { - aw::awLedcSetup(ch, frequency, bits); + ledcSetup(ch, frequency, bits); ledcWrite(ch, value); aw::pinsStatus[ch / aw::chd].frequency = frequency; } @@ -138,7 +121,7 @@ float analogWrite(int8_t pin, int32_t value, float frequency) { } } } - return aw::awLedcReadFreq(ch); + return ledcReadFreq(ch); } return 0; } @@ -157,7 +140,7 @@ float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high if ((aw::pinsStatus[ch / aw::chd].frequency != frequency) || (aw::pinsStatus[ch / aw::chd].resolution != bits)) { - aw::awLedcSetup(ch, frequency, bits); + ledcSetup(ch, frequency, bits); ledcWrite(ch, value); aw::pinsStatus[ch / aw::chd].frequency = frequency; aw::pinsStatus[ch / aw::chd].resolution = bits; @@ -168,7 +151,7 @@ float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution } } } - return aw::awLedcReadFreq(ch); + return ledcReadFreq(ch); } return 0; } @@ -187,7 +170,7 @@ float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high if ((aw::pinsStatus[ch / aw::chd].frequency != frequency) || (aw::pinsStatus[ch / aw::chd].resolution != bits)) { - aw::awLedcSetup(ch, frequency, bits); + ledcSetup(ch, frequency, bits); ledcWrite(ch, value); aw::pinsStatus[ch / aw::chd].frequency = frequency; aw::pinsStatus[ch / aw::chd].resolution = bits; @@ -213,7 +196,7 @@ float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution } } } - return aw::awLedcReadFreq(ch); + return ledcReadFreq(ch); } return 0; } @@ -223,12 +206,12 @@ float analogWriteFrequency(int8_t pin, float frequency) { if (ch >= 0) { if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; if (aw::pinsStatus[ch / aw::chd].frequency != frequency) { - aw::awLedcSetup(ch, frequency, aw::pinsStatus[ch / aw::chd].resolution); + ledcSetup(ch, frequency, aw::pinsStatus[ch / aw::chd].resolution); ledcWrite(ch, aw::pinsStatus[ch / aw::chd].value); aw::pinsStatus[ch / aw::chd].frequency = frequency; } } - return aw::awLedcReadFreq(ch); + return ledcReadFreq(ch); } int32_t analogWriteResolution(int8_t pin, uint8_t resolution) { @@ -237,7 +220,7 @@ int32_t analogWriteResolution(int8_t pin, uint8_t resolution) { if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; if (aw::pinsStatus[ch / aw::chd].resolution != resolution) { ledcDetachPin(pin); - aw::awLedcSetup(ch, aw::pinsStatus[ch / aw::chd].frequency, resolution & 0xF); + ledcSetup(ch, aw::pinsStatus[ch / aw::chd].frequency, resolution & 0xF); ledcAttachPin(pin, ch); ledcWrite(ch, aw::pinsStatus[ch / aw::chd].value); aw::pinsStatus[ch / aw::chd].resolution = resolution & 0xF; @@ -256,10 +239,10 @@ void setPinsStatusDefaults(int32_t value, float frequency, uint8_t resolution, u } void printPinsStatus() { - Serial.print("PWM pins: "); + Serial.print(F("PWM pins: ")); for (int i = 0; i < aw::muxSize; i++) { if ((aw::pinMask >> i) & 1) { - Serial.print(i); Serial.print(", "); + Serial.print(i); Serial.print(F(", ")); } } Serial.println(); @@ -267,30 +250,30 @@ void printPinsStatus() { Serial.println(); for (int i = 0; i < 8; i++) { int ch = aw::pinsStatus[i].channel; - Serial.print("ch: "); - if (ch < 10) Serial.print(" "); Serial.print(ch); Serial.print(" "); - Serial.print("Pin: "); - if ((aw::pinsStatus[ch / aw::chd].pin >= 0) && (aw::pinsStatus[ch / aw::chd].pin < 10)) Serial.print(" "); - Serial.print(aw::pinsStatus[ch / aw::chd].pin); Serial.print(" "); - Serial.print("Hz: "); - if (aw::awLedcReadFreq(ch) < 10000) Serial.print(" "); - if (aw::awLedcReadFreq(ch) < 1000) Serial.print(" "); - if (aw::awLedcReadFreq(ch) < 100) Serial.print(" "); - if (aw::awLedcReadFreq(ch) < 10) Serial.print(" "); - Serial.print(aw::awLedcReadFreq(ch)); Serial.print(" "); - Serial.print("Bits: "); - if (aw::pinsStatus[ch / aw::chd].resolution < 10) Serial.print(" "); - Serial.print(aw::pinsStatus[ch / aw::chd].resolution); Serial.print(" "); - Serial.print("Duty: "); - if (aw::pinsStatus[ch / aw::chd].value < 10000) Serial.print(" "); - if (aw::pinsStatus[ch / aw::chd].value < 1000) Serial.print(" "); - if (aw::pinsStatus[ch / aw::chd].value < 100) Serial.print(" "); - if (aw::pinsStatus[ch / aw::chd].value < 10) Serial.print(" "); - Serial.print(aw::pinsStatus[ch / aw::chd].value); Serial.print(" "); - Serial.print("Ø: "); - if (aw::pinsStatus[ch / aw::chd].phase < 1000) Serial.print(" "); - if (aw::pinsStatus[ch / aw::chd].phase < 100) Serial.print(" "); - if (aw::pinsStatus[ch / aw::chd].phase < 10) Serial.print(" "); + Serial.print(F("ch: ")); + if (ch < 10) Serial.print(F(" ")); Serial.print(ch); Serial.print(F(" ")); + Serial.print(F("Pin: ")); + if ((aw::pinsStatus[ch / aw::chd].pin >= 0) && (aw::pinsStatus[ch / aw::chd].pin < 10)) Serial.print(F(" ")); + Serial.print(aw::pinsStatus[ch / aw::chd].pin); Serial.print(F(" ")); + Serial.print(F("Hz: ")); + if (ledcReadFreq(ch) < 10000) Serial.print(F(" ")); + if (ledcReadFreq(ch) < 1000) Serial.print(F(" ")); + if (ledcReadFreq(ch) < 100) Serial.print(F(" ")); + if (ledcReadFreq(ch) < 10) Serial.print(F(" ")); + Serial.print(ledcReadFreq(ch)); Serial.print(F(" ")); + Serial.print(F("Bits: ")); + if (aw::pinsStatus[ch / aw::chd].resolution < 10) Serial.print(F(" ")); + Serial.print(aw::pinsStatus[ch / aw::chd].resolution); Serial.print(F(" ")); + Serial.print(F("Duty: ")); + if (aw::pinsStatus[ch / aw::chd].value < 10000) Serial.print(F(" ")); + if (aw::pinsStatus[ch / aw::chd].value < 1000) Serial.print(F(" ")); + if (aw::pinsStatus[ch / aw::chd].value < 100) Serial.print(F(" ")); + if (aw::pinsStatus[ch / aw::chd].value < 10) Serial.print(F(" ")); + Serial.print(aw::pinsStatus[ch / aw::chd].value); Serial.print(F(" ")); + Serial.print(F("Ø: ")); + if (aw::pinsStatus[ch / aw::chd].phase < 1000) Serial.print(F(" ")); + if (aw::pinsStatus[ch / aw::chd].phase < 100) Serial.print(F(" ")); + if (aw::pinsStatus[ch / aw::chd].phase < 10) Serial.print(F(" ")); Serial.print(aw::pinsStatus[ch / aw::chd].phase); Serial.println(); } diff --git a/src/analogWrite.h b/src/analogWrite.h index 34975bd..c971d4e 100644 --- a/src/analogWrite.h +++ b/src/analogWrite.h @@ -35,9 +35,7 @@ typedef struct pinStatus { uint32_t phase; } pinStatus_t; -float awLedcSetup(uint8_t ch, double frequency, uint8_t bits); void awDetachPin(uint8_t pin, uint8_t ch); -float awLedcReadFreq(uint8_t ch); int8_t awGetChannel(int8_t pin); } //namespace aw From fb6297e6c179600bfe09c0e84636f5756d3c3135 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 24 Sep 2021 23:15:43 -0400 Subject: [PATCH 065/101] Update Fixed sign of derivative terms (issue #29) --- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/library.json b/library.json index a14196e..a3c1964 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "2.4.7", + "version": "2.4.8", "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": diff --git a/library.properties b/library.properties index 6db1cd9..c12d630 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.4.7 +version=2.4.8 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 2a0559e..c39e257 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.7 + QuickPID Library for Arduino - Version 2.4.8 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. @@ -63,14 +63,14 @@ bool QuickPID::Compute() { peTerm = kpe * error; iTerm = ki * error; dmTerm = kdm * dInput; - deTerm = -kde * error; + deTerm = kde * error; outputSum += iTerm; // include integral amount if (outputSum > outMax) outputSum -= outputSum - outMax; // early integral anti-windup at outMax else if (outputSum < outMin) outputSum += outMin - outputSum; // early integral anti-windup at outMin outputSum = constrain(outputSum, outMin, outMax); // integral anti-windup clamp outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp - *myOutput = constrain(outputSum + peTerm + dmTerm - deTerm, outMin, outMax); // totalize, clamp and drive the output + *myOutput = constrain(outputSum + peTerm - dmTerm + deTerm, outMin, outMax); // totalize, clamp and drive the output lastInput = input; lastTime = now; From c9ab2a0808cb165ab340d057edbda7de8278dc2e Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 12 Nov 2021 09:49:52 -0500 Subject: [PATCH 066/101] Update `SetOutputLimits` now uses float type parameters to allow fine control of output limits if needed --- README.md | 2 +- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 4 ++-- src/QuickPID.h | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 28d3429..1d9bcdc 100644 --- a/README.md +++ b/README.md @@ -97,7 +97,7 @@ Sets the period, in microseconds, at which the calculation is performed. The def #### SetOutputLimits ```c++ -void QuickPID::SetOutputLimits(int Min, int Max); +void QuickPID::SetOutputLimits(float Min, float Max); ``` The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. diff --git a/library.json b/library.json index a3c1964..1eb0cd5 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "2.4.8", + "version": "2.4.9", "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": diff --git a/library.properties b/library.properties index c12d630..b8db346 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.4.8 +version=2.4.9 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index c39e257..d9982f8 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.8 + QuickPID Library for Arduino - Version 2.4.9 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. @@ -122,7 +122,7 @@ void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) { The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. ******************************************************************************/ -void QuickPID::SetOutputLimits(int Min, int Max) { +void QuickPID::SetOutputLimits(float Min, float Max) { if (Min >= Max) return; outMin = Min; outMax = Max; diff --git a/src/QuickPID.h b/src/QuickPID.h index c745a17..ee954a5 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -89,7 +89,7 @@ class QuickPID { void clearAutoTune(); // Sets and clamps the output to a specific range (0-255 by default). - void SetOutputLimits(int Min, int Max); + void SetOutputLimits(float Min, float Max); // available but not commonly used functions ****************************************************************** From cdaac7d2b94ca0163a4238d9217ff46bed990ddf Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 30 Nov 2021 10:21:35 -0500 Subject: [PATCH 067/101] Update - Fixes derivative on error calculation (issue#32) - Updated documentation and examples --- README.md | 21 +++++++++++++++---- .../AutoTune_AVR_Interrupt_TIMER.ino | 12 +++++------ .../AutoTune_AVR_Software_TIMER.ino | 12 +++++------ .../AutoTune_Filter_DIRECT.ino | 12 +++++------ .../AutoTune_Filter_REVERSE.ino | 14 ++++++------- library.json | 4 ++-- library.properties | 2 +- src/QuickPID.cpp | 6 ++++-- src/QuickPID.h | 2 +- 9 files changed, 50 insertions(+), 35 deletions(-) diff --git a/README.md b/README.md index 1d9bcdc..7ba6873 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,9 @@ The examples [AutoTune_Filter_DIRECT.ino](https://github.com/Dlloydev/QuickPID/b #### Direct and Reverse Controller Action -If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). +Direct controller action leads the output to increase when the input is larger than the setpoint (i.e. heating process). Reverse controller leads the output to decrease when the input is larger than the setpoint (i.e. cooling process). + +When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). ### Functions @@ -49,13 +51,24 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, ``` - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. + - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. + - `POn` controls the mix of Proportional on Error to Proportional on Measurement. Range is 0.0-1.0, default = 1.0 -- `DOn` controls the mix of Derivative on Error to Derivative on Measurement. Range is 0.0-1.0, default = 0.0 -![POnDOn](https://user-images.githubusercontent.com/63488701/120000053-68de3e00-bfa0-11eb-9db2-04c2f4be76a2.png) +- `DOn` controls the amount of Derivative on Error and the amount of Derivative on Measurement. Range is 0.0-1.0, default = 0.0. Note that Derivative on Error (at any point in time) is opposite and equal to Derivative on Measurement, so equal mixing of both (DOn =0.5) will result in the derivative term being 0. At this point, the controller becomes PI only. + + | DOn Setting | Description | + | ----------- | ---------------------------------------- | + | 0.0 | 100% Derivative on Measurement (default) | + | 0.25 | 50% Derivative on Measurement | + | 0.5 | 0% Derivative (PI control only) | + | 0.75 | 50% Derivative on Error | + | 1.0 | 100% Derivative on Error | + + ![POnDOn](https://user-images.githubusercontent.com/63488701/120000053-68de3e00-bfa0-11eb-9db2-04c2f4be76a2.png) -- `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. +- `ControllerDirection` Either DIRECT or REVERSE sets how the controller responds to a change in input. DIRECT action will lead the output to increase when the input is larger than the setpoint (i.e. heating process). REVERSE action will lead the output to decrease when the input is larger than the setpoint (i.e. cooling process). ```c++ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, diff --git a/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino b/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino index 5c6a1f8..bd00771 100644 --- a/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino +++ b/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino @@ -9,17 +9,17 @@ void runPid(); const uint32_t sampleTimeUs = 10000; // 10ms const byte inputPin = 0; const byte outputPin = 3; -const int outputMax = 255; -const int outputMin = 0; +const float outputMax = 255; +const float outputMin = 0; bool printOrPlotter = 0; // on(1) monitor, off(0) plotter float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 -byte outputStep = 5; -byte hysteresis = 1; -int setpoint = 341; // 1/3 of range for symetrical waveform -int output = 85; // 1/3 of range for symetrical waveform +float outputStep = 5; +float hysteresis = 1; +float setpoint = 341; // 1/3 of range for symetrical waveform +float output = 85; // 1/3 of range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; diff --git a/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino b/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino index 32f9576..a1a9d98 100644 --- a/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino +++ b/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino @@ -9,17 +9,17 @@ void runPid(); const uint32_t sampleTimeUs = 10000; // 10ms const byte inputPin = 0; const byte outputPin = 3; -const int outputMax = 255; -const int outputMin = 0; +const float outputMax = 255; +const float outputMin = 0; bool printOrPlotter = 0; // on(1) monitor, off(0) plotter float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 -byte outputStep = 5; -byte hysteresis = 1; -int setpoint = 341; // 1/3 of range for symetrical waveform -int output = 85; // 1/3 of range for symetrical waveform +float outputStep = 5; +float hysteresis = 1; +float setpoint = 341; // 1/3 of range for symetrical waveform +float output = 85; // 1/3 of range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino index 4105908..b3546c7 100644 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino @@ -8,17 +8,17 @@ const uint32_t sampleTimeUs = 10000; // 10ms const byte inputPin = 0; const byte outputPin = 3; -const int outputMax = 255; -const int outputMin = 0; +const float outputMax = 255; +const float outputMin = 0; bool printOrPlotter = 0; // on(1) monitor, off(0) plotter float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 -byte outputStep = 5; -byte hysteresis = 1; -int setpoint = 341; // 1/3 of range for symetrical waveform -int output = 85; // 1/3 of range for symetrical waveform +float outputStep = 5; +float hysteresis = 1; +float setpoint = 341; // 1/3 of range for symetrical waveform +float output = 85; // 1/3 of range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; diff --git a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino index 21cf9a6..6e8253b 100644 --- a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino +++ b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino @@ -8,18 +8,18 @@ const uint32_t sampleTimeUs = 10000; // 10ms const byte inputPin = 0; const byte outputPin = 3; -const int inputMax = 1023; -const int outputMax = 255; -const int outputMin = 0; +const float inputMax = 1023; +const float outputMax = 255; +const float outputMin = 0; bool printOrPlotter = 0; // on(1) monitor, off(0) plotter float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 -byte outputStep = 5; -byte hysteresis = 1; -int setpoint = 341; // 1/3 of range for symetrical waveform -int output = 85; // 1/3 of range for symetrical waveform +float outputStep = 5; +float hysteresis = 1; +float setpoint = 341; // 1/3 of range for symetrical waveform +float output = 85; // 1/3 of range for symetrical waveform float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; diff --git a/library.json b/library.json index 1eb0cd5..19ca709 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "2.4.9", + "version": "2.4.10", "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~2.4.6" + "QuickPID": "~2.4.10" }, "frameworks": "arduino", "platforms": "*" diff --git a/library.properties b/library.properties index b8db346..e53214f 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=2.4.9 +version=2.4.10 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index d9982f8..5182985 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.9 + QuickPID Library for Arduino - Version 2.4.10 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. @@ -55,6 +55,7 @@ bool QuickPID::Compute() { float input = *myInput; float dInput = input - lastInput; error = *mySetpoint - input; + float dError = error - lastError; if (controllerDirection == REVERSE) { error = -error; dInput = -dInput; @@ -63,7 +64,7 @@ bool QuickPID::Compute() { peTerm = kpe * error; iTerm = ki * error; dmTerm = kdm * dInput; - deTerm = kde * error; + deTerm = kde * dError; outputSum += iTerm; // include integral amount if (outputSum > outMax) outputSum -= outputSum - outMax; // early integral anti-windup at outMax @@ -72,6 +73,7 @@ bool QuickPID::Compute() { outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp *myOutput = constrain(outputSum + peTerm - dmTerm + deTerm, outMin, outMax); // totalize, clamp and drive the output + lastError = error; lastInput = input; lastTime = now; return true; diff --git a/src/QuickPID.h b/src/QuickPID.h index ee954a5..108bcdd 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -151,7 +151,7 @@ class QuickPID { mode_t mode = MANUAL; direction_t controllerDirection; uint32_t sampleTimeUs, lastTime; - float outputSum, outMin, outMax, error, lastInput; + float outputSum, outMin, outMax, error, lastError, lastInput; bool inAuto; }; // class QuickPID From dd773fe28454d72e84e7f4e757cc99be103b8529 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Thu, 2 Dec 2021 19:47:47 -0500 Subject: [PATCH 068/101] Update Added Conditional Anti-Windup feature (issue#34) and updated documentation. --- README.md | 8 ++++---- library.json | 6 +++--- library.properties | 4 ++-- src/QuickPID.cpp | 26 ++++++++++++++++---------- 4 files changed, 25 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index 7ba6873..7f370df 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is an updated implementation of the Arduino PID library with a built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available are POn and DOn settings where POn controls the mix of Proportional on Error to Proportional on Measurement and DOn controls the mix of Derivative on Error to Derivative on Measurement. +QuickPID is an updated implementation of the Arduino PID library with a built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available are POn and DOn settings where POn controls the mix of Proportional on Error to Proportional on Measurement and DOn controls the mix of Derivative on Error to Derivative on Measurement. #### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) @@ -17,13 +17,13 @@ Development began with a fork of the Arduino PID Library. Modifications and new - [x] Variable Proportional on Error to Proportional on Measurement parameter `POn` - [x] Variable Derivative on Error to Derivative on Measurement parameter `DOn` - [x] New PID Query Functions: `GetPterm();` `GetIterm();` `GetDterm();` -- [x] 2-stage Integral windup prevention when output exceeds limits +- [x] Uses conditional and clamping Integral anti-windup methods - [x] New REVERSE mode only changes sign of `error` and `dInput` - [x] Uses `float` instead of `double` -#### AutoTune Features +#### AutoTune Features (to be updated in upcoming version 3.0.0) -- [x] New AutoTune class added as a dynamic object and includes 10 tuning methods +- [x] AutoTune class provided as a dynamic object and includes 10 tuning methods - [x] Compatible with reverse acting controllers - [x] Fast, non-blocking tuning sequence completes in only 1.5 oscillations - [x] Determines how easy the process is to control diff --git a/library.json b/library.json index 19ca709..009a301 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", - "version": "2.4.10", - "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", + "version": "2.5.0", + "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, conditional anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": { @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~2.4.10" + "QuickPID": "~2.5.0" }, "frameworks": "arduino", "platforms": "*" diff --git a/library.properties b/library.properties index e53214f..c43975a 100644 --- a/library.properties +++ b/library.properties @@ -1,8 +1,8 @@ name=QuickPID -version=2.4.10 +version=2.5.0 author=David Lloyd maintainer=David Lloyd -sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. +sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, conditional anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. paragraph=Also includes analogWrite compatibility for ESP32 and ESP32-S2. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 5182985..40d5bf6 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.4.10 + QuickPID Library for Arduino - Version 2.5.0 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID Library and work on AutoTunePID class by gnalbandian (Gonzalo). Licensed under the MIT License. @@ -52,26 +52,32 @@ bool QuickPID::Compute() { uint32_t now = micros(); uint32_t timeChange = (now - lastTime); if (mode == TIMER || timeChange >= sampleTimeUs) { + float input = *myInput; float dInput = input - lastInput; + if (controllerDirection == REVERSE) dInput = -dInput; + error = *mySetpoint - input; + if (controllerDirection == REVERSE) error = -error; float dError = error - lastError; - if (controllerDirection == REVERSE) { - error = -error; - dInput = -dInput; - } + pmTerm = kpm * dInput; peTerm = kpe * error; - iTerm = ki * error; + iTerm = ki * error; dmTerm = kdm * dInput; deTerm = kde * dError; + //conditional anti-windup + bool aw = false; + float iTermOut = (peTerm - pmTerm) + ki * (iTerm + error); + if (iTermOut > outMax && dError > 0) aw = true; + else if (iTermOut < outMin && dError < 0) aw = true; + if (aw && ki) iTerm = constrain(iTermOut, -outMax, outMax); + + //compute output as per PID_v1 outputSum += iTerm; // include integral amount - if (outputSum > outMax) outputSum -= outputSum - outMax; // early integral anti-windup at outMax - else if (outputSum < outMin) outputSum += outMin - outputSum; // early integral anti-windup at outMin - outputSum = constrain(outputSum, outMin, outMax); // integral anti-windup clamp outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp - *myOutput = constrain(outputSum + peTerm - dmTerm + deTerm, outMin, outMax); // totalize, clamp and drive the output + *myOutput = constrain(outputSum + peTerm - dmTerm + deTerm, outMin, outMax); // totalize, clamp, drive output lastError = error; lastInput = input; From 576d7143dcb82eed3a456cbf2da6b3cfe825aecf Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 12 Dec 2021 11:08:53 -0500 Subject: [PATCH 069/101] Major update ### QuickPID 3.0.0 Many changes: - Removed AutoTune in preparation for a new AutoTune library compatible with `QuickPID`, `PID_v1` and others (coming soon) - Added a few more controller options, all easily configured using enum named values - Proportional and Derivative options are also easily configured using enum named values - New integral ant-windup options include `CONDITION`, `CLAMP` (default) and `OFF` - Updated documentation and examples --- README.md | 99 ++---- .../AutoTune_AVR_Interrupt_TIMER.ino | 107 ------ .../AutoTune_AVR_Software_TIMER.ino | 107 ------ .../AutoTune_Filter_DIRECT.ino | 98 ------ .../AutoTune_Filter_REVERSE.ino | 99 ------ .../PID_AVR_Basic_Interrupt_TIMER.ino | 10 +- .../PID_AVR_Basic_Software_TIMER.ino | 10 +- .../PID_AdaptiveTunings.ino | 18 +- examples/PID_Basic/PID_Basic.ino | 10 +- .../PID_Controller_Options.ino | 68 ++++ .../PID_POn_DOn_Error_Measurement.ino | 34 -- examples/PID_RelayOutput/PID_RelayOutput.ino | 10 +- keywords.txt | 21 +- library.json | 6 +- library.properties | 4 +- src/QuickPID.cpp | 314 +++--------------- src/QuickPID.h | 125 ++----- 17 files changed, 227 insertions(+), 913 deletions(-) delete mode 100644 examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino delete mode 100644 examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino delete mode 100644 examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino delete mode 100644 examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino create mode 100644 examples/PID_Controller_Options/PID_Controller_Options.ino delete mode 100644 examples/PID_POn_DOn_Error_Measurement/PID_POn_DOn_Error_Measurement.ino diff --git a/README.md b/README.md index 7f370df..6bf6fcb 100644 --- a/README.md +++ b/README.md @@ -1,45 +1,27 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is an updated implementation of the Arduino PID library with a built-in [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) class as a dynamic object to reduce memory if not used, thanks to contributions by [gnalbandian (Gonzalo)](https://github.com/gnalbandian). This controller can automatically determine and set parameters `Kp, Ki, Kd`. Additionally the Ultimate Gain `Ku`, Ultimate Period `Tu`, Dead Time `td` and determine how easy the process is to control. There are 10 tuning rules available to choose from. Also available are POn and DOn settings where POn controls the mix of Proportional on Error to Proportional on Measurement and DOn controls the mix of Derivative on Error to Derivative on Measurement. - -#### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) +QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library. However, the additional features like integral anti-windup based on conditional-conditioning, clamping, or turning it off completely. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. The controller modes includes `TIMER`, which allows external timer or ISR timing control. ### Features -Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the [change log](https://github.com/Dlloydev/QuickPID/wiki/Change-Log). +Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the [releases](https://github.com/Dlloydev/QuickPID/releases). #### New feature Summary -- [x] Fast PID read-compute-write cycle (Arduino UNO): QuickPID = **51µs**, PID_v1 = **128µs** - [x] `TIMER` mode for calling PID compute by an external timer function or ISR -- [x] `analogReadFast()` support for AVR (4x faster) - [x] `analogWrite()` support for ESP32 and ESP32-S2 -- [x] Variable Proportional on Error to Proportional on Measurement parameter `POn` -- [x] Variable Derivative on Error to Derivative on Measurement parameter `DOn` -- [x] New PID Query Functions: `GetPterm();` `GetIterm();` `GetDterm();` -- [x] Uses conditional and clamping Integral anti-windup methods -- [x] New REVERSE mode only changes sign of `error` and `dInput` +- [x] Proportional on error `PE`, measurement `PM` or both `PEM` options +- [x] Derivative on error `DE` and measurement `DM` options +- [x] New PID Query Functions `GetPterm`, `GetIterm`, `GetDterm`, `GetPmode`, `GetDmode` and `GetAwMode` +- [x] New integral anti-windup options `CONDITION`, `CLAMP` and `OFF` +- [x] New `REVERSE` mode only changes sign of `error` and `dInput` - [x] Uses `float` instead of `double` -#### AutoTune Features (to be updated in upcoming version 3.0.0) - -- [x] AutoTune class provided as a dynamic object and includes 10 tuning methods -- [x] Compatible with reverse acting controllers -- [x] Fast, non-blocking tuning sequence completes in only 1.5 oscillations -- [x] Determines how easy the process is to control -- [x] Determines ultimate period `Tu`, dead time `td`, ultimate gain `Ku`, and tuning parameters `Kp, Ki, Kd` - -### [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) - -#### [AutoTune Filter Examples](https://github.com/Dlloydev/QuickPID/wiki/AutoTune-Filter-Examples) - -The examples [AutoTune_Filter_DIRECT.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino) and [AutoTune_Filter_REVERSE.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino) allow you to experiment with the AutoTunePID class, various tuning rules and the POn and DOn controls using ADC and PWM with RC filter. It automatically determines and sets the tuning parameters and works with both `DIRECT` and `REVERSE` acting controllers. - #### Direct and Reverse Controller Action Direct controller action leads the output to increase when the input is larger than the setpoint (i.e. heating process). Reverse controller leads the output to decrease when the input is larger than the setpoint (i.e. cooling process). -When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). +When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). ### Functions @@ -47,32 +29,20 @@ When the controller is set to `REVERSE` acting, the sign of the `error` and `dIn ```c++ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, float POn, float DOn, uint8_t ControllerDirection); + float Kp, float Ki, float Kd, uint8_t pMode = PE, uint8_t dMode = DM, + uint8_t awMode = CLAMP, uint8_t Action = DIRECT) ``` - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. - - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. - -- `POn` controls the mix of Proportional on Error to Proportional on Measurement. Range is 0.0-1.0, default = 1.0 - -- `DOn` controls the amount of Derivative on Error and the amount of Derivative on Measurement. Range is 0.0-1.0, default = 0.0. Note that Derivative on Error (at any point in time) is opposite and equal to Derivative on Measurement, so equal mixing of both (DOn =0.5) will result in the derivative term being 0. At this point, the controller becomes PI only. - - | DOn Setting | Description | - | ----------- | ---------------------------------------- | - | 0.0 | 100% Derivative on Measurement (default) | - | 0.25 | 50% Derivative on Measurement | - | 0.5 | 0% Derivative (PI control only) | - | 0.75 | 50% Derivative on Error | - | 1.0 | 100% Derivative on Error | - - ![POnDOn](https://user-images.githubusercontent.com/63488701/120000053-68de3e00-bfa0-11eb-9db2-04c2f4be76a2.png) - -- `ControllerDirection` Either DIRECT or REVERSE sets how the controller responds to a change in input. DIRECT action will lead the output to increase when the input is larger than the setpoint (i.e. heating process). REVERSE action will lead the output to decrease when the input is larger than the setpoint (i.e. cooling process). +- `pMode` is the proportional mode parameter with options for `PE` proportional on error (default), `PM` proportional on measurement and `PEM` which is 0.5 `PE` + 0.5 `PM`. +- `dMode` is the derivative mode parameter with options for `DE` derivative on error (default), `DM` derivative on measurement (default). +- `awMode` is the integral anti-windup parameter with options for `CONDITION` which is based on PI terms to provide some integral correction and prevent deep saturation, `CLAMP` (default) which clamps the summation of the pmTerm and iTerm. The `OFF` option turns off all anti-windup. +- `Action` is the controller action parameter which has `DIRECT` (default) and `REVERSE` options. These options set how the controller responds to a change in input. `DIRECT` action will lead the output to increase when the input is larger than the setpoint (i.e. heating process). `REVERSE` action will lead the output to decrease when the input is larger than the setpoint (i.e. cooling process). ```c++ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, uint8_t ControllerDirection); + float Kp, float Ki, float Kd, uint8_t Action = DIRECT) ``` This allows you to use Proportional on Error without explicitly saying so. @@ -88,7 +58,7 @@ This function contains the PID algorithm and it should be called once every loop #### SetTunings ```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn, float DOn); +void QuickPID::SetTunings(float Kp, float Ki, float Kd, uint8_t pMode = PE, uint8_t dMode = DM, uint8_t awMode = CLAMP) ``` This function allows the controller's dynamic performance to be adjusted. It's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. The parameters are as described in the constructor. @@ -118,15 +88,13 @@ The PID controller is designed to vary its output within a given range. By defa #### SetMode ```c++ -void QuickPID::SetMode(uint8_t Mode); +void QuickPID::SetMode(uint8_t Mode) ``` Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (1) or `TIMER` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. `TIMER` mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See examples: -- [AutoTune_AVR_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino) -- [AutoTune_AVR_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino) - [PID_AVR_Basic_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino) - [PID_AVR_Basic_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino) @@ -141,34 +109,29 @@ Does all the things that need to happen to ensure a bump-less transfer from manu #### SetControllerDirection ```c++ -void QuickPID::SetControllerDirection(uint8_t Direction) +void QuickPID::SetControllerDirection(uint8_t Action) ``` -The PID will either be connected to a DIRECT acting process (+Output leads to +Input) or a REVERSE acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. +The PID will either be connected to a `DIRECT` acting process (+Output leads to +Input) or a `REVERSE` acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. #### PID Query Functions ```c++ - float GetKp(); // proportional gain - float GetKi(); // integral gain - float GetKd(); // derivative gain - float GetPterm(); // proportional component of output - float GetIterm(); // integral component of output - float GetDterm(); // derivative component of output - mode_t GetMode(); // MANUAL (0) or AUTOMATIC (1) or TIMER (2) - direction_t GetDirection(); // DIRECT (0) or REVERSE (1) + float GetKp(); // proportional gain + float GetKi(); // integral gain + float GetKd(); // derivative gain + float GetPterm(); // proportional component of output + float GetIterm(); // integral component of output + float GetDterm(); // derivative component of output + uint8_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) + uint8_t GetDirection(); // DIRECT (0), REVERSE (1) + uint8_t GetPmode(); // PE (0), PM (1), PEM (2) + uint8_t GetDmode(); // DE (0), DM (1) + uint8_t GetAwMode(); // CONDITION (0, CLAMP (1), OFF (2) ``` These functions query the internal state of the PID. -#### Utility Functions - -```c++ -int QuickPID::analogReadFast(int ADCpin) -``` - -A faster configuration of `analogRead()`where a preset of 32 is used. If the architecture definition isn't found, normal `analogRead()`is used to return a value. - #### [AnalogWrite (PWM and DAC) for ESP32](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) Use this link for reference. Note that if you're using QuickPID, there's no need to install the AnalogWrite library as this feature is already included. @@ -184,7 +147,7 @@ Use this link for reference. Note that if you're using QuickPID, there's no need *************************************************************** ``` - - For an ultra-detailed explanation of why the code is the way it is, please visit: + - For an ultra-detailed explanation of the original code, please visit: http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary diff --git a/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino b/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino deleted file mode 100644 index bd00771..0000000 --- a/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino +++ /dev/null @@ -1,107 +0,0 @@ -/****************************************************************************** - AutoTune AVR Interrupt TIMER Example - Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter - ******************************************************************************/ -#include "TimerOne.h" // https://github.com/PaulStoffregen/TimerOne -#include "QuickPID.h" -void runPid(); - -const uint32_t sampleTimeUs = 10000; // 10ms -const byte inputPin = 0; -const byte outputPin = 3; -const float outputMax = 255; -const float outputMin = 0; - -bool printOrPlotter = 0; // on(1) monitor, off(0) plotter -float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 -float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 - -float outputStep = 5; -float hysteresis = 1; -float setpoint = 341; // 1/3 of range for symetrical waveform -float output = 85; // 1/3 of range for symetrical waveform - -float Input, Output, Setpoint; -float Kp = 0, Ki = 0, Kd = 0; -bool pidLoop = false; -volatile bool computeNow = false; - -QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); - -void setup() { - Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval - Timer1.attachInterrupt(runPid); // attaches runPid() as a timer overflow interrupt - - Serial.begin(115200); - Serial.println(); - if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { - Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); - while (1); - } - // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki - //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); - _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); - //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); - //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); - //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); - //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); - //_myPID.AutoTune(tuningMethod::AMIGOF_PID); - //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); - //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); - //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); - - _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); -} - -void loop() { - if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() - { - switch (_myPID.autoTune->autoTuneLoop()) { - case _myPID.autoTune->AUTOTUNE: - Input = avg(_myPID.analogReadFast(inputPin)); - analogWrite(outputPin, Output); - break; - case _myPID.autoTune->TUNINGS: - _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings - _myPID.SetMode(QuickPID::TIMER); // setup PID - _myPID.SetSampleTimeUs(sampleTimeUs); - _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID - Setpoint = 500; - break; - case _myPID.autoTune->CLR: - if (!pidLoop) { - _myPID.clearAutoTune(); // releases memory used by AutoTune object - pidLoop = true; - } - break; - } - } - if (pidLoop) { - if (printOrPlotter == 0) { // plotter - Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); - Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.println(","); - } - if (computeNow) { - Input = _myPID.analogReadFast(inputPin); - _myPID.Compute(); - analogWrite(outputPin, Output); - computeNow = false; - } - } -} - -void runPid() { - computeNow = true; -} - -float avg(int inputVal) { - static int arrDat[16]; - static int pos; - static long sum; - pos++; - if (pos >= 16) pos = 0; - sum = sum - arrDat[pos] + inputVal; - arrDat[pos] = inputVal; - return (float)sum / 16.0; -} diff --git a/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino b/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino deleted file mode 100644 index a1a9d98..0000000 --- a/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino +++ /dev/null @@ -1,107 +0,0 @@ -/****************************************************************************** - AutoTune AVR Software TIMER Example - Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter - ******************************************************************************/ -#include "Ticker.h" // https://github.com/sstaub/Ticker -#include "QuickPID.h" -void runPid(); - -const uint32_t sampleTimeUs = 10000; // 10ms -const byte inputPin = 0; -const byte outputPin = 3; -const float outputMax = 255; -const float outputMin = 0; - -bool printOrPlotter = 0; // on(1) monitor, off(0) plotter -float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 -float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 - -float outputStep = 5; -float hysteresis = 1; -float setpoint = 341; // 1/3 of range for symetrical waveform -float output = 85; // 1/3 of range for symetrical waveform - -float Input, Output, Setpoint; -float Kp = 0, Ki = 0, Kd = 0; -bool pidLoop = false; -static boolean computeNow = false; - -Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); -QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); - -void setup() { - timer1.start(); - Serial.begin(115200); - Serial.println(); - if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { - Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); - while (1); - } - // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki - //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); - _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); - //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); - //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); - //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); - //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); - //_myPID.AutoTune(tuningMethod::AMIGOF_PID); - //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); - //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); - //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); - - _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); -} - -void loop() { - timer1.update(); - if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() - { - switch (_myPID.autoTune->autoTuneLoop()) { - case _myPID.autoTune->AUTOTUNE: - Input = avg(_myPID.analogReadFast(inputPin)); - analogWrite(outputPin, Output); - break; - case _myPID.autoTune->TUNINGS: - _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings - _myPID.SetMode(QuickPID::TIMER); // setup PID - _myPID.SetSampleTimeUs(sampleTimeUs); - _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID - Setpoint = 500; - break; - case _myPID.autoTune->CLR: - if (!pidLoop) { - _myPID.clearAutoTune(); // releases memory used by AutoTune object - pidLoop = true; - } - break; - } - } - if (pidLoop) { - if (printOrPlotter == 0) { // plotter - Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); - Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.println(","); - } - if (computeNow) { - Input = _myPID.analogReadFast(inputPin); - _myPID.Compute(); - analogWrite(outputPin, Output); - computeNow = false; - } - } -} - -void runPid() { - computeNow = true; -} - -float avg(int inputVal) { - static int arrDat[16]; - static int pos; - static long sum; - pos++; - if (pos >= 16) pos = 0; - sum = sum - arrDat[pos] + inputVal; - arrDat[pos] = inputVal; - return (float)sum / 16.0; -} diff --git a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino b/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino deleted file mode 100644 index b3546c7..0000000 --- a/examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino +++ /dev/null @@ -1,98 +0,0 @@ -/****************************************************************************** - AutoTune Filter DIRECT Example - Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter - ******************************************************************************/ - -#include "QuickPID.h" - -const uint32_t sampleTimeUs = 10000; // 10ms -const byte inputPin = 0; -const byte outputPin = 3; -const float outputMax = 255; -const float outputMin = 0; - -bool printOrPlotter = 0; // on(1) monitor, off(0) plotter -float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 -float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 - -float outputStep = 5; -float hysteresis = 1; -float setpoint = 341; // 1/3 of range for symetrical waveform -float output = 85; // 1/3 of range for symetrical waveform - -float Input, Output, Setpoint; -float Kp = 0, Ki = 0, Kd = 0; -bool pidLoop = false; - -QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); - -void setup() { - Serial.begin(115200); - Serial.println(); - if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { - Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); - while (1); - } - // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki - //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); - _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); - //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); - //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); - //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); - //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); - //_myPID.AutoTune(tuningMethod::AMIGOF_PID); - //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); - //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); - //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); - - _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); -} - -void loop() { - - if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() - { - switch (_myPID.autoTune->autoTuneLoop()) { - case _myPID.autoTune->AUTOTUNE: - Input = avg(_myPID.analogReadFast(inputPin)); - analogWrite(outputPin, Output); - break; - - case _myPID.autoTune->TUNINGS: - _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings - _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID - _myPID.SetSampleTimeUs(sampleTimeUs); - _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID - Setpoint = 500; - break; - - case _myPID.autoTune->CLR: - if (!pidLoop) { - _myPID.clearAutoTune(); // releases memory used by AutoTune object - pidLoop = true; - } - break; - } - } - if (pidLoop) { - if (printOrPlotter == 0) { // plotter - Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); - Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.println(","); - } - Input = _myPID.analogReadFast(inputPin); - _myPID.Compute(); - analogWrite(outputPin, Output); - } -} - -float avg(int inputVal) { - static int arrDat[16]; - static int pos; - static long sum; - pos++; - if (pos >= 16) pos = 0; - sum = sum - arrDat[pos] + inputVal; - arrDat[pos] = inputVal; - return (float)sum / 16.0; -} diff --git a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino b/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino deleted file mode 100644 index 6e8253b..0000000 --- a/examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino +++ /dev/null @@ -1,99 +0,0 @@ -/****************************************************************************** - AutoTune Filter REVERSE Example - Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter - ******************************************************************************/ - -#include "QuickPID.h" - -const uint32_t sampleTimeUs = 10000; // 10ms -const byte inputPin = 0; -const byte outputPin = 3; -const float inputMax = 1023; -const float outputMax = 255; -const float outputMin = 0; - -bool printOrPlotter = 0; // on(1) monitor, off(0) plotter -float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 -float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 - -float outputStep = 5; -float hysteresis = 1; -float setpoint = 341; // 1/3 of range for symetrical waveform -float output = 85; // 1/3 of range for symetrical waveform - -float Input, Output, Setpoint; -float Kp = 0, Ki = 0, Kd = 0; -bool pidLoop = false; - -QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::REVERSE); - -void setup() { - Serial.begin(115200); - Serial.println(); - if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { - Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); - while (1); - } - // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki - //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); - _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); - //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); - //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); - //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); - //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); - //_myPID.AutoTune(tuningMethod::AMIGOF_PID); - //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); - //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); - //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); - - _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, inputMax - setpoint, output, QuickPID::REVERSE, printOrPlotter, sampleTimeUs); -} - -void loop() { - - if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() - { - switch (_myPID.autoTune->autoTuneLoop()) { - case _myPID.autoTune->AUTOTUNE: - Input = inputMax - avg(_myPID.analogReadFast(inputPin)); // filtered, reverse acting - analogWrite(outputPin, Output); - break; - - case _myPID.autoTune->TUNINGS: - _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings - _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID - _myPID.SetSampleTimeUs(sampleTimeUs); - _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID - Setpoint = 500; - break; - - case _myPID.autoTune->CLR: - if (!pidLoop) { - _myPID.clearAutoTune(); // releases memory used by AutoTune object - pidLoop = true; - } - break; - } - } - if (pidLoop) { - if (printOrPlotter == 0) { // plotter - Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); - Serial.print("Input:"); Serial.print(Input); Serial.print(","); - Serial.print("Output:"); Serial.print(Output); Serial.println(","); - } - Input = inputMax - _myPID.analogReadFast(inputPin); // reverse acting - _myPID.Compute(); - analogWrite(outputPin, Output); - } -} - -float avg(int inputVal) { - static int arrDat[16]; - static int pos; - static long sum; - pos++; - if (pos >= 16) pos = 0; - sum = sum - arrDat[pos] + inputVal; - arrDat[pos] = inputVal; - return (float)sum / 16.0; -} diff --git a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino index e8632b2..5ec40a5 100644 --- a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino +++ b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino @@ -16,24 +16,24 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); void setup() { Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval Timer1.attachInterrupt(runPid); // attaches runPid() as a timer overflow interrupt //initialize the variables we're linked to - Input = myQuickPID.analogReadFast(PIN_INPUT); + Input = analogRead(PIN_INPUT); Setpoint = 100; //turn the PID on - myQuickPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(QuickPID::AUTOMATIC); } void loop() { if (computeNow) { - Input = myQuickPID.analogReadFast(PIN_INPUT); - myQuickPID.Compute(); + Input = analogRead(PIN_INPUT); + myPID.Compute(); analogWrite(PIN_OUTPUT, Output); computeNow = false; } diff --git a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino index 7c49e50..c33c570 100644 --- a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino +++ b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino @@ -18,24 +18,24 @@ float Setpoint, Input, Output; float Kp = 2, Ki = 5, Kd = 1; Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); void setup() { timer1.start(); //initialize the variables we're linked to - Input = myQuickPID.analogReadFast(PIN_INPUT); + Input = analogRead(PIN_INPUT); Setpoint = 100; //turn the PID on - myQuickPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(QuickPID::AUTOMATIC); } void loop() { timer1.update(); if (computeNow) { - Input = myQuickPID.analogReadFast(PIN_INPUT); - myQuickPID.Compute(); + Input = analogRead(PIN_INPUT); + myPID.Compute(); analogWrite(PIN_OUTPUT, Output); computeNow = false; } diff --git a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino index c12756b..9d60f4c 100644 --- a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino +++ b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino @@ -20,35 +20,31 @@ float Setpoint, Input, Output; //Define the aggressive and conservative and POn Tuning Parameters float aggKp = 4, aggKi = 0.2, aggKd = 1; float consKp = 1, consKi = 0.05, consKd = 0.25; -float aggPOn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0) -float consPOn = 0.0; // proportional on Error to Measurement ratio (0.0-1.0) -float aggDOn = 1.0; // derivative on Error to Measurement ratio (0.0-1.0) -float consDOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0) //Specify the links and initial tuning parameters -QuickPID myQuickPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, aggPOn, consDOn, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, QuickPID::DIRECT); void setup() { //initialize the variables we're linked to - Input = myQuickPID.analogReadFast(PIN_INPUT); + Input = analogRead(PIN_INPUT); Setpoint = 100; //turn the PID on - myQuickPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(QuickPID::AUTOMATIC); } void loop() { - Input = myQuickPID.analogReadFast(PIN_INPUT); + Input = analogRead(PIN_INPUT); float gap = abs(Setpoint - Input); //distance away from setpoint if (gap < 10) { //we're close to setpoint, use conservative tuning parameters - myQuickPID.SetTunings(consKp, consKi, consKd, consPOn, consDOn); + myPID.SetTunings(consKp, consKi, consKd); } else { //we're far from setpoint, use aggressive tuning parameters - myQuickPID.SetTunings(aggKp, aggKi, aggKd, aggPOn, aggDOn); + myPID.SetTunings(aggKp, aggKi, aggKd); } - myQuickPID.Compute(); + myPID.Compute(); analogWrite(PIN_OUTPUT, Output); } diff --git a/examples/PID_Basic/PID_Basic.ino b/examples/PID_Basic/PID_Basic.ino index 1aa4ede..da2875d 100644 --- a/examples/PID_Basic/PID_Basic.ino +++ b/examples/PID_Basic/PID_Basic.ino @@ -14,21 +14,21 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); void setup() { //initialize the variables we're linked to - Input = myQuickPID.analogReadFast(PIN_INPUT); + Input = analogRead(PIN_INPUT); Setpoint = 100; //turn the PID on - myQuickPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(QuickPID::AUTOMATIC); } void loop() { - Input = myQuickPID.analogReadFast(PIN_INPUT); - myQuickPID.Compute(); + Input = analogRead(PIN_INPUT); + myPID.Compute(); analogWrite(PIN_OUTPUT, Output); } diff --git a/examples/PID_Controller_Options/PID_Controller_Options.ino b/examples/PID_Controller_Options/PID_Controller_Options.ino new file mode 100644 index 0000000..8263834 --- /dev/null +++ b/examples/PID_Controller_Options/PID_Controller_Options.ino @@ -0,0 +1,68 @@ +/************************************************************************************** + For testing and checking parameter options. From QuickPID.h: + + enum Mode : uint8_t {MANUAL, AUTOMATIC, TIMER}; // controller modes + enum Action : uint8_t {DIRECT, REVERSE}; // controller actions + enum pMode : uint8_t {PE, PM, PEM}; // proportional modes + enum dMode : uint8_t {DE, DM}; // derivative modes + enum awMode : uint8_t {CONDITION, CLAMP, OFF}; // integral anti-windup modes + + float GetKp(); // proportional gain + float GetKi(); // integral gain + float GetKd(); // derivative gain + float GetPterm(); // proportional component of output + float GetIterm(); // integral component of output + float GetDterm(); // derivative component of output + uint8_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) + uint8_t GetDirection(); // DIRECT (0), REVERSE (1) + uint8_t GetPmode(); // PE (0), PM (1), PEM (2) + uint8_t GetDmode(); // DE (0), DM (1) + uint8_t GetAwMode(); // CONDITION (0, CLAMP (1), OFF (2) + + Documentation (GitHub): https://github.com/Dlloydev/QuickPID + **************************************************************************************/ + +#include "QuickPID.h" + +const byte inputPin = 0; +const byte outputPin = 3; + +//Define variables we'll be connecting to +float Setpoint, Input, Output, Kp = 2, Ki = 5, Kd = 1; + +/* pMode dMode awMode Action */ +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.PM, myPID.DM, myPID.CLAMP, myPID.DIRECT); +/* PE DE CONDITION DIRECT + PM DM CLAMP REVERSE + PEM OFF +*/ +void setup() +{ + Serial.begin(115200); + delay(2000); + + myPID.SetMode(QuickPID::AUTOMATIC); // controller modes + myPID.SetSampleTimeUs(100000); + myPID.SetTunings(Kp, Ki, Kd); + + Setpoint = 50; + Input = analogRead(inputPin); + myPID.Compute(); + analogWrite(outputPin, 10); + + Serial.println(); + Serial.print(F(" Pterm: ")); Serial.println(myPID.GetPterm()); + Serial.print(F(" Iterm: ")); Serial.println(myPID.GetIterm()); + Serial.print(F(" Dterm: ")); Serial.println(myPID.GetDterm()); + Serial.print(F(" Mode: ")); Serial.println(myPID.GetMode()); + Serial.print(F(" Direction: ")); Serial.println(myPID.GetDirection()); + Serial.print(F(" Pmode: ")); Serial.println(myPID.GetPmode()); + Serial.print(F(" Dmode: ")); Serial.println(myPID.GetDmode()); + Serial.print(F(" AwMode: ")); Serial.println(myPID.GetAwMode()); + + analogWrite(outputPin, 0); +} + +void loop() +{ +} diff --git a/examples/PID_POn_DOn_Error_Measurement/PID_POn_DOn_Error_Measurement.ino b/examples/PID_POn_DOn_Error_Measurement/PID_POn_DOn_Error_Measurement.ino deleted file mode 100644 index 9ff0daa..0000000 --- a/examples/PID_POn_DOn_Error_Measurement/PID_POn_DOn_Error_Measurement.ino +++ /dev/null @@ -1,34 +0,0 @@ -/************************************************************************************** - Proportional and Derivative on the ratio of Error to Measurement Example - Increasing the proportional on measurement setting will make the output move more - smoothly when the setpoint is changed. Also, it can eliminate overshoot. - Decreasing the derivative on measurement adds more derivative on error. This reduces - reduce overshoot but may increase output spikes. Adjust to suit your requirements. - **************************************************************************************/ - -#include "QuickPID.h" - -//Define Variables we'll be connecting to -float Setpoint, Input, Output; -float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 -float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 - -//Specify the links and initial tuning parameters -QuickPID myQuickPID(&Input, &Output, &Setpoint, 2, 5, 1, POn, DOn, QuickPID::DIRECT); - -void setup() -{ - //initialize the variables we're linked to - Input = myQuickPID.analogReadFast(0); - Setpoint = 100; - - //turn the PID on - myQuickPID.SetMode(QuickPID::AUTOMATIC); -} - -void loop() -{ - Input = myQuickPID.analogReadFast(0); - myQuickPID.Compute(); - analogWrite(3, Output); -} diff --git a/examples/PID_RelayOutput/PID_RelayOutput.ino b/examples/PID_RelayOutput/PID_RelayOutput.ino index f281eb1..f351818 100644 --- a/examples/PID_RelayOutput/PID_RelayOutput.ino +++ b/examples/PID_RelayOutput/PID_RelayOutput.ino @@ -25,10 +25,8 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 -float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 -QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); unsigned int WindowSize = 5000; unsigned int minWindow = 500; @@ -43,10 +41,10 @@ void setup() Setpoint = 100; //tell the PID to range between 0 and the full window size - myQuickPID.SetOutputLimits(0, WindowSize); + myPID.SetOutputLimits(0, WindowSize); //turn the PID on - myQuickPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(QuickPID::AUTOMATIC); } void loop() @@ -59,7 +57,7 @@ void loop() if (millis() - windowStartTime >= WindowSize) { //time to shift the Relay Window windowStartTime += WindowSize; - myQuickPID.Compute(); + myPID.Compute(); } if (((unsigned int)Output > minWindow) && ((unsigned int)Output > (millis() - windowStartTime))) digitalWrite(RELAY_PIN, HIGH); else digitalWrite(RELAY_PIN, LOW); diff --git a/keywords.txt b/keywords.txt index 886db57..e9a367e 100644 --- a/keywords.txt +++ b/keywords.txt @@ -7,7 +7,7 @@ ########################################## QuickPID KEYWORD1 -myQuickPID KEYWORD1 +myPID KEYWORD1 ########################################## # Methods and Functions (KEYWORD2) @@ -15,7 +15,6 @@ myQuickPID KEYWORD1 SetMode KEYWORD2 Compute KEYWORD2 -AutoTune KEYWORD2 SetOutputLimits KEYWORD2 SetTunings KEYWORD2 SetControllerDirection KEYWORD2 @@ -28,8 +27,9 @@ GetIterm KEYWORD2 GetDterm KEYWORD2 GetMode KEYWORD2 GetDirection KEYWORD2 -analogReadFast KEYWORD2 -analogReadAvg KEYWORD2 +GetPmode KEYWORD2 +GetDmode KEYWORD2 +GetAwMode KEYWORD2 analogWrite KEYWORD2 analogWriteFrequency KEYWORD2 analogWriteResolution KEYWORD2 @@ -42,7 +42,12 @@ AUTOMATIC LITERAL1 MANUAL LITERAL1 TIMER LITERAL1 DIRECT LITERAL1 -REVERSE LITERAL1 -mode_t LITERAL1 -direction_t LITERAL1 -analog_write_channel_t LITERAL1 +REV LITERAL1 +PE LITERAL1 +PM LITERAL1 +PEM LITERAL1 +CONDITION LITERAL1 +CLAMP LITERAL1 +OFF LITERAL1 +DE LITERAL1 +DM LITERAL1 diff --git a/library.json b/library.json index 009a301..4e88fe7 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", - "version": "2.5.0", - "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, conditional anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", + "version": "3.0.0", + "description": "A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": { @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~2.5.0" + "QuickPID": "~3.0.0" }, "frameworks": "arduino", "platforms": "*" diff --git a/library.properties b/library.properties index c43975a..39fab21 100644 --- a/library.properties +++ b/library.properties @@ -1,8 +1,8 @@ name=QuickPID -version=2.5.0 +version=3.0.0 author=David Lloyd maintainer=David Lloyd -sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, conditional anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. +sentence=A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. paragraph=Also includes analogWrite compatibility for ESP32 and ESP32-S2. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 40d5bf6..0b9f91b 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,8 +1,7 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 2.5.0 + QuickPID Library for Arduino - Version 3.0.0 by dlloydev https://github.com/Dlloydev/QuickPID - Based on the Arduino PID Library and work on AutoTunePID class - by gnalbandian (Gonzalo). Licensed under the MIT License. + Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ #if ARDUINO >= 100 @@ -18,8 +17,8 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, float POn = 1.0, float DOn = 0.0, - QuickPID::direction_t ControllerDirection = DIRECT) { + float Kp, float Ki, float Kd, uint8_t pMode = PE, uint8_t dMode = DM, + uint8_t awMode = CLAMP, uint8_t Action = DIRECT) { myOutput = Output; myInput = Input; @@ -28,8 +27,8 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, QuickPID::SetOutputLimits(0, 255); // same default as Arduino PWM limit sampleTimeUs = 100000; // 0.1 sec default - QuickPID::SetControllerDirection(ControllerDirection); - QuickPID::SetTunings(Kp, Ki, Kd, POn, DOn); + QuickPID::SetControllerDirection(Action); + QuickPID::SetTunings(Kp, Ki, Kd, pMode, dMode, awMode); lastTime = micros() - sampleTimeUs; } @@ -38,8 +37,8 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, To allow using Proportional on Error without explicitly saying so. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, direction_t ControllerDirection = DIRECT) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1.0, dOn = 0.0, ControllerDirection = DIRECT) { + float Kp, float Ki, float Kd, uint8_t Action = DIRECT) + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pmode = PE, dmode = DM, awmode = CLAMP, Action = DIRECT) { } /* Compute() *********************************************************************** @@ -55,29 +54,39 @@ bool QuickPID::Compute() { float input = *myInput; float dInput = input - lastInput; - if (controllerDirection == REVERSE) dInput = -dInput; + if (action == REVERSE) dInput = -dInput; error = *mySetpoint - input; - if (controllerDirection == REVERSE) error = -error; + if (action == REVERSE) error = -error; float dError = error - lastError; - pmTerm = kpm * dInput; - peTerm = kpe * error; + float peTerm = kp * error; + float pmTerm = kp * dInput; + if (pmode == PE) pmTerm = 0; + else if (pmode == PM) peTerm = 0; + else { //PEM + peTerm *= 0.5; + pmTerm *= 0.5; + } + pTerm = peTerm - pmTerm; // used by GetDterm() iTerm = ki * error; - dmTerm = kdm * dInput; - deTerm = kde * dError; - - //conditional anti-windup - bool aw = false; - float iTermOut = (peTerm - pmTerm) + ki * (iTerm + error); - if (iTermOut > outMax && dError > 0) aw = true; - else if (iTermOut < outMin && dError < 0) aw = true; - if (aw && ki) iTerm = constrain(iTermOut, -outMax, outMax); - - //compute output as per PID_v1 - outputSum += iTerm; // include integral amount - outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp - *myOutput = constrain(outputSum + peTerm - dmTerm + deTerm, outMin, outMax); // totalize, clamp, drive output + if (dmode == DE) dTerm = kd * dError; + else dTerm = -kd * dInput; // DM + + //condition anti-windup + if (awmode == CONDITION) { + bool aw = false; + float iTermOut = (peTerm - pmTerm) + ki * (iTerm + error); + if (iTermOut > outMax && dError > 0) aw = true; + else if (iTermOut < outMin && dError < 0) aw = true; + if (aw && ki) iTerm = constrain(iTermOut, -outMax, outMax); + } + + // by default, compute output as per PID_v1 + outputSum += iTerm; // include integral amount + if (awmode == OFF) outputSum -= pmTerm; // include pmTerm (no anti-windup) + else outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp (default) + *myOutput = constrain(outputSum + peTerm + dTerm, outMin, outMax); // include dTerm, clamp and drive output lastError = error; lastInput = input; @@ -92,26 +101,21 @@ bool QuickPID::Compute() { it's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. ******************************************************************************/ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1.0, float DOn = 0.0) { +void QuickPID::SetTunings(float Kp, float Ki, float Kd, uint8_t pMode = PE, uint8_t dMode = DM, uint8_t awMode = CLAMP) { if (Kp < 0 || Ki < 0 || Kd < 0) return; - pOn = POn; - dOn = DOn; + pmode = pMode; dmode = dMode; awmode = awMode; dispKp = Kp; dispKi = Ki; dispKd = Kd; float SampleTimeSec = (float)sampleTimeUs / 1000000; kp = Kp; ki = Ki * SampleTimeSec; kd = Kd / SampleTimeSec; - kpe = kp * pOn; - kpm = kp * (1 - pOn); - kde = kd * dOn; - kdm = kd * (1 - dOn); } /* SetTunings(...)************************************************************ - Set Tunings using the last remembered POn and DOn setting. + Set Tunings using the last remembered pMode and dMode setting. ******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { - SetTunings(Kp, Ki, Kd, pOn, dOn); + SetTunings(Kp, Ki, Kd, pmode, dmode, awmode); } /* SetSampleTime(.)*********************************************************** @@ -146,7 +150,7 @@ void QuickPID::SetOutputLimits(float Min, float Max) { when the transition from MANUAL to AUTOMATIC or TIMER occurs, the controller is automatically initialized. ******************************************************************************/ -void QuickPID::SetMode(mode_t Mode) { +void QuickPID::SetMode(uint8_t Mode) { if (mode == MANUAL && Mode != MANUAL) { // just went from MANUAL to AUTOMATIC or TIMER QuickPID::Initialize(); } @@ -167,13 +171,12 @@ void QuickPID::Initialize() { The PID will either be connected to a DIRECT acting process (+Output leads to +Input) or a REVERSE acting process(+Output leads to -Input). ******************************************************************************/ -void QuickPID::SetControllerDirection(direction_t ControllerDirection) { - controllerDirection = ControllerDirection; +void QuickPID::SetControllerDirection(uint8_t Action) { + action = Action; } /* Status Functions************************************************************ - These functions query the internal state of the PID. They're here for display - purposes. These are the functions the PID Front-end uses for example. + These functions query the internal state of the PID. ******************************************************************************/ float QuickPID::GetKp() { return dispKp; @@ -185,233 +188,26 @@ float QuickPID::GetKd() { return dispKd; } float QuickPID::GetPterm() { - return peTerm + pmTerm; + return pTerm; } float QuickPID::GetIterm() { return iTerm; } float QuickPID::GetDterm() { - return deTerm + dmTerm; + return dTerm; } -QuickPID::mode_t QuickPID::GetMode() { +uint8_t QuickPID::GetMode() { return mode; } -QuickPID::direction_t QuickPID::GetDirection() { - return controllerDirection; -} - -/* AutoTune Functions*********************************************************/ - -void QuickPID::AutoTune(tuningMethod tuningRule) { - autoTune = new AutoTunePID(myInput, myOutput, tuningRule); -} - -void QuickPID::clearAutoTune() { - if (autoTune) - delete autoTune; -} - -AutoTunePID::AutoTunePID() { - _input = nullptr; - _output = nullptr; - reset(); -} - -AutoTunePID::AutoTunePID(float *input, float *output, tuningMethod tuningRule) { - AutoTunePID(); - _input = input; - _output = output; - _tuningRule = tuningRule; -} - -void AutoTunePID::reset() { - _tLast = 0; - _t0 = 0; - _t1 = 0; - _t2 = 0; - _t3 = 0; - _Ku = 0.0; - _Tu = 0.0; - _td = 0.0; - _kp = 0.0; - _ki = 0.0; - _kd = 0.0; - _rdAvg = 0.0; - _peakHigh = 0.0; - _peakLow = 0.0; - _autoTuneStage = 0; -} - -void AutoTunePID::autoTuneConfig(const float outputStep, const float hysteresis, const float atSetpoint, - const float atOutput, const bool dir, const bool printOrPlotter, uint32_t sampleTimeUs) -{ - _outputStep = outputStep; - _hysteresis = hysteresis; - _atSetpoint = atSetpoint; - _atOutput = atOutput; - _direction = dir; - _printOrPlotter = printOrPlotter; - _tLoop = constrain((sampleTimeUs / 8), 500, 16383); - _autoTuneStage = STABILIZING; +uint8_t QuickPID::GetDirection() { + return action; } - -byte AutoTunePID::autoTuneLoop() { - if ((micros() - _tLast) <= _tLoop) return WAIT; - else _tLast = micros(); - switch (_autoTuneStage) { - case AUTOTUNE: - return AUTOTUNE; - break; - case WAIT: - return WAIT; - break; - case STABILIZING: - if (_printOrPlotter == 1) Serial.print(F("Stabilizing →")); - _t0 = millis(); - _peakHigh = _atSetpoint; - _peakLow = _atSetpoint; - (!_direction) ? *_output = 0 : *_output = _atOutput + (_outputStep * 2); - _autoTuneStage = COARSE; - return AUTOTUNE; - break; - case COARSE: // coarse adjust - if (millis() - _t0 < 2000) { - return AUTOTUNE; - break; - } - if (*_input < (_atSetpoint - _hysteresis)) { - (!_direction) ? *_output = _atOutput + (_outputStep * 2) : *_output = _atOutput - (_outputStep * 2); - _autoTuneStage = FINE; - } - return AUTOTUNE; - break; - case FINE: // fine adjust - if (*_input > _atSetpoint) { - (!_direction) ? *_output = _atOutput - _outputStep : *_output = _atOutput + _outputStep; - _autoTuneStage = TEST; - } - return AUTOTUNE; - break; - case TEST: // run AutoTune relay method - if (*_input < _atSetpoint) { - if (_printOrPlotter == 1) Serial.print(F(" AutoTune →")); - (!_direction) ? *_output = _atOutput + _outputStep : *_output = _atOutput - _outputStep; - _autoTuneStage = T0; - } - return AUTOTUNE; - break; - case T0: // get t0 - if (*_input > _atSetpoint) { - _t0 = micros(); - if (_printOrPlotter == 1) Serial.print(F(" t0 →")); - _inputLast = *_input; - _autoTuneStage = T1; - } - return AUTOTUNE; - break; - case T1: // get t1 - if ((*_input > _atSetpoint) && (*_input > _inputLast)) { - _t1 = micros(); - if (_printOrPlotter == 1) Serial.print(F(" t1 →")); - _autoTuneStage = T2; - } - return AUTOTUNE; - break; - case T2: // get t2 - _rdAvg = *_input; - if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; - if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; - if (_rdAvg > _atSetpoint + _hysteresis) { - _t2 = micros(); - if (_printOrPlotter == 1) Serial.print(F(" t2 →")); - (!_direction) ? *_output = _atOutput - _outputStep : *_output = _atOutput + _outputStep; - _autoTuneStage = T3L; - } - return AUTOTUNE; - break; - case T3L: // t3 low cycle - _rdAvg = *_input; - if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; - if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; - if (_rdAvg < _atSetpoint - _hysteresis) { - (!_direction) ? *_output = _atOutput + _outputStep : *_output = _atOutput - _outputStep; - _autoTuneStage = T3H; - } - return AUTOTUNE; - break; - case T3H: // t3 high cycle, relay test done - _rdAvg = *_input; - if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; - if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; - if (_rdAvg > _atSetpoint + _hysteresis) { - _t3 = micros(); - if (_printOrPlotter == 1) Serial.println(F(" t3 → done.")); - _autoTuneStage = CALC; - } - return AUTOTUNE; - break; - case CALC: // calculations - _td = (float)(_t1 - _t0) / 1000000.0; // dead time (seconds) - _Tu = (float)(_t3 - _t2) / 1000000.0; // ultimate period (seconds) - _Ku = (float)(4 * _outputStep * 2) / (float)(3.14159 * sqrt (sq (_peakHigh - _peakLow) - sq (_hysteresis))); // ultimate gain - if (_tuningRule == tuningMethod::AMIGOF_PID) { - (_td < 0.1) ? _td = 0.1 : _td = _td; - _kp = (0.2 + 0.45 * (_Tu / _td)) / _Ku; - float Ti = (((0.4 * _td) + (0.8 * _Tu)) / (_td + (0.1 * _Tu)) * _td); - float Td = (0.5 * _td * _Tu) / ((0.3 * _td) + _Tu); - _ki = _kp / Ti; - _kd = Td * _kp; - } else { //other rules - _kp = (float)(RulesContants[static_cast(_tuningRule)][0] / 1000.0) * _Ku; - _ki = (float)(RulesContants[static_cast(_tuningRule)][1] / 1000.0) * (_Ku / _Tu); - _kd = (float)(RulesContants[static_cast(_tuningRule)][2] / 1000.0) * (_Ku * _Tu); - } - if (_printOrPlotter == 1) { - // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png - if ((_Tu / _td + 0.0001) > 0.75) Serial.println(F("This process is easy to control.")); - else if ((_Tu / _td + 0.0001) > 0.25) Serial.println(F("This process has average controllability.")); - else Serial.println(F("This process is difficult to control.")); - Serial.print(F("Tu: ")); Serial.print(_Tu); // Ultimate Period (sec) - Serial.print(F(" td: ")); Serial.print(_td); // Dead Time (sec) - Serial.print(F(" Ku: ")); Serial.print(_Ku); // Ultimate Gain - Serial.print(F(" Kp: ")); Serial.print(_kp); - Serial.print(F(" Ki: ")); Serial.print(_ki); - Serial.print(F(" Kd: ")); Serial.println(_kd); - Serial.println(); - } - _autoTuneStage = TUNINGS; - return AUTOTUNE; - break; - case TUNINGS: - _autoTuneStage = CLR; - return TUNINGS; - break; - case CLR: - return CLR; - break; - default: - return CLR; - break; - } - return CLR; +uint8_t QuickPID::GetPmode() { + return pmode; } - -void AutoTunePID::setAutoTuneConstants(float * kp, float * ki, float * kd) { - *kp = _kp; - *ki = _ki; - *kd = _kd; +uint8_t QuickPID::GetDmode() { + return dmode; } - -/* Utility************************************************************/ -// https://github.com/avandalen/avdweb_AnalogReadFast -int QuickPID::analogReadFast(int ADCpin) { -#if defined(__AVR_ATmega328P__) - byte ADCregOriginal = ADCSRA; - ADCSRA = (ADCSRA & B11111000) | 5; // 32 prescaler - int adc = analogRead(ADCpin); - ADCSRA = ADCregOriginal; - return adc; -#else - return analogRead(ADCpin); -# endif +uint8_t QuickPID::GetAwMode() { + return awmode; } diff --git a/src/QuickPID.h b/src/QuickPID.h index 108bcdd..16fc05f 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -2,92 +2,31 @@ #ifndef QuickPID_h #define QuickPID_h -enum class tuningMethod : uint8_t -{ - ZIEGLER_NICHOLS_PI, - ZIEGLER_NICHOLS_PID, - TYREUS_LUYBEN_PI, - TYREUS_LUYBEN_PID, - CIANCONE_MARLIN_PI, - CIANCONE_MARLIN_PID, - AMIGOF_PID, - PESSEN_INTEGRAL_PID, - SOME_OVERSHOOT_PID, - NO_OVERSHOOT_PID -}; - -class AutoTunePID { - public: - AutoTunePID(); - AutoTunePID(float *input, float *output, tuningMethod tuningRule); - ~AutoTunePID() {}; - - void reset(); - void autoTuneConfig(const float outputStep, const float hysteresis, const float setpoint, const float output, - const bool dir = false, const bool printOrPlotter = false, uint32_t sampleTimeUs = 10000); - byte autoTuneLoop(); - void setAutoTuneConstants(float* kp, float* ki, float* kd); - enum atStage : byte { AUTOTUNE, WAIT, STABILIZING, COARSE, FINE, TEST, T0, T1, T2, T3L, T3H, CALC, TUNINGS, CLR }; - - private: - - float *_input = nullptr; // Pointers to the Input, Output, and Setpoint variables. This creates a - float *_output = nullptr; // hard link between the variables and the PID, freeing the user from having - // float *mySetpoint = nullptr; // to constantly tell us what these values are. With pointers we'll just know. - - byte _autoTuneStage = 1; - tuningMethod _tuningRule; - bool _direction = false; - bool _printOrPlotter = false; - uint32_t _tLoop, _tLast, _t0, _t1, _t2, _t3; - float _outputStep, _hysteresis, _atSetpoint, _atOutput; - float _Ku, _Tu, _td, _kp, _ki, _kd, _rdAvg, _peakHigh, _peakLow, _inputLast; - - const uint16_t RulesContants[10][3] = - { //ckp, cki, ckd x 1000 - { 450, 540, 0 }, // ZIEGLER_NICHOLS_PI - { 600, 176, 75 }, // ZIEGLER_NICHOLS_PID - { 313, 142, 0 }, // TYREUS_LUYBEN_PI - { 454, 206, 72 }, // TYREUS_LUYBEN_PID - { 303, 1212, 0 }, // CIANCONE_MARLIN_PI - { 303, 1333, 37 }, // CIANCONE_MARLIN_PID - { 0, 0, 0 }, // AMIGOF_PID - { 700, 1750, 105 }, // PESSEN_INTEGRAL_PID - { 333, 667, 111 }, // SOME_OVERSHOOT_PID - { 333, 100, 67 } // NO_OVERSHOOT_PID - }; - -}; // class AutoTunePID - class QuickPID { public: - // controller mode - typedef enum { MANUAL = 0, AUTOMATIC = 1, TIMER = 2 } mode_t; - - // DIRECT: intput increases when the error is positive. REVERSE: intput decreases when the error is positive. - typedef enum { DIRECT = 0, REVERSE = 1 } direction_t; + enum Mode : uint8_t {MANUAL, AUTOMATIC, TIMER}; // controller modes + enum Action : uint8_t {DIRECT, REVERSE}; // controller actions + enum pMode : uint8_t {PE, PM, PEM}; // proportional modes + enum dMode : uint8_t {DE, DM}; // derivative modes + enum awMode : uint8_t {CONDITION, CLAMP, OFF}; // integral anti-windup modes // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, float POn, float DOn, direction_t ControllerDirection); + QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, uint8_t pMode, uint8_t dMode, uint8_t awMode, uint8_t Action); // Overload constructor with proportional ratio. Links the PID to Input, Output, Setpoint and Tuning Parameters. - QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, direction_t ControllerDirection); + QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, uint8_t Action); // Sets PID mode to MANUAL (0), AUTOMATIC (1) or TIMER (2). - void SetMode(mode_t Mode); + void SetMode(uint8_t Mode); // Performs the PID calculation. It should be called every time loop() cycles ON/OFF and calculation frequency // can be set using SetMode and SetSampleTime respectively. bool Compute(); - // Automatically determines and sets the tuning parameters Kp, Ki and Kd. Use this in the setup loop. - void AutoTune(tuningMethod tuningRule); - void clearAutoTune(); - // Sets and clamps the output to a specific range (0-255 by default). void SetOutputLimits(float Min, float Max); @@ -98,28 +37,27 @@ class QuickPID { void SetTunings(float Kp, float Ki, float Kd); // Overload for specifying proportional ratio. - void SetTunings(float Kp, float Ki, float Kd, float POn, float DOn); + void SetTunings(float Kp, float Ki, float Kd, uint8_t pMode, uint8_t dMode, uint8_t awMode); // Sets the controller Direction or Action. DIRECT means the output will increase when the error is positive. // REVERSE means the output will decrease when the error is positive. - void SetControllerDirection(direction_t ControllerDirection); + void SetControllerDirection(uint8_t Action); // Sets the sample time in microseconds with which each PID calculation is performed. Default is 100000 µs. void SetSampleTimeUs(uint32_t NewSampleTimeUs); // PID Query functions *********************************************************************************** - float GetKp(); // proportional gain - float GetKi(); // integral gain - float GetKd(); // derivative gain - float GetPterm(); // proportional component of output - float GetIterm(); // integral component of output - float GetDterm(); // derivative component of output - mode_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) - direction_t GetDirection(); // DIRECT (0) or REVERSE (1) - - int analogReadFast(int ADCpin); - - AutoTunePID *autoTune; + float GetKp(); // proportional gain + float GetKi(); // integral gain + float GetKd(); // derivative gain + float GetPterm(); // proportional component of output + float GetIterm(); // integral component of output + float GetDterm(); // derivative component of output + uint8_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) + uint8_t GetDirection(); // DIRECT (0), REVERSE (1) + uint8_t GetPmode(); // PE (0), PM (1), PEM (2) + uint8_t GetDmode(); // DE (0), DM (1) + uint8_t GetAwMode(); // CONDITION (0, CLAMP (1), OFF (2) private: @@ -128,31 +66,26 @@ class QuickPID { float dispKp; // tuning parameters for display purposes. float dispKi; float dispKd; - float peTerm; - float pmTerm; + float pTerm; float iTerm; - float deTerm; - float dmTerm; + float dTerm; - float pOn; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 - float dOn; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 float kp; // (P)roportional Tuning Parameter float ki; // (I)ntegral Tuning Parameter float kd; // (D)erivative Tuning Parameter - float kpe; // proportional on error amount - float kpm; // proportional on measurement amount - float kde; // derivative on error amount - float kdm; // derivative on measurement amount float *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a float *myOutput; // hard link between the variables and the PID, freeing the user from having float *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. - mode_t mode = MANUAL; - direction_t controllerDirection; + uint8_t mode = MANUAL; + uint8_t action = DIRECT; + uint8_t pmode = PE; + uint8_t dmode = DM; + uint8_t awmode = CONDITION; + uint32_t sampleTimeUs, lastTime; float outputSum, outMin, outMax, error, lastError, lastInput; - bool inAuto; }; // class QuickPID From add2c3f18dcb6250d71ef1be43d25e6127fa38c5 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 13 Dec 2021 10:30:17 -0500 Subject: [PATCH 070/101] Update README.md --- README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 6bf6fcb..c02d8fd 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library. However, the additional features like integral anti-windup based on conditional-conditioning, clamping, or turning it off completely. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. The controller modes includes `TIMER`, which allows external timer or ISR timing control. +QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library. One of the additional features includes integral anti-windup which can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include `TIMER`, which allows external timer or ISR timing control. ### Features @@ -37,8 +37,8 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. - `pMode` is the proportional mode parameter with options for `PE` proportional on error (default), `PM` proportional on measurement and `PEM` which is 0.5 `PE` + 0.5 `PM`. - `dMode` is the derivative mode parameter with options for `DE` derivative on error (default), `DM` derivative on measurement (default). -- `awMode` is the integral anti-windup parameter with options for `CONDITION` which is based on PI terms to provide some integral correction and prevent deep saturation, `CLAMP` (default) which clamps the summation of the pmTerm and iTerm. The `OFF` option turns off all anti-windup. -- `Action` is the controller action parameter which has `DIRECT` (default) and `REVERSE` options. These options set how the controller responds to a change in input. `DIRECT` action will lead the output to increase when the input is larger than the setpoint (i.e. heating process). `REVERSE` action will lead the output to decrease when the input is larger than the setpoint (i.e. cooling process). +- `awMode` is the integral anti-windup parameter with an option for `CONDITION` which is based on PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. The`CLAMP` option (default), clamps the summation of the pmTerm and iTerm. The `OFF` option turns off all anti-windup. +- `Action` is the controller action parameter which has `DIRECT` (default) and `REVERSE` options. These options set how the controller responds to a change in input. `DIRECT` action is used if the input moves in the same direction as the controller output (i.e. heating process). `REVERSE` action is used if the input moves in the opposite direction as the controller output (i.e. cooling process). ```c++ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, @@ -67,7 +67,7 @@ This function allows the controller's dynamic performance to be adjusted. It's c void QuickPID::SetTunings(float Kp, float Ki, float Kd); ``` -Set Tunings using the last remembered POn and DOn settings. See example [PID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino) +Set Tunings using the last remembered `pMode`, `dMode` and `awMode` settings. See example [PID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino) #### SetSampleTime @@ -75,7 +75,7 @@ Set Tunings using the last remembered POn and DOn settings. See example [PID_Ada void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs); ``` -Sets the period, in microseconds, at which the calculation is performed. The default is 100ms. +Sets the period, in microseconds, at which the calculation is performed. The default is 100000µs (100ms). #### SetOutputLimits @@ -127,14 +127,14 @@ The PID will either be connected to a `DIRECT` acting process (+Output leads to uint8_t GetDirection(); // DIRECT (0), REVERSE (1) uint8_t GetPmode(); // PE (0), PM (1), PEM (2) uint8_t GetDmode(); // DE (0), DM (1) - uint8_t GetAwMode(); // CONDITION (0, CLAMP (1), OFF (2) + uint8_t GetAwMode(); // CONDITION (0), CLAMP (1), OFF (2) ``` These functions query the internal state of the PID. #### [AnalogWrite (PWM and DAC) for ESP32](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) -Use this link for reference. Note that if you're using QuickPID, there's no need to install the AnalogWrite library as this feature is already included. +If you're using QuickPID with an ESP32 and need analogWrite compatibility, there's no need to install a library as this feature is already included. ### Original README (Arduino PID) From 46156693bda960807e2ccab16ff2cc0142f9fc37 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 14 Dec 2021 03:20:02 -0500 Subject: [PATCH 071/101] Use enum class Resolves issue #37 Documentation and examples updated. --- README.md | 15 ++--- .../PID_AVR_Basic_Interrupt_TIMER.ino | 4 +- .../PID_AVR_Basic_Software_TIMER.ino | 4 +- .../PID_AdaptiveTunings.ino | 4 +- examples/PID_Basic/PID_Basic.ino | 4 +- .../PID_Controller_Options.ino | 53 ++++++++-------- examples/PID_RelayOutput/PID_RelayOutput.ino | 4 +- library.json | 4 +- library.properties | 2 +- src/QuickPID.cpp | 61 ++++++++++--------- src/QuickPID.h | 32 +++++----- 11 files changed, 99 insertions(+), 88 deletions(-) diff --git a/README.md b/README.md index c02d8fd..e4a9a8c 100644 --- a/README.md +++ b/README.md @@ -29,8 +29,8 @@ When the controller is set to `REVERSE` acting, the sign of the `error` and `dIn ```c++ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, uint8_t pMode = PE, uint8_t dMode = DM, - uint8_t awMode = CLAMP, uint8_t Action = DIRECT) + float Kp, float Ki, float Kd, pMode pMode = pMode::PE, dMode dMode = dMode::DM, + awMode awMode = awMode::CONDITION, Action action = Action::DIRECT); ``` - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. @@ -41,8 +41,8 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - `Action` is the controller action parameter which has `DIRECT` (default) and `REVERSE` options. These options set how the controller responds to a change in input. `DIRECT` action is used if the input moves in the same direction as the controller output (i.e. heating process). `REVERSE` action is used if the input moves in the opposite direction as the controller output (i.e. cooling process). ```c++ -QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, uint8_t Action = DIRECT) +QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, + Action action); ``` This allows you to use Proportional on Error without explicitly saying so. @@ -58,7 +58,8 @@ This function contains the PID algorithm and it should be called once every loop #### SetTunings ```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, uint8_t pMode = PE, uint8_t dMode = DM, uint8_t awMode = CLAMP) +void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::PE, dMode dMode = dMode::DM, + awMode awMode = awMode::CONDITION); ``` This function allows the controller's dynamic performance to be adjusted. It's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. The parameters are as described in the constructor. @@ -88,7 +89,7 @@ The PID controller is designed to vary its output within a given range. By defa #### SetMode ```c++ -void QuickPID::SetMode(uint8_t Mode) +void QuickPID::SetMode(Control Mode); ``` Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (1) or `TIMER` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. @@ -109,7 +110,7 @@ Does all the things that need to happen to ensure a bump-less transfer from manu #### SetControllerDirection ```c++ -void QuickPID::SetControllerDirection(uint8_t Action) +void QuickPID::SetControllerDirection(Action Action); ``` The PID will either be connected to a `DIRECT` acting process (+Output leads to +Input) or a `REVERSE` acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. diff --git a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino index 5ec40a5..71ebfdc 100644 --- a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino +++ b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino @@ -16,7 +16,7 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::DIRECT); void setup() { Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval @@ -27,7 +27,7 @@ void setup() { Setpoint = 100; //turn the PID on - myPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(myPID.Control::AUTOMATIC); } void loop() { diff --git a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino index c33c570..3954177 100644 --- a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino +++ b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino @@ -18,7 +18,7 @@ float Setpoint, Input, Output; float Kp = 2, Ki = 5, Kd = 1; Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::DIRECT); void setup() { timer1.start(); @@ -28,7 +28,7 @@ void setup() { Setpoint = 100; //turn the PID on - myPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(myPID.Control::AUTOMATIC); } void loop() { diff --git a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino index 9d60f4c..f6b5045 100644 --- a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino +++ b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino @@ -22,7 +22,7 @@ float aggKp = 4, aggKi = 0.2, aggKd = 1; float consKp = 1, consKi = 0.05, consKd = 0.25; //Specify the links and initial tuning parameters -QuickPID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, myPID.Action::DIRECT); void setup() { @@ -31,7 +31,7 @@ void setup() Setpoint = 100; //turn the PID on - myPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(myPID.Control::AUTOMATIC); } void loop() diff --git a/examples/PID_Basic/PID_Basic.ino b/examples/PID_Basic/PID_Basic.ino index da2875d..0290987 100644 --- a/examples/PID_Basic/PID_Basic.ino +++ b/examples/PID_Basic/PID_Basic.ino @@ -14,7 +14,7 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::DIRECT); void setup() { @@ -23,7 +23,7 @@ void setup() Setpoint = 100; //turn the PID on - myPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(myPID.Control::AUTOMATIC); } void loop() diff --git a/examples/PID_Controller_Options/PID_Controller_Options.ino b/examples/PID_Controller_Options/PID_Controller_Options.ino index 8263834..df91f37 100644 --- a/examples/PID_Controller_Options/PID_Controller_Options.ino +++ b/examples/PID_Controller_Options/PID_Controller_Options.ino @@ -1,11 +1,11 @@ /************************************************************************************** For testing and checking parameter options. From QuickPID.h: - enum Mode : uint8_t {MANUAL, AUTOMATIC, TIMER}; // controller modes - enum Action : uint8_t {DIRECT, REVERSE}; // controller actions - enum pMode : uint8_t {PE, PM, PEM}; // proportional modes - enum dMode : uint8_t {DE, DM}; // derivative modes - enum awMode : uint8_t {CONDITION, CLAMP, OFF}; // integral anti-windup modes + enum class Mode : uint8_t {MANUAL, AUTOMATIC, TIMER}; // controller modes + enum class Action : uint8_t {DIRECT, REVERSE}; // controller actions + enum class pMode : uint8_t {PE, PM, PEM}; // proportional modes + enum class dMode : uint8_t {DE, DM}; // derivative modes + enum class awMode : uint8_t {CONDITION, CLAMP, OFF}; // integral anti-windup modes float GetKp(); // proportional gain float GetKi(); // integral gain @@ -17,8 +17,8 @@ uint8_t GetDirection(); // DIRECT (0), REVERSE (1) uint8_t GetPmode(); // PE (0), PM (1), PEM (2) uint8_t GetDmode(); // DE (0), DM (1) - uint8_t GetAwMode(); // CONDITION (0, CLAMP (1), OFF (2) - + uint8_t GetAwMode(); // CONDITION (0), CLAMP (1), OFF (2) + Documentation (GitHub): https://github.com/Dlloydev/QuickPID **************************************************************************************/ @@ -27,38 +27,43 @@ const byte inputPin = 0; const byte outputPin = 3; + //Define variables we'll be connecting to -float Setpoint, Input, Output, Kp = 2, Ki = 5, Kd = 1; +float Setpoint = 0, Input = 0, Output = 0, Kp = 2, Ki = 5, Kd = 1; -/* pMode dMode awMode Action */ -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.PM, myPID.DM, myPID.CLAMP, myPID.DIRECT); -/* PE DE CONDITION DIRECT - PM DM CLAMP REVERSE - PEM OFF +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.pMode::PE, myPID.dMode::DM, myPID.awMode::CONDITION, myPID.Action::DIRECT); +/* PE DE CONDITION DIRECT + PM DM CLAMP REVERSE + PEM OFF */ + void setup() { Serial.begin(115200); delay(2000); - myPID.SetMode(QuickPID::AUTOMATIC); // controller modes + myPID.SetOutputLimits(0, 255); myPID.SetSampleTimeUs(100000); myPID.SetTunings(Kp, Ki, Kd); + myPID.SetMode(myPID.Control::AUTOMATIC); - Setpoint = 50; Input = analogRead(inputPin); myPID.Compute(); - analogWrite(outputPin, 10); + + analogWrite(outputPin, Output); Serial.println(); - Serial.print(F(" Pterm: ")); Serial.println(myPID.GetPterm()); - Serial.print(F(" Iterm: ")); Serial.println(myPID.GetIterm()); - Serial.print(F(" Dterm: ")); Serial.println(myPID.GetDterm()); - Serial.print(F(" Mode: ")); Serial.println(myPID.GetMode()); - Serial.print(F(" Direction: ")); Serial.println(myPID.GetDirection()); - Serial.print(F(" Pmode: ")); Serial.println(myPID.GetPmode()); - Serial.print(F(" Dmode: ")); Serial.println(myPID.GetDmode()); - Serial.print(F(" AwMode: ")); Serial.println(myPID.GetAwMode()); + Serial.print(F(" Setpoint: ")); Serial.println(Setpoint); + Serial.print(F(" Input: ")); Serial.println(Input); + Serial.print(F(" Output: ")); Serial.println(Output); + Serial.print(F(" Pterm: ")); Serial.println(myPID.GetPterm()); + Serial.print(F(" Iterm: ")); Serial.println(myPID.GetIterm()); + Serial.print(F(" Dterm: ")); Serial.println(myPID.GetDterm()); + Serial.print(F(" Control: ")); Serial.println(myPID.GetMode()); + Serial.print(F(" Action: ")); Serial.println(myPID.GetDirection()); + Serial.print(F(" Pmode: ")); Serial.println(myPID.GetPmode()); + Serial.print(F(" Dmode: ")); Serial.println(myPID.GetDmode()); + Serial.print(F(" AwMode: ")); Serial.println(myPID.GetAwMode()); analogWrite(outputPin, 0); } diff --git a/examples/PID_RelayOutput/PID_RelayOutput.ino b/examples/PID_RelayOutput/PID_RelayOutput.ino index f351818..696d007 100644 --- a/examples/PID_RelayOutput/PID_RelayOutput.ino +++ b/examples/PID_RelayOutput/PID_RelayOutput.ino @@ -26,7 +26,7 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::Action::DIRECT); unsigned int WindowSize = 5000; unsigned int minWindow = 500; @@ -44,7 +44,7 @@ void setup() myPID.SetOutputLimits(0, WindowSize); //turn the PID on - myPID.SetMode(QuickPID::AUTOMATIC); + myPID.SetMode(myPID.Control::AUTOMATIC); } void loop() diff --git a/library.json b/library.json index 4e88fe7..ebb5815 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.0.0", + "version": "3.0.1", "description": "A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~3.0.0" + "QuickPID": "~3.0.1" }, "frameworks": "arduino", "platforms": "*" diff --git a/library.properties b/library.properties index 39fab21..31e7a08 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.0.0 +version=3.0.1 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 0b9f91b..8ff1750 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.0.0 + QuickPID Library for Arduino - Version 3.0.1 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ @@ -17,17 +17,18 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, uint8_t pMode = PE, uint8_t dMode = DM, - uint8_t awMode = CLAMP, uint8_t Action = DIRECT) { + float Kp, float Ki, float Kd, pMode pMode = pMode::PE, dMode dMode = dMode::DM, + awMode awMode = awMode::CONDITION, Action action = Action::DIRECT) { + myOutput = Output; myInput = Input; mySetpoint = Setpoint; - mode = MANUAL; + mode = Control::MANUAL; QuickPID::SetOutputLimits(0, 255); // same default as Arduino PWM limit sampleTimeUs = 100000; // 0.1 sec default - QuickPID::SetControllerDirection(Action); + QuickPID::SetControllerDirection(action); QuickPID::SetTunings(Kp, Ki, Kd, pMode, dMode, awMode); lastTime = micros() - sampleTimeUs; @@ -37,8 +38,9 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, To allow using Proportional on Error without explicitly saying so. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, uint8_t Action = DIRECT) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pmode = PE, dmode = DM, awmode = CLAMP, Action = DIRECT) { + float Kp, float Ki, float Kd, Action action) + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pmode = pMode::PE, dmode = dMode::DM, + awmode = awMode::CONDITION, action = Action::DIRECT) { } /* Compute() *********************************************************************** @@ -47,34 +49,34 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, when the output is computed, false when nothing has been done. **********************************************************************************/ bool QuickPID::Compute() { - if (mode == MANUAL) return false; + if (mode == Control::MANUAL) return false; uint32_t now = micros(); uint32_t timeChange = (now - lastTime); - if (mode == TIMER || timeChange >= sampleTimeUs) { + if (mode == Control::TIMER || timeChange >= sampleTimeUs) { float input = *myInput; float dInput = input - lastInput; - if (action == REVERSE) dInput = -dInput; + if (action == Action::REVERSE) dInput = -dInput; error = *mySetpoint - input; - if (action == REVERSE) error = -error; + if (action == Action::REVERSE) error = -error; float dError = error - lastError; float peTerm = kp * error; float pmTerm = kp * dInput; - if (pmode == PE) pmTerm = 0; - else if (pmode == PM) peTerm = 0; + if (pmode == pMode::PE) pmTerm = 0; + else if (pmode == pMode::PM) peTerm = 0; else { //PEM peTerm *= 0.5; pmTerm *= 0.5; } pTerm = peTerm - pmTerm; // used by GetDterm() iTerm = ki * error; - if (dmode == DE) dTerm = kd * dError; + if (dmode == dMode::DE) dTerm = kd * dError; else dTerm = -kd * dInput; // DM - //condition anti-windup - if (awmode == CONDITION) { + //condition anti-windup (default) + if (awmode == awMode::CONDITION) { bool aw = false; float iTermOut = (peTerm - pmTerm) + ki * (iTerm + error); if (iTermOut > outMax && dError > 0) aw = true; @@ -84,8 +86,8 @@ bool QuickPID::Compute() { // by default, compute output as per PID_v1 outputSum += iTerm; // include integral amount - if (awmode == OFF) outputSum -= pmTerm; // include pmTerm (no anti-windup) - else outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp (default) + if (awmode == awMode::OFF) outputSum -= pmTerm; // include pmTerm (no anti-windup) + else outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp *myOutput = constrain(outputSum + peTerm + dTerm, outMin, outMax); // include dTerm, clamp and drive output lastError = error; @@ -101,7 +103,7 @@ bool QuickPID::Compute() { it's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. ******************************************************************************/ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, uint8_t pMode = PE, uint8_t dMode = DM, uint8_t awMode = CLAMP) { +void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::PE, dMode dMode = dMode::DM, awMode awMode = awMode::CONDITION) { if (Kp < 0 || Ki < 0 || Kd < 0) return; pmode = pMode; dmode = dMode; awmode = awMode; dispKp = Kp; dispKi = Ki; dispKd = Kd; @@ -139,7 +141,7 @@ void QuickPID::SetOutputLimits(float Min, float Max) { outMin = Min; outMax = Max; - if (mode != MANUAL) { + if (mode != Control::MANUAL) { *myOutput = constrain(*myOutput, outMin, outMax); outputSum = constrain(outputSum, outMin, outMax); } @@ -150,8 +152,11 @@ void QuickPID::SetOutputLimits(float Min, float Max) { when the transition from MANUAL to AUTOMATIC or TIMER occurs, the controller is automatically initialized. ******************************************************************************/ -void QuickPID::SetMode(uint8_t Mode) { - if (mode == MANUAL && Mode != MANUAL) { // just went from MANUAL to AUTOMATIC or TIMER + + + +void QuickPID::SetMode(Control Mode) { + if (mode == Control::MANUAL && Mode != Control::MANUAL) { // just went from MANUAL to AUTOMATIC or TIMER QuickPID::Initialize(); } mode = Mode; @@ -171,7 +176,7 @@ void QuickPID::Initialize() { The PID will either be connected to a DIRECT acting process (+Output leads to +Input) or a REVERSE acting process(+Output leads to -Input). ******************************************************************************/ -void QuickPID::SetControllerDirection(uint8_t Action) { +void QuickPID::SetControllerDirection(Action Action) { action = Action; } @@ -197,17 +202,17 @@ float QuickPID::GetDterm() { return dTerm; } uint8_t QuickPID::GetMode() { - return mode; + return static_cast(mode); } uint8_t QuickPID::GetDirection() { - return action; + return static_cast(action); } uint8_t QuickPID::GetPmode() { - return pmode; + return static_cast(pmode); } uint8_t QuickPID::GetDmode() { - return dmode; + return static_cast(dmode); } uint8_t QuickPID::GetAwMode() { - return awmode; + return static_cast(awmode); } diff --git a/src/QuickPID.h b/src/QuickPID.h index 16fc05f..5765bb9 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -6,22 +6,22 @@ class QuickPID { public: - enum Mode : uint8_t {MANUAL, AUTOMATIC, TIMER}; // controller modes - enum Action : uint8_t {DIRECT, REVERSE}; // controller actions - enum pMode : uint8_t {PE, PM, PEM}; // proportional modes - enum dMode : uint8_t {DE, DM}; // derivative modes - enum awMode : uint8_t {CONDITION, CLAMP, OFF}; // integral anti-windup modes + enum class Control : uint8_t {MANUAL, AUTOMATIC, TIMER}; // controller mode + enum class Action : uint8_t {DIRECT, REVERSE}; // controller action + enum class pMode : uint8_t {PE, PM, PEM}; // proportional mode + enum class dMode : uint8_t {DE, DM}; // derivative mode + enum class awMode : uint8_t {CONDITION, CLAMP, OFF}; // integral anti-windup mode // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, uint8_t pMode, uint8_t dMode, uint8_t awMode, uint8_t Action); + QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, pMode pMode, dMode dMode, awMode awMode, Action Action); // Overload constructor with proportional ratio. Links the PID to Input, Output, Setpoint and Tuning Parameters. - QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, uint8_t Action); + QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, Action Action); // Sets PID mode to MANUAL (0), AUTOMATIC (1) or TIMER (2). - void SetMode(uint8_t Mode); + void SetMode(Control mode); // Performs the PID calculation. It should be called every time loop() cycles ON/OFF and calculation frequency // can be set using SetMode and SetSampleTime respectively. @@ -37,16 +37,16 @@ class QuickPID { void SetTunings(float Kp, float Ki, float Kd); // Overload for specifying proportional ratio. - void SetTunings(float Kp, float Ki, float Kd, uint8_t pMode, uint8_t dMode, uint8_t awMode); + void SetTunings(float Kp, float Ki, float Kd, pMode pMode, dMode dMode, awMode awMode); // Sets the controller Direction or Action. DIRECT means the output will increase when the error is positive. // REVERSE means the output will decrease when the error is positive. - void SetControllerDirection(uint8_t Action); + void SetControllerDirection(Action Action); // Sets the sample time in microseconds with which each PID calculation is performed. Default is 100000 µs. void SetSampleTimeUs(uint32_t NewSampleTimeUs); - // PID Query functions *********************************************************************************** + // PID Query functions **************************************************************************************** float GetKp(); // proportional gain float GetKi(); // integral gain float GetKd(); // derivative gain @@ -78,11 +78,11 @@ class QuickPID { float *myOutput; // hard link between the variables and the PID, freeing the user from having float *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. - uint8_t mode = MANUAL; - uint8_t action = DIRECT; - uint8_t pmode = PE; - uint8_t dmode = DM; - uint8_t awmode = CONDITION; + Control mode = Control::MANUAL; + Action action = Action::DIRECT; + pMode pmode = pMode::PE; + dMode dmode = dMode::DM; + awMode awmode = awMode::CONDITION; uint32_t sampleTimeUs, lastTime; float outputSum, outMin, outMax, error, lastError, lastInput; From 37ed16c8b4b5d1d388a8115ca8addc2d6a707c9e Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 14 Dec 2021 23:17:18 -0500 Subject: [PATCH 072/101] Update - enum values are now in camel case, some renamed to be more descriptive - user code is now more descriptive when using the library - tested with both QuickPID and PID_v1 included the same sketch without conflicts - updated documentation and example files --- README.md | 54 ++++++++-------- .../PID_AVR_Basic_Interrupt_TIMER.ino | 4 +- .../PID_AVR_Basic_Software_TIMER.ino | 4 +- .../PID_AdaptiveTunings.ino | 4 +- examples/PID_Basic/PID_Basic.ino | 4 +- .../PID_Controller_Options.ino | 33 +++++----- examples/PID_RelayOutput/PID_RelayOutput.ino | 4 +- keywords.txt | 26 ++++---- library.json | 4 +- library.properties | 2 +- src/QuickPID.cpp | 62 +++++++++---------- src/QuickPID.h | 40 ++++++------ 12 files changed, 117 insertions(+), 124 deletions(-) diff --git a/README.md b/README.md index e4a9a8c..d2f7b4f 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library. One of the additional features includes integral anti-windup which can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include `TIMER`, which allows external timer or ISR timing control. +QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library. One of the additional features includes integral anti-windup which can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include `timer`, which allows external timer or ISR timing control. ### Features @@ -8,41 +8,41 @@ Development began with a fork of the Arduino PID Library. Modifications and new #### New feature Summary -- [x] `TIMER` mode for calling PID compute by an external timer function or ISR +- [x] `timer` mode for calling PID compute by an external timer function or ISR - [x] `analogWrite()` support for ESP32 and ESP32-S2 -- [x] Proportional on error `PE`, measurement `PM` or both `PEM` options -- [x] Derivative on error `DE` and measurement `DM` options +- [x] Proportional on error `pOnError`, measurement `pOnMeas` or both `pOnErrorMeas` options +- [x] Derivative on error `dOnError` and measurement `dOnMeas` options - [x] New PID Query Functions `GetPterm`, `GetIterm`, `GetDterm`, `GetPmode`, `GetDmode` and `GetAwMode` -- [x] New integral anti-windup options `CONDITION`, `CLAMP` and `OFF` -- [x] New `REVERSE` mode only changes sign of `error` and `dInput` +- [x] New integral anti-windup options `iAwCondition`, `iAwClamp` and `iAwOff` +- [x] New `reverse` mode only changes sign of `error` and `dInput` - [x] Uses `float` instead of `double` #### Direct and Reverse Controller Action Direct controller action leads the output to increase when the input is larger than the setpoint (i.e. heating process). Reverse controller leads the output to decrease when the input is larger than the setpoint (i.e. cooling process). -When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). +When the controller is set to `reverse` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `reverse` acting process from a process that's `direct` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). ### Functions #### QuickPID_Constructor ```c++ -QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, pMode pMode = pMode::PE, dMode dMode = dMode::DM, - awMode awMode = awMode::CONDITION, Action action = Action::DIRECT); +QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, + pMode pMode = pMode::pOnError, dMode dMode = dMode::dOnMeas, + iAwMode iAwMode = iAwMode::iAwCondition, Action action = Action::direct) ``` - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. -- `pMode` is the proportional mode parameter with options for `PE` proportional on error (default), `PM` proportional on measurement and `PEM` which is 0.5 `PE` + 0.5 `PM`. -- `dMode` is the derivative mode parameter with options for `DE` derivative on error (default), `DM` derivative on measurement (default). -- `awMode` is the integral anti-windup parameter with an option for `CONDITION` which is based on PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. The`CLAMP` option (default), clamps the summation of the pmTerm and iTerm. The `OFF` option turns off all anti-windup. -- `Action` is the controller action parameter which has `DIRECT` (default) and `REVERSE` options. These options set how the controller responds to a change in input. `DIRECT` action is used if the input moves in the same direction as the controller output (i.e. heating process). `REVERSE` action is used if the input moves in the opposite direction as the controller output (i.e. cooling process). +- `pMode` is the proportional mode parameter with options for `pOnError` proportional on error (default), `pOnMeas` proportional on measurement and `pOnErrorMeas` which is 0.5 `pOnError` + 0.5 `pOnMeas`. +- `dMode` is the derivative mode parameter with options for `dOnError` derivative on error (default), `dOnMeas` derivative on measurement (default). +- `awMode` is the integral anti-windup parameter with an option for `iAwCondition` which is based on PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. The`iAwClamp` option (default), clamps the summation of the pmTerm and iTerm. The `OFF` option turns off all anti-windup. +- `Action` is the controller action parameter which has `direct` (default) and `reverse` options. These options set how the controller responds to a change in input. `direct` action is used if the input moves in the same direction as the controller output (i.e. heating process). `reverse` action is used if the input moves in the opposite direction as the controller output (i.e. cooling process). ```c++ -QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, - Action action); +QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, + float Kp, float Ki, float Kd, Action action) ``` This allows you to use Proportional on Error without explicitly saying so. @@ -58,8 +58,8 @@ This function contains the PID algorithm and it should be called once every loop #### SetTunings ```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::PE, dMode dMode = dMode::DM, - awMode awMode = awMode::CONDITION); +void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::pOnError, + dMode dMode = dMode::dOnMeas, iAwMode iAwMode = iAwMode::iAwCondition) ``` This function allows the controller's dynamic performance to be adjusted. It's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. The parameters are as described in the constructor. @@ -68,7 +68,7 @@ This function allows the controller's dynamic performance to be adjusted. It's c void QuickPID::SetTunings(float Kp, float Ki, float Kd); ``` -Set Tunings using the last remembered `pMode`, `dMode` and `awMode` settings. See example [PID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino) +Set Tunings using the last remembered `pMode`, `dMode` and `iAwMode` settings. See example [PID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino) #### SetSampleTime @@ -92,9 +92,9 @@ The PID controller is designed to vary its output within a given range. By defa void QuickPID::SetMode(Control Mode); ``` -Allows the controller Mode to be set to `MANUAL` (0) or `AUTOMATIC` (1) or `TIMER` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. +Allows the controller Mode to be set to `manual` (0) or `automatic` (1) or `timer` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. -`TIMER` mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See examples: +`timer` mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See examples: - [PID_AVR_Basic_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino) - [PID_AVR_Basic_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino) @@ -113,7 +113,7 @@ Does all the things that need to happen to ensure a bump-less transfer from manu void QuickPID::SetControllerDirection(Action Action); ``` -The PID will either be connected to a `DIRECT` acting process (+Output leads to +Input) or a `REVERSE` acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. +The PID will either be connected to a `DIRECT` acting process (+Output leads to +Input) or a `reverse` acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. #### PID Query Functions @@ -124,11 +124,11 @@ The PID will either be connected to a `DIRECT` acting process (+Output leads to float GetPterm(); // proportional component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output - uint8_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) - uint8_t GetDirection(); // DIRECT (0), REVERSE (1) - uint8_t GetPmode(); // PE (0), PM (1), PEM (2) - uint8_t GetDmode(); // DE (0), DM (1) - uint8_t GetAwMode(); // CONDITION (0), CLAMP (1), OFF (2) + uint8_t GetMode(); // manual (0), automatic (1) or timer (2) + uint8_t GetDirection(); // direct (0), reverse (1) + uint8_t GetPmode(); // pOnError (0), pOnMeas (1), pOnErrorMeas (2) + uint8_t GetDmode(); // dOnError (0), dOnMeas (1) + uint8_t GetAwMode(); // iAwCondition (0, iAwClamp (1), iAwOff (2) ``` These functions query the internal state of the PID. diff --git a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino index 71ebfdc..e83407b 100644 --- a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino +++ b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino @@ -16,7 +16,7 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::direct); void setup() { Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval @@ -27,7 +27,7 @@ void setup() { Setpoint = 100; //turn the PID on - myPID.SetMode(myPID.Control::AUTOMATIC); + myPID.SetMode(myPID.Control::automatic); } void loop() { diff --git a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino index 3954177..7458d8d 100644 --- a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino +++ b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino @@ -18,7 +18,7 @@ float Setpoint, Input, Output; float Kp = 2, Ki = 5, Kd = 1; Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::direct); void setup() { timer1.start(); @@ -28,7 +28,7 @@ void setup() { Setpoint = 100; //turn the PID on - myPID.SetMode(myPID.Control::AUTOMATIC); + myPID.SetMode(myPID.Control::automatic); } void loop() { diff --git a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino index f6b5045..3c96319 100644 --- a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino +++ b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino @@ -22,7 +22,7 @@ float aggKp = 4, aggKi = 0.2, aggKd = 1; float consKp = 1, consKi = 0.05, consKd = 0.25; //Specify the links and initial tuning parameters -QuickPID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, myPID.Action::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, myPID.Action::direct); void setup() { @@ -31,7 +31,7 @@ void setup() Setpoint = 100; //turn the PID on - myPID.SetMode(myPID.Control::AUTOMATIC); + myPID.SetMode(myPID.Control::automatic); } void loop() diff --git a/examples/PID_Basic/PID_Basic.ino b/examples/PID_Basic/PID_Basic.ino index 0290987..e616b3b 100644 --- a/examples/PID_Basic/PID_Basic.ino +++ b/examples/PID_Basic/PID_Basic.ino @@ -14,7 +14,7 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::direct); void setup() { @@ -23,7 +23,7 @@ void setup() Setpoint = 100; //turn the PID on - myPID.SetMode(myPID.Control::AUTOMATIC); + myPID.SetMode(myPID.Control::automatic); } void loop() diff --git a/examples/PID_Controller_Options/PID_Controller_Options.ino b/examples/PID_Controller_Options/PID_Controller_Options.ino index df91f37..d6e2ceb 100644 --- a/examples/PID_Controller_Options/PID_Controller_Options.ino +++ b/examples/PID_Controller_Options/PID_Controller_Options.ino @@ -1,11 +1,11 @@ /************************************************************************************** For testing and checking parameter options. From QuickPID.h: - enum class Mode : uint8_t {MANUAL, AUTOMATIC, TIMER}; // controller modes - enum class Action : uint8_t {DIRECT, REVERSE}; // controller actions - enum class pMode : uint8_t {PE, PM, PEM}; // proportional modes - enum class dMode : uint8_t {DE, DM}; // derivative modes - enum class awMode : uint8_t {CONDITION, CLAMP, OFF}; // integral anti-windup modes + enum class Mode : uint8_t {manual, automatic, timer}; // controller modes + enum class Action : uint8_t {direct, reverse}; // controller actions + enum class pMode : uint8_t {pOnError, pOnMeas, pOnErrorMeas}; // proportional modes + enum class dMode : uint8_t {dOnError, dOnMeas}; // derivative modes + enum class iAwMode : uint8_t {iAwCondition, iAwClamp, iAwOff}; // integral anti-windup modes float GetKp(); // proportional gain float GetKi(); // integral gain @@ -13,11 +13,11 @@ float GetPterm(); // proportional component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output - uint8_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) - uint8_t GetDirection(); // DIRECT (0), REVERSE (1) - uint8_t GetPmode(); // PE (0), PM (1), PEM (2) - uint8_t GetDmode(); // DE (0), DM (1) - uint8_t GetAwMode(); // CONDITION (0), CLAMP (1), OFF (2) + uint8_t GetMode(); // manual (0), automatic (1) or TIMER (2) + uint8_t GetDirection(); // direct (0), reverse (1) + uint8_t GetPmode(); // pOnError (0), pOnMeas (1), pOnErrorMeas (2) + uint8_t GetDmode(); // dOnError (0), dOnMeas (1) + uint8_t GetAwMode(); // iAwCondition (0), iAwClamp (1), iAwOff (2) Documentation (GitHub): https://github.com/Dlloydev/QuickPID **************************************************************************************/ @@ -27,14 +27,13 @@ const byte inputPin = 0; const byte outputPin = 3; - //Define variables we'll be connecting to float Setpoint = 0, Input = 0, Output = 0, Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.pMode::PE, myPID.dMode::DM, myPID.awMode::CONDITION, myPID.Action::DIRECT); -/* PE DE CONDITION DIRECT - PM DM CLAMP REVERSE - PEM OFF +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.pMode::pOnError, myPID.dMode::dOnMeas, myPID.iAwMode::iAwCondition, myPID.Action::direct); +/* pOnError dOnError iAwCondition direct + pOnMeas dOnMeas iAwClamp reverse + pOnErrorMeas iAwOff */ void setup() @@ -45,11 +44,9 @@ void setup() myPID.SetOutputLimits(0, 255); myPID.SetSampleTimeUs(100000); myPID.SetTunings(Kp, Ki, Kd); - myPID.SetMode(myPID.Control::AUTOMATIC); - + myPID.SetMode(myPID.Control::automatic); Input = analogRead(inputPin); myPID.Compute(); - analogWrite(outputPin, Output); Serial.println(); diff --git a/examples/PID_RelayOutput/PID_RelayOutput.ino b/examples/PID_RelayOutput/PID_RelayOutput.ino index 696d007..3b400b0 100644 --- a/examples/PID_RelayOutput/PID_RelayOutput.ino +++ b/examples/PID_RelayOutput/PID_RelayOutput.ino @@ -26,7 +26,7 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::Action::DIRECT); +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::direct); unsigned int WindowSize = 5000; unsigned int minWindow = 500; @@ -44,7 +44,7 @@ void setup() myPID.SetOutputLimits(0, WindowSize); //turn the PID on - myPID.SetMode(myPID.Control::AUTOMATIC); + myPID.SetMode(myPID.Control::automatic); } void loop() diff --git a/keywords.txt b/keywords.txt index e9a367e..3c37c50 100644 --- a/keywords.txt +++ b/keywords.txt @@ -38,16 +38,16 @@ analogWriteResolution KEYWORD2 # Constants (LITERAL1) ########################################## -AUTOMATIC LITERAL1 -MANUAL LITERAL1 -TIMER LITERAL1 -DIRECT LITERAL1 -REV LITERAL1 -PE LITERAL1 -PM LITERAL1 -PEM LITERAL1 -CONDITION LITERAL1 -CLAMP LITERAL1 -OFF LITERAL1 -DE LITERAL1 -DM LITERAL1 +automatic LITERAL1 +manual LITERAL1 +timer LITERAL1 +direct LITERAL1 +reverse LITERAL1 +pOnError LITERAL1 +pOnMeas LITERAL1 +pOnErrorMeas LITERAL1 +iAwCondition LITERAL1 +iAwClamp LITERAL1 +iAwOff LITERAL1 +dOnError LITERAL1 +dOnMeas LITERAL1 diff --git a/library.json b/library.json index ebb5815..165e8ab 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.0.1", + "version": "3.0.2", "description": "A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~3.0.1" + "QuickPID": "~3.0.2" }, "frameworks": "arduino", "platforms": "*" diff --git a/library.properties b/library.properties index 31e7a08..6fc96d0 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.0.1 +version=3.0.2 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 8ff1750..c80b890 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.0.1 + QuickPID Library for Arduino - Version 3.0.2 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ @@ -17,30 +17,29 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, pMode pMode = pMode::PE, dMode dMode = dMode::DM, - awMode awMode = awMode::CONDITION, Action action = Action::DIRECT) { - + float Kp, float Ki, float Kd, pMode pMode = pMode::pOnError, dMode dMode = dMode::dOnMeas, + iAwMode iAwMode = iAwMode::iAwCondition, Action action = Action::direct) { myOutput = Output; myInput = Input; mySetpoint = Setpoint; - mode = Control::MANUAL; + mode = Control::manual; QuickPID::SetOutputLimits(0, 255); // same default as Arduino PWM limit sampleTimeUs = 100000; // 0.1 sec default QuickPID::SetControllerDirection(action); - QuickPID::SetTunings(Kp, Ki, Kd, pMode, dMode, awMode); + QuickPID::SetTunings(Kp, Ki, Kd, pMode, dMode, iAwMode); lastTime = micros() - sampleTimeUs; } /* Constructor ********************************************************************* - To allow using Proportional on Error without explicitly saying so. + To allow using proportional on error without explicitly saying so. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, Action action) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pmode = pMode::PE, dmode = dMode::DM, - awmode = awMode::CONDITION, action = Action::DIRECT) { + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pmode = pMode::pOnError, dmode = dMode::dOnMeas, + iawmode = iAwMode::iAwCondition, action = Action::direct) { } /* Compute() *********************************************************************** @@ -49,34 +48,34 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, when the output is computed, false when nothing has been done. **********************************************************************************/ bool QuickPID::Compute() { - if (mode == Control::MANUAL) return false; + if (mode == Control::manual) return false; uint32_t now = micros(); uint32_t timeChange = (now - lastTime); - if (mode == Control::TIMER || timeChange >= sampleTimeUs) { + if (mode == Control::timer || timeChange >= sampleTimeUs) { float input = *myInput; float dInput = input - lastInput; - if (action == Action::REVERSE) dInput = -dInput; + if (action == Action::reverse) dInput = -dInput; error = *mySetpoint - input; - if (action == Action::REVERSE) error = -error; + if (action == Action::reverse) error = -error; float dError = error - lastError; float peTerm = kp * error; float pmTerm = kp * dInput; - if (pmode == pMode::PE) pmTerm = 0; - else if (pmode == pMode::PM) peTerm = 0; - else { //PEM + if (pmode == pMode::pOnError) pmTerm = 0; + else if (pmode == pMode::pOnMeas) peTerm = 0; + else { //pOnErrorMeas peTerm *= 0.5; pmTerm *= 0.5; } pTerm = peTerm - pmTerm; // used by GetDterm() iTerm = ki * error; - if (dmode == dMode::DE) dTerm = kd * dError; - else dTerm = -kd * dInput; // DM + if (dmode == dMode::dOnError) dTerm = kd * dError; + else dTerm = -kd * dInput; // dOnMeas //condition anti-windup (default) - if (awmode == awMode::CONDITION) { + if (iawmode == iAwMode::iAwCondition) { bool aw = false; float iTermOut = (peTerm - pmTerm) + ki * (iTerm + error); if (iTermOut > outMax && dError > 0) aw = true; @@ -86,7 +85,7 @@ bool QuickPID::Compute() { // by default, compute output as per PID_v1 outputSum += iTerm; // include integral amount - if (awmode == awMode::OFF) outputSum -= pmTerm; // include pmTerm (no anti-windup) + if (iawmode == iAwMode::iAwOff) outputSum -= pmTerm; // include pmTerm (no anti-windup) else outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp *myOutput = constrain(outputSum + peTerm + dTerm, outMin, outMax); // include dTerm, clamp and drive output @@ -103,9 +102,9 @@ bool QuickPID::Compute() { it's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. ******************************************************************************/ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::PE, dMode dMode = dMode::DM, awMode awMode = awMode::CONDITION) { +void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::pOnError, dMode dMode = dMode::dOnMeas, iAwMode iAwMode = iAwMode::iAwCondition) { if (Kp < 0 || Ki < 0 || Kd < 0) return; - pmode = pMode; dmode = dMode; awmode = awMode; + pmode = pMode; dmode = dMode; iawmode = iAwMode; dispKp = Kp; dispKi = Ki; dispKd = Kd; float SampleTimeSec = (float)sampleTimeUs / 1000000; kp = Kp; @@ -117,7 +116,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::PE, Set Tunings using the last remembered pMode and dMode setting. ******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { - SetTunings(Kp, Ki, Kd, pmode, dmode, awmode); + SetTunings(Kp, Ki, Kd, pmode, dmode, iawmode); } /* SetSampleTime(.)*********************************************************** @@ -141,22 +140,19 @@ void QuickPID::SetOutputLimits(float Min, float Max) { outMin = Min; outMax = Max; - if (mode != Control::MANUAL) { + if (mode != Control::manual) { *myOutput = constrain(*myOutput, outMin, outMax); outputSum = constrain(outputSum, outMin, outMax); } } /* SetMode(.)***************************************************************** - Sets the controller mode to MANUAL (0), AUTOMATIC (1) or TIMER (2) - when the transition from MANUAL to AUTOMATIC or TIMER occurs, the + Sets the controller mode to manual (0), automatic (1) or timer (2) + when the transition from manual to automatic or timer occurs, the controller is automatically initialized. ******************************************************************************/ - - - void QuickPID::SetMode(Control Mode) { - if (mode == Control::MANUAL && Mode != Control::MANUAL) { // just went from MANUAL to AUTOMATIC or TIMER + if (mode == Control::manual && Mode != Control::manual) { // just went from manual to automatic or timer QuickPID::Initialize(); } mode = Mode; @@ -173,8 +169,8 @@ void QuickPID::Initialize() { } /* SetControllerDirection(.)************************************************** - The PID will either be connected to a DIRECT acting process (+Output leads - to +Input) or a REVERSE acting process(+Output leads to -Input). + The PID will either be connected to a direct acting process (+Output leads + to +Input) or a reverse acting process(+Output leads to -Input). ******************************************************************************/ void QuickPID::SetControllerDirection(Action Action) { action = Action; @@ -214,5 +210,5 @@ uint8_t QuickPID::GetDmode() { return static_cast(dmode); } uint8_t QuickPID::GetAwMode() { - return static_cast(awmode); + return static_cast(iawmode); } diff --git a/src/QuickPID.h b/src/QuickPID.h index 5765bb9..cf4a879 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -6,21 +6,21 @@ class QuickPID { public: - enum class Control : uint8_t {MANUAL, AUTOMATIC, TIMER}; // controller mode - enum class Action : uint8_t {DIRECT, REVERSE}; // controller action - enum class pMode : uint8_t {PE, PM, PEM}; // proportional mode - enum class dMode : uint8_t {DE, DM}; // derivative mode - enum class awMode : uint8_t {CONDITION, CLAMP, OFF}; // integral anti-windup mode + enum class Control : uint8_t {manual, automatic, timer}; // controller mode + enum class Action : uint8_t {direct, reverse}; // controller action + enum class pMode : uint8_t {pOnError, pOnMeas, pOnErrorMeas}; // proportional mode + enum class dMode : uint8_t {dOnError, dOnMeas}; // derivative mode + enum class iAwMode : uint8_t {iAwCondition, iAwClamp, iAwOff}; // integral anti-windup mode // commonly used functions ************************************************************************************ // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, pMode pMode, dMode dMode, awMode awMode, Action Action); + QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, pMode pMode, dMode dMode, iAwMode iAwMode, Action Action); // Overload constructor with proportional ratio. Links the PID to Input, Output, Setpoint and Tuning Parameters. QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, Action Action); - // Sets PID mode to MANUAL (0), AUTOMATIC (1) or TIMER (2). + // Sets PID mode to manual (0), automatic (1) or timer (2). void SetMode(Control mode); // Performs the PID calculation. It should be called every time loop() cycles ON/OFF and calculation frequency @@ -37,10 +37,10 @@ class QuickPID { void SetTunings(float Kp, float Ki, float Kd); // Overload for specifying proportional ratio. - void SetTunings(float Kp, float Ki, float Kd, pMode pMode, dMode dMode, awMode awMode); + void SetTunings(float Kp, float Ki, float Kd, pMode pMode, dMode dMode, iAwMode iAwMode); - // Sets the controller Direction or Action. DIRECT means the output will increase when the error is positive. - // REVERSE means the output will decrease when the error is positive. + // Sets the controller direction or action. Direct means the output will increase when the error is positive. + // Reverse means the output will decrease when the error is positive. void SetControllerDirection(Action Action); // Sets the sample time in microseconds with which each PID calculation is performed. Default is 100000 µs. @@ -53,11 +53,11 @@ class QuickPID { float GetPterm(); // proportional component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output - uint8_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) - uint8_t GetDirection(); // DIRECT (0), REVERSE (1) - uint8_t GetPmode(); // PE (0), PM (1), PEM (2) - uint8_t GetDmode(); // DE (0), DM (1) - uint8_t GetAwMode(); // CONDITION (0, CLAMP (1), OFF (2) + uint8_t GetMode(); // manual (0), automatic (1) or timer (2) + uint8_t GetDirection(); // direct (0), reverse (1) + uint8_t GetPmode(); // pOnError (0), pOnMeas (1), pOnErrorMeas (2) + uint8_t GetDmode(); // dOnError (0), dOnMeas (1) + uint8_t GetAwMode(); // iAwCondition (0, iAwClamp (1), iAwOff (2) private: @@ -78,11 +78,11 @@ class QuickPID { float *myOutput; // hard link between the variables and the PID, freeing the user from having float *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. - Control mode = Control::MANUAL; - Action action = Action::DIRECT; - pMode pmode = pMode::PE; - dMode dmode = dMode::DM; - awMode awmode = awMode::CONDITION; + Control mode = Control::manual; + Action action = Action::direct; + pMode pmode = pMode::pOnError; + dMode dmode = dMode::dOnMeas; + iAwMode iawmode = iAwMode::iAwCondition; uint32_t sampleTimeUs, lastTime; float outputSum, outMin, outMax, error, lastError, lastInput; From 03b51fd8c357b100a14bde2065ae58a5a4e881ab Mon Sep 17 00:00:00 2001 From: Guilherme Gonzaga Date: Thu, 16 Dec 2021 10:54:36 -0400 Subject: [PATCH 073/101] Add simplified constructor and new control functions Resolves issue #38 - Add SetProportionalMode, SetDerivativeMode and SetAntiWindupMode - Add class constants defKp, defKi, defKd for maintainability - Updated documentation - Documentation fix: dMode in constructor listed two default values --- README.md | 43 ++++++++++++++++++++++++++++++++++++------- src/QuickPID.cpp | 36 ++++++++++++++++++++++++++++++++++++ src/QuickPID.h | 21 +++++++++++++++++++++ 3 files changed, 93 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index d2f7b4f..47b4053 100644 --- a/README.md +++ b/README.md @@ -9,11 +9,11 @@ Development began with a fork of the Arduino PID Library. Modifications and new #### New feature Summary - [x] `timer` mode for calling PID compute by an external timer function or ISR -- [x] `analogWrite()` support for ESP32 and ESP32-S2 +- [x] `analogWrite()` support for ESP32 and ESP32-S2 - [x] Proportional on error `pOnError`, measurement `pOnMeas` or both `pOnErrorMeas` options - [x] Derivative on error `dOnError` and measurement `dOnMeas` options - [x] New PID Query Functions `GetPterm`, `GetIterm`, `GetDterm`, `GetPmode`, `GetDmode` and `GetAwMode` -- [x] New integral anti-windup options `iAwCondition`, `iAwClamp` and `iAwOff` +- [x] New integral anti-windup options `iAwCondition`, `iAwClamp` and `iAwOff` - [x] New `reverse` mode only changes sign of `error` and `dInput` - [x] Uses `float` instead of `double` @@ -35,9 +35,9 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. -- `pMode` is the proportional mode parameter with options for `pOnError` proportional on error (default), `pOnMeas` proportional on measurement and `pOnErrorMeas` which is 0.5 `pOnError` + 0.5 `pOnMeas`. -- `dMode` is the derivative mode parameter with options for `dOnError` derivative on error (default), `dOnMeas` derivative on measurement (default). -- `awMode` is the integral anti-windup parameter with an option for `iAwCondition` which is based on PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. The`iAwClamp` option (default), clamps the summation of the pmTerm and iTerm. The `OFF` option turns off all anti-windup. +- `pMode` is the proportional mode parameter with options for `pOnError` proportional on error (default), `pOnMeas` proportional on measurement and `pOnErrorMeas` which is 0.5 `pOnError` + 0.5 `pOnMeas`. +- `dMode` is the derivative mode parameter with options for `dOnError` derivative on error, `dOnMeas` derivative on measurement (default). +- `awMode` is the integral anti-windup parameter with an option for `iAwCondition` which is based on PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. The`iAwClamp` option (default), clamps the summation of the pmTerm and iTerm. The `iAwOff` option turns off all anti-windup. - `Action` is the controller action parameter which has `direct` (default) and `reverse` options. These options set how the controller responds to a change in input. `direct` action is used if the input moves in the same direction as the controller output (i.e. heating process). `reverse` action is used if the input moves in the opposite direction as the controller output (i.e. cooling process). ```c++ @@ -47,6 +47,12 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, This allows you to use Proportional on Error without explicitly saying so. +```c++ +QuickPID::QuickPID(float *Input, float *Output, float *Setpoint) +``` + +This simplified version allows you to define the remaining parameters later via specific setter functions. By default, Kp, Ki, and Kd will be initialized to zero, and should be later set via `SetTunings` to relevant values. + #### Compute ```c++ @@ -92,7 +98,7 @@ The PID controller is designed to vary its output within a given range. By defa void QuickPID::SetMode(Control Mode); ``` -Allows the controller Mode to be set to `manual` (0) or `automatic` (1) or `timer` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. +Allows the controller Mode to be set to `manual` (0) or `automatic` (1) or `timer` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. `timer` mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See examples: @@ -115,6 +121,30 @@ void QuickPID::SetControllerDirection(Action Action); The PID will either be connected to a `DIRECT` acting process (+Output leads to +Input) or a `reverse` acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. +#### SetProportionalMode + +```c++ +void QuickPID::SetProportionalMode(pMode pMode); +``` + +Sets the computation method for the proportional term, to compute based either on error (default), on measurement, or the average of both. + +#### SetDerivativeMode + +```c++ +void QuickPID::SetDerivativeMode(dMode dMode); +``` + +Sets the computation method for the derivative term, to compute based either on error (default), or on measurement. + +#### SetAntiWindupMode + +```c++ +void QuickPID::SetAntiWindupMode(iAwMode iAwMode); +``` + +Sets the integral anti-windup mode to one of iAwClamp, which clamps the output after adding integral and proportional (on measurement) terms, or iAwCondition, which provides some integral correction, prevents deep saturation and reduces overshoot. Option iAwOff disables anti-windup altogether. + #### PID Query Functions ```c++ @@ -154,4 +184,3 @@ If you're using QuickPID with an ESP32 and need analogWrite compatibility, there - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary ------ - diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index c80b890..79ce870 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -42,6 +42,15 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, iawmode = iAwMode::iAwCondition, action = Action::direct) { } +/* Constructor ********************************************************************* + Simplified constructor which uses defaults for remaining parameters. + **********************************************************************************/ +QuickPID::QuickPID(float* Input, float* Output, float* Setpoint) + : QuickPID::QuickPID(Input, Output, Setpoint, defKp, defKi, defKd, + pmode = pMode::pOnError, dmode = dMode::dOnMeas, + iawmode = iAwMode::iAwCondition, action = Action::direct) { +} + /* Compute() *********************************************************************** This function should be called every time "void loop()" executes. The function will decide whether a new PID Output needs to be computed. Returns true @@ -176,6 +185,33 @@ void QuickPID::SetControllerDirection(Action Action) { action = Action; } +/* SetProportionalMode(.)***************************************************** + Sets the computation method for the proportional term, to compute based + either on error (default), on measurement, or the average of both. +******************************************************************************/ +void QuickPID::SetProportionalMode(pMode pMode) { + pmode = pMode; +} + +/* SetDerivativeMode(.)******************************************************* + Sets the computation method for the derivative term, to compute based + either on error (default), or on measurement. +******************************************************************************/ +void QuickPID::SetDerivativeMode(dMode dMode) { + dmode = dMode; +} + +/* SetAntiWindupMode(.)******************************************************* + Sets the integral anti-windup mode to one of iAwClamp, which clamps + the output after adding integral and proportional (on measurement) terms, + or iAwCondition, which provides some integral correction, prevents + deep saturation and reduces overshoot. + Option iAwOff disables anti-windup altogether. +******************************************************************************/ +void QuickPID::SetAntiWindupMode(iAwMode iAwMode) { + iawmode = iAwMode; +} + /* Status Functions************************************************************ These functions query the internal state of the PID. ******************************************************************************/ diff --git a/src/QuickPID.h b/src/QuickPID.h index cf4a879..f3f41ec 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -20,6 +20,9 @@ class QuickPID { // Overload constructor with proportional ratio. Links the PID to Input, Output, Setpoint and Tuning Parameters. QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, Action Action); + // Simplified constructor which uses defaults for remaining parameters. + QuickPID(float *Input, float *Output, float *Setpoint); + // Sets PID mode to manual (0), automatic (1) or timer (2). void SetMode(Control mode); @@ -46,6 +49,20 @@ class QuickPID { // Sets the sample time in microseconds with which each PID calculation is performed. Default is 100000 µs. void SetSampleTimeUs(uint32_t NewSampleTimeUs); + // Sets the computation method for the proportional term, to compute based either on error (default), + // on measurement, or the average of both. + void SetProportionalMode(pMode pMode); + + // Sets the computation method for the derivative term, to compute based either on error (default), + // or measurement. + void SetDerivativeMode(dMode dMode); + + // Sets the integral anti-windup mode to one of iAwClamp, which clamps the output after + // adding integral and proportional (on measurement) terms, or iAwCondition, which + // provides some integral correction, prevents deep saturation and reduces overshoot. + // Option iAwOff disables anti-windup altogether. + void SetAntiWindupMode(iAwMode iAwMode); + // PID Query functions **************************************************************************************** float GetKp(); // proportional gain float GetKi(); // integral gain @@ -63,6 +80,10 @@ class QuickPID { void Initialize(); + static constexpr float defKp = 0; // default controller gains + static constexpr float defKi = 0; + static constexpr float defKd = 0; + float dispKp; // tuning parameters for display purposes. float dispKi; float dispKd; From 8f3568bfdb84b521aa499926bc261208c32c2033 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 17 Dec 2021 01:29:28 -0500 Subject: [PATCH 074/101] Cosmetic and Readme update --- README.md | 13 +++---- .../PID_Controller_Options.ino | 2 +- keywords.txt | 3 ++ library.json | 4 +-- library.properties | 2 +- src/QuickPID.cpp | 34 +++++++++++++------ src/QuickPID.h | 13 +++---- 7 files changed, 44 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index 47b4053..cc62361 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,7 @@ Development began with a fork of the Arduino PID Library. Modifications and new #### New feature Summary +- [x] New functions added: `SetProportionalMode`, `SetDerivativeMode` and `SetAntiWindupMode` - [x] `timer` mode for calling PID compute by an external timer function or ISR - [x] `analogWrite()` support for ESP32 and ESP32-S2 - [x] Proportional on error `pOnError`, measurement `pOnMeas` or both `pOnErrorMeas` options @@ -119,7 +120,7 @@ Does all the things that need to happen to ensure a bump-less transfer from manu void QuickPID::SetControllerDirection(Action Action); ``` -The PID will either be connected to a `DIRECT` acting process (+Output leads to +Input) or a `reverse` acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. +The PID will either be connected to a `direct` acting process (+Output leads to +Input) or a `reverse` acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. #### SetProportionalMode @@ -135,7 +136,7 @@ Sets the computation method for the proportional term, to compute based either o void QuickPID::SetDerivativeMode(dMode dMode); ``` -Sets the computation method for the derivative term, to compute based either on error (default), or on measurement. +Sets the computation method for the derivative term, to compute based either on error or on measurement (default). #### SetAntiWindupMode @@ -143,7 +144,7 @@ Sets the computation method for the derivative term, to compute based either on void QuickPID::SetAntiWindupMode(iAwMode iAwMode); ``` -Sets the integral anti-windup mode to one of iAwClamp, which clamps the output after adding integral and proportional (on measurement) terms, or iAwCondition, which provides some integral correction, prevents deep saturation and reduces overshoot. Option iAwOff disables anti-windup altogether. +Sets the integral anti-windup mode to one of iAwClamp, which clamps the output after adding integral and proportional (on measurement) terms, or iAwCondition (default), which provides some integral correction, prevents deep saturation and reduces overshoot. Option iAwOff disables anti-windup altogether. #### PID Query Functions @@ -163,9 +164,9 @@ Sets the integral anti-windup mode to one of iAwClamp, which clamps the output a These functions query the internal state of the PID. -#### [AnalogWrite (PWM and DAC) for ESP32](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) +#### AnalogWrite (PWM and DAC) for ESP32 -If you're using QuickPID with an ESP32 and need analogWrite compatibility, there's no need to install a library as this feature is already included. +If you're using QuickPID with an ESP32 and need analogWrite compatibility, there's no need to install a library as this feature is already included. AnalogWrite [documentation](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) ### Original README (Arduino PID) @@ -178,7 +179,7 @@ If you're using QuickPID with an ESP32 and need analogWrite compatibility, there *************************************************************** ``` - - For an ultra-detailed explanation of the original code, please visit: + - For a detailed explanation of the original code, please visit: http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary diff --git a/examples/PID_Controller_Options/PID_Controller_Options.ino b/examples/PID_Controller_Options/PID_Controller_Options.ino index d6e2ceb..e68db4d 100644 --- a/examples/PID_Controller_Options/PID_Controller_Options.ino +++ b/examples/PID_Controller_Options/PID_Controller_Options.ino @@ -13,7 +13,7 @@ float GetPterm(); // proportional component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output - uint8_t GetMode(); // manual (0), automatic (1) or TIMER (2) + uint8_t GetMode(); // manual (0), automatic (1) or timer (2) uint8_t GetDirection(); // direct (0), reverse (1) uint8_t GetPmode(); // pOnError (0), pOnMeas (1), pOnErrorMeas (2) uint8_t GetDmode(); // dOnError (0), dOnMeas (1) diff --git a/keywords.txt b/keywords.txt index 3c37c50..01d619f 100644 --- a/keywords.txt +++ b/keywords.txt @@ -19,6 +19,9 @@ SetOutputLimits KEYWORD2 SetTunings KEYWORD2 SetControllerDirection KEYWORD2 SetSampleTimeUs KEYWORD2 +SetProportionalMode KEYWORD2 +SetDerivativeMode KEYWORD2 +SetAntiWindupMode KEYWORD2 GetKp KEYWORD2 GetKi KEYWORD2 GetKd KEYWORD2 diff --git a/library.json b/library.json index 165e8ab..ddab6e0 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.0.2", + "version": "3.0.3", "description": "A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~3.0.2" + "QuickPID": "~3.0.3" }, "frameworks": "arduino", "platforms": "*" diff --git a/library.properties b/library.properties index 6fc96d0..08d6de7 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.0.2 +version=3.0.3 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 79ce870..8f7000b 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.0.2 + QuickPID Library for Arduino - Version 3.0.3 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ @@ -17,8 +17,11 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, pMode pMode = pMode::pOnError, dMode dMode = dMode::dOnMeas, - iAwMode iAwMode = iAwMode::iAwCondition, Action action = Action::direct) { + float Kp, float Ki, float Kd, + pMode pMode = pMode::pOnError, + dMode dMode = dMode::dOnMeas, + iAwMode iAwMode = iAwMode::iAwCondition, + Action action = Action::direct) { myOutput = Output; myInput = Input; @@ -38,8 +41,11 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, Action action) - : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pmode = pMode::pOnError, dmode = dMode::dOnMeas, - iawmode = iAwMode::iAwCondition, action = Action::direct) { + : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, + pmode = pMode::pOnError, + dmode = dMode::dOnMeas, + iawmode = iAwMode::iAwCondition, + action = Action::direct) { } /* Constructor ********************************************************************* @@ -47,8 +53,10 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint) : QuickPID::QuickPID(Input, Output, Setpoint, defKp, defKi, defKd, - pmode = pMode::pOnError, dmode = dMode::dOnMeas, - iawmode = iAwMode::iAwCondition, action = Action::direct) { + pmode = pMode::pOnError, + dmode = dMode::dOnMeas, + iawmode = iAwMode::iAwCondition, + action = Action::direct) { } /* Compute() *********************************************************************** @@ -111,7 +119,11 @@ bool QuickPID::Compute() { it's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. ******************************************************************************/ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::pOnError, dMode dMode = dMode::dOnMeas, iAwMode iAwMode = iAwMode::iAwCondition) { +void QuickPID::SetTunings(float Kp, float Ki, float Kd, + pMode pMode = pMode::pOnError, + dMode dMode = dMode::dOnMeas, + iAwMode iAwMode = iAwMode::iAwCondition) { + if (Kp < 0 || Ki < 0 || Kd < 0) return; pmode = pMode; dmode = dMode; iawmode = iAwMode; dispKp = Kp; dispKi = Ki; dispKd = Kd; @@ -122,7 +134,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::pOn } /* SetTunings(...)************************************************************ - Set Tunings using the last remembered pMode and dMode setting. + Set Tunings using the last remembered pMode, dMode and iAwMode settings. ******************************************************************************/ void QuickPID::SetTunings(float Kp, float Ki, float Kd) { SetTunings(Kp, Ki, Kd, pmode, dmode, iawmode); @@ -195,7 +207,7 @@ void QuickPID::SetProportionalMode(pMode pMode) { /* SetDerivativeMode(.)******************************************************* Sets the computation method for the derivative term, to compute based - either on error (default), or on measurement. + either on error or on measurement (default). ******************************************************************************/ void QuickPID::SetDerivativeMode(dMode dMode) { dmode = dMode; @@ -204,7 +216,7 @@ void QuickPID::SetDerivativeMode(dMode dMode) { /* SetAntiWindupMode(.)******************************************************* Sets the integral anti-windup mode to one of iAwClamp, which clamps the output after adding integral and proportional (on measurement) terms, - or iAwCondition, which provides some integral correction, prevents + or iAwCondition (default), which provides some integral correction, prevents deep saturation and reduces overshoot. Option iAwOff disables anti-windup altogether. ******************************************************************************/ diff --git a/src/QuickPID.h b/src/QuickPID.h index f3f41ec..8f197ba 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -14,10 +14,12 @@ class QuickPID { // commonly used functions ************************************************************************************ - // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. - QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, pMode pMode, dMode dMode, iAwMode iAwMode, Action Action); + // Constructor. Links the PID to Input, Output, Setpoint, initial tuning parameters and control modes. + QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, + pMode pMode, dMode dMode, iAwMode iAwMode, Action Action); - // Overload constructor with proportional ratio. Links the PID to Input, Output, Setpoint and Tuning Parameters. + // Overload constructor links the PID to Input, Output, Setpoint, tuning parameters and control Action. + // Uses defaults for remaining parameters. QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, Action Action); // Simplified constructor which uses defaults for remaining parameters. @@ -53,12 +55,11 @@ class QuickPID { // on measurement, or the average of both. void SetProportionalMode(pMode pMode); - // Sets the computation method for the derivative term, to compute based either on error (default), - // or measurement. + // Sets the computation method for the derivative term, to compute based either on error or measurement (default). void SetDerivativeMode(dMode dMode); // Sets the integral anti-windup mode to one of iAwClamp, which clamps the output after - // adding integral and proportional (on measurement) terms, or iAwCondition, which + // adding integral and proportional (on measurement) terms, or iAwCondition (default), which // provides some integral correction, prevents deep saturation and reduces overshoot. // Option iAwOff disables anti-windup altogether. void SetAntiWindupMode(iAwMode iAwMode); From 9317b2d0ccb0abb0e2c4a7909355f107fe1239be Mon Sep 17 00:00:00 2001 From: Guilherme Gonzaga Date: Fri, 17 Dec 2021 17:10:33 -0400 Subject: [PATCH 075/101] Avoid double promotion in QuickPID::Compute() --- src/QuickPID.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 8f7000b..f418cbc 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -83,8 +83,8 @@ bool QuickPID::Compute() { if (pmode == pMode::pOnError) pmTerm = 0; else if (pmode == pMode::pOnMeas) peTerm = 0; else { //pOnErrorMeas - peTerm *= 0.5; - pmTerm *= 0.5; + peTerm *= 0.5f; + pmTerm *= 0.5f; } pTerm = peTerm - pmTerm; // used by GetDterm() iTerm = ki * error; From 1a4a84c890820e96e1e6577426cba37671e59138 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 19 Dec 2021 17:38:49 -0500 Subject: [PATCH 076/101] Update - Avoid double promotion in QuickPID::Compute(), pull request issue #40 - Update readme and examples --- README.md | 97 +++++++++++-------- .../PID_AVR_Basic_Interrupt_TIMER.ino | 11 ++- .../PID_AVR_Basic_Software_TIMER.ino | 5 +- .../PID_AdaptiveTunings.ino | 20 ++-- examples/PID_Basic/PID_Basic.ino | 15 ++- .../PID_Controller_Options.ino | 29 +----- examples/PID_RelayOutput/PID_RelayOutput.ino | 5 +- library.json | 4 +- library.properties | 2 +- src/QuickPID.cpp | 2 +- 10 files changed, 102 insertions(+), 88 deletions(-) diff --git a/README.md b/README.md index cc62361..b9cd74b 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) -QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library. One of the additional features includes integral anti-windup which can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include `timer`, which allows external timer or ISR timing control. +QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library except for using a more advanced anti-windup mode. Integral anti-windup can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include `timer`, which allows external timer or ISR timing control. ### Features @@ -38,7 +38,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. - `pMode` is the proportional mode parameter with options for `pOnError` proportional on error (default), `pOnMeas` proportional on measurement and `pOnErrorMeas` which is 0.5 `pOnError` + 0.5 `pOnMeas`. - `dMode` is the derivative mode parameter with options for `dOnError` derivative on error, `dOnMeas` derivative on measurement (default). -- `awMode` is the integral anti-windup parameter with an option for `iAwCondition` which is based on PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. The`iAwClamp` option (default), clamps the summation of the pmTerm and iTerm. The `iAwOff` option turns off all anti-windup. +- `awMode` is the integral anti-windup parameter with an option for `iAwCondition` (default) that is based on PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. The`iAwClamp` option clamps the summation of the pmTerm and iTerm. The `iAwOff` option turns off all anti-windup. - `Action` is the controller action parameter which has `direct` (default) and `reverse` options. These options set how the controller responds to a change in input. `direct` action is used if the input moves in the same direction as the controller output (i.e. heating process). `reverse` action is used if the input moves in the opposite direction as the controller output (i.e. cooling process). ```c++ @@ -52,7 +52,7 @@ This allows you to use Proportional on Error without explicitly saying so. QuickPID::QuickPID(float *Input, float *Output, float *Setpoint) ``` -This simplified version allows you to define the remaining parameters later via specific setter functions. By default, Kp, Ki, and Kd will be initialized to zero, and should be later set via `SetTunings` to relevant values. +This simplified version allows you to define the remaining parameters later via specific setter functions. By default, Kp, Ki, and Kd will be initialized to zero and should be later set via `SetTunings` to relevant values. #### Compute @@ -62,28 +62,57 @@ bool QuickPID::Compute(); This function contains the PID algorithm and it should be called once every loop(). Most of the time it will just return false without doing anything. However, at a frequency specified by `SetSampleTime` it will calculate a new Output and return true. -#### SetTunings +#### Initialize ```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::pOnError, - dMode dMode = dMode::dOnMeas, iAwMode iAwMode = iAwMode::iAwCondition) +void QuickPID::Initialize(); ``` -This function allows the controller's dynamic performance to be adjusted. It's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. The parameters are as described in the constructor. +Does all the things that need to happen to ensure a bump-less transfer from manual to automatic mode. + +#### PID Query Functions + +These functions query the internal state of the PID. ```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd); +float GetKp(); // proportional gain +float GetKi(); // integral gain +float GetKd(); // derivative gain +float GetPterm(); // proportional component of output +float GetIterm(); // integral component of output +float GetDterm(); // derivative component of output +uint8_t GetMode(); // manual (0), automatic (1) or timer (2) +uint8_t GetDirection(); // direct (0), reverse (1) +uint8_t GetPmode(); // pOnError (0), pOnMeas (1), pOnErrorMeas (2) +uint8_t GetDmode(); // dOnError (0), dOnMeas (1) +uint8_t GetAwMode(); // iAwCondition (0, iAwClamp (1), iAwOff (2) ``` -Set Tunings using the last remembered `pMode`, `dMode` and `iAwMode` settings. See example [PID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino) +#### PID Set Functions -#### SetSampleTime +These functions set the internal state of the PID. ```c++ -void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs); +void SetMode(Control mode); // Set PID mode to manual, automatic or timer +void SetOutputLimits(float Min, float Max); // Set and clamps the output to (0-255 by default) +void SetTunings(float Kp, float Ki, float Kd, // set pid tunings and all computational modes + pMode pMode, dMode dMode, iAwMode iAwMode); +void SetTunings(float Kp, float Ki, float Kd); // only set pid tunings, other pid modes are unchanged +void SetControllerDirection(Action Action); // Set controller action to direct (default) or reverse +void SetSampleTimeUs(uint32_t NewSampleTimeUs); // Set PID compute sample time, default = 100000 µs +void SetProportionalMode(pMode pMode); // Set pTerm based on error (default), measurement, or both +void SetDerivativeMode(dMode dMode); // Set the dTerm, based error or measurement (default). +void SetAntiWindupMode(iAwMode iAwMode); // Set iTerm anti-windup to iAwCondition, iAwClamp or iAwOff ``` -Sets the period, in microseconds, at which the calculation is performed. The default is 100000µs (100ms). +#### SetMode + +```c++ +void QuickPID::SetMode(Control Mode); +``` + +Allows the controller Mode to be set to `manual` (0) (default) or `automatic` (1) or `timer` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. `timer` mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and `SetSampleTimeUs` use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. +See examples: [PID_AVR_Basic_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino) and [PID_AVR_Basic_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino) #### SetOutputLimits @@ -93,26 +122,20 @@ void QuickPID::SetOutputLimits(float Min, float Max); The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. -#### SetMode +#### SetTunings ```c++ -void QuickPID::SetMode(Control Mode); +void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::pOnError, + dMode dMode = dMode::dOnMeas, iAwMode iAwMode = iAwMode::iAwCondition) ``` -Allows the controller Mode to be set to `manual` (0) or `automatic` (1) or `timer` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. - -`timer` mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and SetSampleTimeUs use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. See examples: - -- [PID_AVR_Basic_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino) -- [PID_AVR_Basic_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino) - -#### Initialize +This function allows the controller's dynamic performance to be adjusted. It's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. The parameters are as described in the constructor. ```c++ -void QuickPID::Initialize(); +void QuickPID::SetTunings(float Kp, float Ki, float Kd); ``` -Does all the things that need to happen to ensure a bump-less transfer from manual to automatic mode. +Set Tunings using the last remembered `pMode`, `dMode` and `iAwMode` settings. See example [PID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino) #### SetControllerDirection @@ -122,6 +145,14 @@ void QuickPID::SetControllerDirection(Action Action); The PID will either be connected to a `direct` acting process (+Output leads to +Input) or a `reverse` acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. +#### SetSampleTime + +```c++ +void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs); +``` + +Sets the period, in microseconds, at which the calculation is performed. The default is 100000µs (100ms). + #### SetProportionalMode ```c++ @@ -146,24 +177,6 @@ void QuickPID::SetAntiWindupMode(iAwMode iAwMode); Sets the integral anti-windup mode to one of iAwClamp, which clamps the output after adding integral and proportional (on measurement) terms, or iAwCondition (default), which provides some integral correction, prevents deep saturation and reduces overshoot. Option iAwOff disables anti-windup altogether. -#### PID Query Functions - -```c++ - float GetKp(); // proportional gain - float GetKi(); // integral gain - float GetKd(); // derivative gain - float GetPterm(); // proportional component of output - float GetIterm(); // integral component of output - float GetDterm(); // derivative component of output - uint8_t GetMode(); // manual (0), automatic (1) or timer (2) - uint8_t GetDirection(); // direct (0), reverse (1) - uint8_t GetPmode(); // pOnError (0), pOnMeas (1), pOnErrorMeas (2) - uint8_t GetDmode(); // dOnError (0), dOnMeas (1) - uint8_t GetAwMode(); // iAwCondition (0, iAwClamp (1), iAwOff (2) -``` - -These functions query the internal state of the PID. - #### AnalogWrite (PWM and DAC) for ESP32 If you're using QuickPID with an ESP32 and need analogWrite compatibility, there's no need to install a library as this feature is already included. AnalogWrite [documentation](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) diff --git a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino index e83407b..5577371 100644 --- a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino +++ b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino @@ -13,19 +13,22 @@ volatile bool computeNow = false; //Define Variables we'll be connecting to float Setpoint, Input, Output; -//Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::direct); +//specify the links +QuickPID myPID(&Input, &Output, &Setpoint); void setup() { - Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval - Timer1.attachInterrupt(runPid); // attaches runPid() as a timer overflow interrupt + Timer1.initialize(sampleTimeUs); //initialize timer1, and set the time interval + Timer1.attachInterrupt(runPid); //attaches runPid() as a timer overflow interrupt //initialize the variables we're linked to Input = analogRead(PIN_INPUT); Setpoint = 100; + //apply PID gains + myPID.SetTunings(Kp, Ki, Kd); + //turn the PID on myPID.SetMode(myPID.Control::automatic); } diff --git a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino index 7458d8d..e64c3d5 100644 --- a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino +++ b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino @@ -18,7 +18,7 @@ float Setpoint, Input, Output; float Kp = 2, Ki = 5, Kd = 1; Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::direct); +QuickPID myPID(&Input, &Output, &Setpoint); void setup() { timer1.start(); @@ -27,6 +27,9 @@ void setup() { Input = analogRead(PIN_INPUT); Setpoint = 100; + //apply PID gains + myPID.SetTunings(Kp, Ki, Kd); + //turn the PID on myPID.SetMode(myPID.Control::automatic); } diff --git a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino index 3c96319..4908fcb 100644 --- a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino +++ b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino @@ -1,13 +1,11 @@ -/******************************************************** +/****************************************************************************** PID Adaptive Tuning Example - One of the benefits of the PID library is that you can - change the tuning parameters at any time. this can be - helpful if we want the controller to be agressive at some - times, and conservative at others. in the example below - we set the controller to use Conservative Tuning Parameters - when we're near setpoint and more agressive Tuning - Parameters when we're farther away. - ********************************************************/ + One of the benefits of the PID library is that you can change the tuning + parameters at any time. This can be helpful if we want the controller to + be agressive at some times and conservative at others. In the example below, + we set the controller to use conservative tuning parameters when we're near + setpoint and more agressive tuning parameters when we're farther away. + ******************************************************************************/ #include "QuickPID.h" @@ -21,8 +19,8 @@ float Setpoint, Input, Output; float aggKp = 4, aggKi = 0.2, aggKd = 1; float consKp = 1, consKi = 0.05, consKd = 0.25; -//Specify the links and initial tuning parameters -QuickPID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, myPID.Action::direct); +//Specify the links +QuickPID myPID(&Input, &Output, &Setpoint); void setup() { diff --git a/examples/PID_Basic/PID_Basic.ino b/examples/PID_Basic/PID_Basic.ino index e616b3b..6f569f5 100644 --- a/examples/PID_Basic/PID_Basic.ino +++ b/examples/PID_Basic/PID_Basic.ino @@ -14,7 +14,17 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::direct); +/* Instantiate PID with simplified constructor, remaining parameters use defaults: + Parameter Default Description + 4 Kp = 0 proportional gain + 5 Ki = 0 integral gain + 6 Kd = 0 derivative gain + 7 pMode::pOnError proportional on error + 8 dMode::dOnMeas derivative on measurement + 9 iAwMode::iAwCondition advanced integral anti-windup + 10 Action::direct direct acting controller +*/ +QuickPID myPID(&Input, &Output, &Setpoint); void setup() { @@ -22,6 +32,9 @@ void setup() Input = analogRead(PIN_INPUT); Setpoint = 100; + //apply PID gains + myPID.SetTunings(Kp, Ki, Kd); + //turn the PID on myPID.SetMode(myPID.Control::automatic); } diff --git a/examples/PID_Controller_Options/PID_Controller_Options.ino b/examples/PID_Controller_Options/PID_Controller_Options.ino index e68db4d..7faade6 100644 --- a/examples/PID_Controller_Options/PID_Controller_Options.ino +++ b/examples/PID_Controller_Options/PID_Controller_Options.ino @@ -1,24 +1,5 @@ /************************************************************************************** For testing and checking parameter options. From QuickPID.h: - - enum class Mode : uint8_t {manual, automatic, timer}; // controller modes - enum class Action : uint8_t {direct, reverse}; // controller actions - enum class pMode : uint8_t {pOnError, pOnMeas, pOnErrorMeas}; // proportional modes - enum class dMode : uint8_t {dOnError, dOnMeas}; // derivative modes - enum class iAwMode : uint8_t {iAwCondition, iAwClamp, iAwOff}; // integral anti-windup modes - - float GetKp(); // proportional gain - float GetKi(); // integral gain - float GetKd(); // derivative gain - float GetPterm(); // proportional component of output - float GetIterm(); // integral component of output - float GetDterm(); // derivative component of output - uint8_t GetMode(); // manual (0), automatic (1) or timer (2) - uint8_t GetDirection(); // direct (0), reverse (1) - uint8_t GetPmode(); // pOnError (0), pOnMeas (1), pOnErrorMeas (2) - uint8_t GetDmode(); // dOnError (0), dOnMeas (1) - uint8_t GetAwMode(); // iAwCondition (0), iAwClamp (1), iAwOff (2) - Documentation (GitHub): https://github.com/Dlloydev/QuickPID **************************************************************************************/ @@ -30,11 +11,11 @@ const byte outputPin = 3; //Define variables we'll be connecting to float Setpoint = 0, Input = 0, Output = 0, Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.pMode::pOnError, myPID.dMode::dOnMeas, myPID.iAwMode::iAwCondition, myPID.Action::direct); -/* pOnError dOnError iAwCondition direct - pOnMeas dOnMeas iAwClamp reverse - pOnErrorMeas iAwOff -*/ +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, /* OPTIONS */ + myPID.pMode::pOnError, /* pOnError (0), pOnMeas (1), pOnErrorMeas (2) */ + myPID.dMode::dOnMeas, /* dOnError (0), dOnMeas (1) */ + myPID.iAwMode::iAwCondition, /* iAwCondition (0), iAwClamp (1), iAwOff (2) */ + myPID.Action::direct); /* direct (0), reverse (1) */ void setup() { diff --git a/examples/PID_RelayOutput/PID_RelayOutput.ino b/examples/PID_RelayOutput/PID_RelayOutput.ino index 3b400b0..6ca75db 100644 --- a/examples/PID_RelayOutput/PID_RelayOutput.ino +++ b/examples/PID_RelayOutput/PID_RelayOutput.ino @@ -26,7 +26,7 @@ float Setpoint, Input, Output; //Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.Action::direct); +QuickPID myPID(&Input, &Output, &Setpoint); unsigned int WindowSize = 5000; unsigned int minWindow = 500; @@ -43,6 +43,9 @@ void setup() //tell the PID to range between 0 and the full window size myPID.SetOutputLimits(0, WindowSize); + //apply PID gains + myPID.SetTunings(Kp, Ki, Kd); + //turn the PID on myPID.SetMode(myPID.Control::automatic); } diff --git a/library.json b/library.json index ddab6e0..bf407c0 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.0.3", + "version": "3.0.4", "description": "A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal", "repository": @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~3.0.3" + "QuickPID": "~3.0.4" }, "frameworks": "arduino", "platforms": "*" diff --git a/library.properties b/library.properties index 08d6de7..3ff98d4 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.0.3 +version=3.0.4 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index f418cbc..f276e85 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.0.3 + QuickPID Library for Arduino - Version 3.0.4 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ From e1f98edf39fe7b02f8fc1ce9e775fc050515f7e5 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Tue, 21 Dec 2021 10:48:08 -0500 Subject: [PATCH 077/101] Update Minor update to readme and example files --- README.md | 2 +- examples/PID_Basic/PID_Basic.ino | 12 +----------- .../PID_Controller_Options.ino | 8 ++++---- library.properties | 2 +- 4 files changed, 7 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index b9cd74b..907d6c5 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) +# QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) [![PlatformIO Registry](https://badges.registry.platformio.org/packages/dlloydev/library/QuickPID.svg)](https://registry.platformio.org/packages/libraries/dlloydev/QuickPID) QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library except for using a more advanced anti-windup mode. Integral anti-windup can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include `timer`, which allows external timer or ISR timing control. diff --git a/examples/PID_Basic/PID_Basic.ino b/examples/PID_Basic/PID_Basic.ino index 6f569f5..73b2624 100644 --- a/examples/PID_Basic/PID_Basic.ino +++ b/examples/PID_Basic/PID_Basic.ino @@ -11,19 +11,9 @@ //Define Variables we'll be connecting to float Setpoint, Input, Output; -//Specify the links and initial tuning parameters float Kp = 2, Ki = 5, Kd = 1; -/* Instantiate PID with simplified constructor, remaining parameters use defaults: - Parameter Default Description - 4 Kp = 0 proportional gain - 5 Ki = 0 integral gain - 6 Kd = 0 derivative gain - 7 pMode::pOnError proportional on error - 8 dMode::dOnMeas derivative on measurement - 9 iAwMode::iAwCondition advanced integral anti-windup - 10 Action::direct direct acting controller -*/ +//Specify PID links QuickPID myPID(&Input, &Output, &Setpoint); void setup() diff --git a/examples/PID_Controller_Options/PID_Controller_Options.ino b/examples/PID_Controller_Options/PID_Controller_Options.ino index 7faade6..f413dab 100644 --- a/examples/PID_Controller_Options/PID_Controller_Options.ino +++ b/examples/PID_Controller_Options/PID_Controller_Options.ino @@ -12,10 +12,10 @@ const byte outputPin = 3; float Setpoint = 0, Input = 0, Output = 0, Kp = 2, Ki = 5, Kd = 1; QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, /* OPTIONS */ - myPID.pMode::pOnError, /* pOnError (0), pOnMeas (1), pOnErrorMeas (2) */ - myPID.dMode::dOnMeas, /* dOnError (0), dOnMeas (1) */ - myPID.iAwMode::iAwCondition, /* iAwCondition (0), iAwClamp (1), iAwOff (2) */ - myPID.Action::direct); /* direct (0), reverse (1) */ + myPID.pMode::pOnError, /* pOnError, pOnMeas, pOnErrorMeas */ + myPID.dMode::dOnMeas, /* dOnError, dOnMeas */ + myPID.iAwMode::iAwCondition, /* iAwCondition, iAwClamp, iAwOff */ + myPID.Action::direct); /* direct, reverse */ void setup() { diff --git a/library.properties b/library.properties index 3ff98d4..e896ab3 100644 --- a/library.properties +++ b/library.properties @@ -2,7 +2,7 @@ name=QuickPID version=3.0.4 author=David Lloyd maintainer=David Lloyd -sentence=A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. +sentence=A fast PID controller with Integral anti-windup, timer mode and multiple options for Proportional, Derivative and anti-windup modes of operation. paragraph=Also includes analogWrite compatibility for ESP32 and ESP32-S2. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID From a08205389566f67630750f679eee49ae6ba03532 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 1 Jan 2022 15:31:57 -0500 Subject: [PATCH 078/101] New sTune (autotune) examples --- LICENSE | 2 +- examples/Autotune_PID_v1/Autotune_PID_v1.ino | 68 ++++++++++++++++++ .../Autotune_QuickPID/Autotune_QuickPID.ino | 71 +++++++++++++++++++ library.json | 8 +-- library.properties | 2 +- src/QuickPID.cpp | 2 +- 6 files changed, 146 insertions(+), 7 deletions(-) create mode 100644 examples/Autotune_PID_v1/Autotune_PID_v1.ino create mode 100644 examples/Autotune_QuickPID/Autotune_QuickPID.ino diff --git a/LICENSE b/LICENSE index 1d6e014..5f44edf 100644 --- a/LICENSE +++ b/LICENSE @@ -1,4 +1,4 @@ -Copyright 2021 David Lloyd +Copyright 2022 David Lloyd Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in diff --git a/examples/Autotune_PID_v1/Autotune_PID_v1.ino b/examples/Autotune_PID_v1/Autotune_PID_v1.ino new file mode 100644 index 0000000..3c93fba --- /dev/null +++ b/examples/Autotune_PID_v1/Autotune_PID_v1.ino @@ -0,0 +1,68 @@ +/******************************************************************** + PID_v1 Autotune Example (using Temperature Control Lab) + http://apmonitor.com/pdc/index.php/Main/ArduinoTemperatureControl + ********************************************************************/ + +#include +#include + +const uint8_t inputPin = 0; +const uint8_t outputPin = 3; + +//user settings +const uint32_t testTimeSec = 300; +const float outputStart = 0; +const float outputStep = 20; +const uint16_t samples = 500; +const uint32_t settleTimeSec = 10; + +//input constants +const float mvResolution = 3300 / 1024.0f; +const float bias = 50; + +double Input = 0, Output = 0, Setpoint = 30; //myPID +float input = 0, output = 0, kp = 0, ki = 0, kd = 0; //tuner + +PID myPID(&Input, &Output, &Setpoint, 0, 0, 0, DIRECT); + +sTune tuner = sTune(&input, &output, tuner.zieglerNicholsPID, tuner.directIP, tuner.printALL); +/* zieglerNicholsPI directIP serialOFF + zieglerNicholsPID direct5T printALL + tyreusLuybenPI reverseIP printSUMMARY + tyreusLuybenPID reverse5T printDEBUG + cianconeMarlinPI printPIDTUNER + cianconeMarlinPID serialPLOTTER + amigofPID + pessenIntegralPID + someOvershootPID + noOvershootPID +*/ +void setup() { + analogReference(EXTERNAL); + Serial.begin(115200); + analogWrite(outputPin, outputStart); + tuner.Configure(outputStart, outputStep, testTimeSec, settleTimeSec, samples); +} + +void loop() { + + switch (tuner.Run()) { + case tuner.inOut: + input = (analogRead(inputPin) / mvResolution) - bias; + analogWrite(outputPin, output); + break; + + case tuner.tunings: + tuner.SetAutoTunings(&kp, &ki, &kd); + myPID.SetMode(AUTOMATIC); + myPID.SetSampleTime((testTimeSec * 1000) / samples); + myPID.SetTunings(kp, ki, kd); + break; + + case tuner.runPid: + Input = (analogRead(inputPin) / mvResolution) - bias; + myPID.Compute(); + analogWrite(outputPin, Output); + break; + } +} diff --git a/examples/Autotune_QuickPID/Autotune_QuickPID.ino b/examples/Autotune_QuickPID/Autotune_QuickPID.ino new file mode 100644 index 0000000..07a71f3 --- /dev/null +++ b/examples/Autotune_QuickPID/Autotune_QuickPID.ino @@ -0,0 +1,71 @@ +/******************************************************************** + QuickPID Autotune Example (using Temperature Control Lab) + http://apmonitor.com/pdc/index.php/Main/ArduinoTemperatureControl + ********************************************************************/ + +#include +#include + +const uint8_t inputPin = 0; +const uint8_t outputPin = 3; + +//user settings +const uint32_t testTimeSec = 300; +const float outputStart = 0; +const float outputStep = 20; +const uint16_t samples = 500; +const uint32_t settleTimeSec = 10; + +//input constants +const float mvResolution = 3300 / 1024.0f; +const float bias = 50; + +float Input = 0, Output = 0, Setpoint = 30, Kp = 0, Ki = 0, Kd = 0; + +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, + myPID.pMode::pOnError, + myPID.dMode::dOnMeas, + myPID.iAwMode::iAwClamp, + myPID.Action::reverse); + +sTune tuner = sTune(&Input, &Output, tuner.zieglerNicholsPID, tuner.directIP, tuner.printALL); +/* zieglerNicholsPI directIP serialOFF + zieglerNicholsPID direct5T printALL + tyreusLuybenPI reverseIP printSUMMARY + tyreusLuybenPID reverse5T printDEBUG + cianconeMarlinPI printPIDTUNER + cianconeMarlinPID serialPLOTTER + amigofPID + pessenIntegralPID + someOvershootPID + noOvershootPID +*/ +void setup() { + analogReference(EXTERNAL); + Serial.begin(115200); + analogWrite(outputPin, outputStart); + tuner.Configure(outputStart, outputStep, testTimeSec, settleTimeSec, samples); +} + +void loop() { + + switch (tuner.Run()) { + case tuner.inOut: + Input = (analogRead(inputPin) / mvResolution) - bias; + analogWrite(outputPin, Output); + break; + + case tuner.tunings: + tuner.SetAutoTunings(&Kp, &Ki, &Kd); + myPID.SetMode(myPID.Control::automatic); + myPID.SetSampleTimeUs((testTimeSec * 1000000) / samples); + myPID.SetTunings(Kp, Ki, Kd); + break; + + case tuner.runPid: + Input = (analogRead(inputPin) / mvResolution) - bias; + myPID.Compute(); + analogWrite(outputPin, Output); + break; + } +} diff --git a/library.json b/library.json index bf407c0..a9bd5ac 100644 --- a/library.json +++ b/library.json @@ -1,8 +1,8 @@ { "name": "QuickPID", - "version": "3.0.4", - "description": "A fast PID controller with Integral anti-windup, TIMER mode and multiple options for Proportional, Derivative and anti-windup modes of operation. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", - "keywords": "PID, controller, signal", + "version": "3.0.5", + "description": "A fast PID controller with Integral anti-windup, timer mode and multiple options for Proportional, Derivative and anti-windup modes of operation. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", + "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": { "type": "git", @@ -20,7 +20,7 @@ "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", "dependencies": { - "QuickPID": "~3.0.4" + "QuickPID": "~3.0.5" }, "frameworks": "arduino", "platforms": "*" diff --git a/library.properties b/library.properties index e896ab3..f9e3285 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.0.4 +version=3.0.5 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with Integral anti-windup, timer mode and multiple options for Proportional, Derivative and anti-windup modes of operation. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index f276e85..8935061 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.0.4 + QuickPID Library for Arduino - Version 3.0.5 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ From 5f774cc58f3acf5baa74d593df12227893bd42e2 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 2 Jan 2022 03:04:29 -0500 Subject: [PATCH 079/101] Update Update ReadMe --- README.md | 109 +++------------------------------------------ library.json | 2 +- library.properties | 4 +- 3 files changed, 10 insertions(+), 105 deletions(-) diff --git a/README.md b/README.md index 907d6c5..6e1752c 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,12 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) [![PlatformIO Registry](https://badges.registry.platformio.org/packages/dlloydev/library/QuickPID.svg)](https://registry.platformio.org/packages/libraries/dlloydev/QuickPID) -QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library except for using a more advanced anti-windup mode. Integral anti-windup can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include `timer`, which allows external timer or ISR timing control. +QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library except for using a more advanced anti-windup mode. Integral anti-windup can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include timer, which allows external timer or ISR timing control. + +### Need Autotune? + +###### Get [sTune](https://github.com/Dlloydev/sTune) [![arduino-library-badge](https://camo.githubusercontent.com/2f6685943640fc03f25d1851ccbb5dbcd5963d9e3c26341c9f2b1c0f564c9016/68747470733a2f2f7777772e617264752d62616467652e636f6d2f62616467652f7354756e652e7376673f)](https://www.ardu-badge.com/sTune) [![PlatformIO Registry](https://camo.githubusercontent.com/269bbd52c6be846bab52a8afe727604d3bce8e7f068e317e36042fe3c9330203/68747470733a2f2f6261646765732e72656769737472792e706c6174666f726d696f2e6f72672f7061636b616765732f646c6c6f796465762f6c6962726172792f7354756e652e737667)](https://registry.platformio.org/packages/libraries/dlloydev/sTune) + +A very fast autotuner that's capable of on-the-fly tunings. Example: [Autotune_QuickPID.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/Autotune_QuickPID/Autotune_QuickPID.ino) ### Features @@ -15,14 +21,6 @@ Development began with a fork of the Arduino PID Library. Modifications and new - [x] Derivative on error `dOnError` and measurement `dOnMeas` options - [x] New PID Query Functions `GetPterm`, `GetIterm`, `GetDterm`, `GetPmode`, `GetDmode` and `GetAwMode` - [x] New integral anti-windup options `iAwCondition`, `iAwClamp` and `iAwOff` -- [x] New `reverse` mode only changes sign of `error` and `dInput` -- [x] Uses `float` instead of `double` - -#### Direct and Reverse Controller Action - -Direct controller action leads the output to increase when the input is larger than the setpoint (i.e. heating process). Reverse controller leads the output to decrease when the input is larger than the setpoint (i.e. cooling process). - -When the controller is set to `reverse` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `reverse` acting process from a process that's `direct` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). ### Functions @@ -105,96 +103,3 @@ void SetDerivativeMode(dMode dMode); // Set the dTerm, based error or void SetAntiWindupMode(iAwMode iAwMode); // Set iTerm anti-windup to iAwCondition, iAwClamp or iAwOff ``` -#### SetMode - -```c++ -void QuickPID::SetMode(Control Mode); -``` - -Allows the controller Mode to be set to `manual` (0) (default) or `automatic` (1) or `timer` (2). when the transition from manual to automatic or timer occurs, the controller is automatically initialized. `timer` mode is used when the PID compute is called by an external timer function or ISR. In this mode, the timer function and `SetSampleTimeUs` use the same time period value. The PID compute and timer will always remain in sync because the sample time variable and calculations remain constant. -See examples: [PID_AVR_Basic_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino) and [PID_AVR_Basic_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino) - -#### SetOutputLimits - -```c++ -void QuickPID::SetOutputLimits(float Min, float Max); -``` - -The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. - -#### SetTunings - -```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd, pMode pMode = pMode::pOnError, - dMode dMode = dMode::dOnMeas, iAwMode iAwMode = iAwMode::iAwCondition) -``` - -This function allows the controller's dynamic performance to be adjusted. It's called automatically from the constructor, but tunings can also be adjusted on the fly during normal operation. The parameters are as described in the constructor. - -```c++ -void QuickPID::SetTunings(float Kp, float Ki, float Kd); -``` - -Set Tunings using the last remembered `pMode`, `dMode` and `iAwMode` settings. See example [PID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino) - -#### SetControllerDirection - -```c++ -void QuickPID::SetControllerDirection(Action Action); -``` - -The PID will either be connected to a `direct` acting process (+Output leads to +Input) or a `reverse` acting process (+Output leads to -Input.) We need to know which one, because otherwise we may increase the output when we should be decreasing. This is called from the constructor. - -#### SetSampleTime - -```c++ -void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs); -``` - -Sets the period, in microseconds, at which the calculation is performed. The default is 100000µs (100ms). - -#### SetProportionalMode - -```c++ -void QuickPID::SetProportionalMode(pMode pMode); -``` - -Sets the computation method for the proportional term, to compute based either on error (default), on measurement, or the average of both. - -#### SetDerivativeMode - -```c++ -void QuickPID::SetDerivativeMode(dMode dMode); -``` - -Sets the computation method for the derivative term, to compute based either on error or on measurement (default). - -#### SetAntiWindupMode - -```c++ -void QuickPID::SetAntiWindupMode(iAwMode iAwMode); -``` - -Sets the integral anti-windup mode to one of iAwClamp, which clamps the output after adding integral and proportional (on measurement) terms, or iAwCondition (default), which provides some integral correction, prevents deep saturation and reduces overshoot. Option iAwOff disables anti-windup altogether. - -#### AnalogWrite (PWM and DAC) for ESP32 - -If you're using QuickPID with an ESP32 and need analogWrite compatibility, there's no need to install a library as this feature is already included. AnalogWrite [documentation](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) - -### Original README (Arduino PID) - -``` -*************************************************************** -* Arduino PID Library - Version 1.2.1 -* by Brett Beauregard brettbeauregard.com -* -* This Library is licensed under the MIT License -*************************************************************** -``` - - - For a detailed explanation of the original code, please visit: - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ - - - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary - ------- diff --git a/library.json b/library.json index a9bd5ac..9cf794c 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", "version": "3.0.5", - "description": "A fast PID controller with Integral anti-windup, timer mode and multiple options for Proportional, Derivative and anti-windup modes of operation. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", + "description": "A fast PID controller with timer mode and multiple options for Proportional, Derivative and Integral anti-windup modes of operation. Includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": { diff --git a/library.properties b/library.properties index f9e3285..c7330ac 100644 --- a/library.properties +++ b/library.properties @@ -2,8 +2,8 @@ name=QuickPID version=3.0.5 author=David Lloyd maintainer=David Lloyd -sentence=A fast PID controller with Integral anti-windup, timer mode and multiple options for Proportional, Derivative and anti-windup modes of operation. -paragraph=Also includes analogWrite compatibility for ESP32 and ESP32-S2. +sentence=A fast PID controller with timer mode and multiple options for Proportional, Derivative and Integral anti-windup modes of operation. +paragraph=Includes analogWrite compatibility for ESP32 and ESP32-S2. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID architectures=* From 98414168546debf488ae345e38747254acaf87fe Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 2 Jan 2022 03:09:16 -0500 Subject: [PATCH 080/101] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 6e1752c..e3cbe38 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ QuickPID is an updated implementation of the Arduino PID library with additional ### Need Autotune? -###### Get [sTune](https://github.com/Dlloydev/sTune) [![arduino-library-badge](https://camo.githubusercontent.com/2f6685943640fc03f25d1851ccbb5dbcd5963d9e3c26341c9f2b1c0f564c9016/68747470733a2f2f7777772e617264752d62616467652e636f6d2f62616467652f7354756e652e7376673f)](https://www.ardu-badge.com/sTune) [![PlatformIO Registry](https://camo.githubusercontent.com/269bbd52c6be846bab52a8afe727604d3bce8e7f068e317e36042fe3c9330203/68747470733a2f2f6261646765732e72656769737472792e706c6174666f726d696f2e6f72672f7061636b616765732f646c6c6f796465762f6c6962726172792f7354756e652e737667)](https://registry.platformio.org/packages/libraries/dlloydev/sTune) +#### Get [sTune](https://github.com/Dlloydev/sTune) [![arduino-library-badge](https://camo.githubusercontent.com/2f6685943640fc03f25d1851ccbb5dbcd5963d9e3c26341c9f2b1c0f564c9016/68747470733a2f2f7777772e617264752d62616467652e636f6d2f62616467652f7354756e652e7376673f)](https://www.ardu-badge.com/sTune) [![PlatformIO Registry](https://camo.githubusercontent.com/269bbd52c6be846bab52a8afe727604d3bce8e7f068e317e36042fe3c9330203/68747470733a2f2f6261646765732e72656769737472792e706c6174666f726d696f2e6f72672f7061636b616765732f646c6c6f796465762f6c6962726172792f7354756e652e737667)](https://registry.platformio.org/packages/libraries/dlloydev/sTune) A very fast autotuner that's capable of on-the-fly tunings. Example: [Autotune_QuickPID.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/Autotune_QuickPID/Autotune_QuickPID.ino) From 1f823eecf9fb501009d07fa6fb41faff3ba4efae Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 3 Jan 2022 10:59:41 -0500 Subject: [PATCH 081/101] Update Update dependencies in library.json --- library.json | 5 +---- library.properties | 2 +- src/QuickPID.cpp | 2 +- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/library.json b/library.json index 9cf794c..9023204 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.0.5", + "version": "3.0.6", "description": "A fast PID controller with timer mode and multiple options for Proportional, Derivative and Integral anti-windup modes of operation. Includes analogWrite compatibility for ESP32 and ESP32-S2.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": @@ -19,9 +19,6 @@ ], "license": "MIT", "homepage": "https://github.com/Dlloydev/QuickPID", - "dependencies": { - "QuickPID": "~3.0.5" - }, "frameworks": "arduino", "platforms": "*" } diff --git a/library.properties b/library.properties index c7330ac..e86073c 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.0.5 +version=3.0.6 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with timer mode and multiple options for Proportional, Derivative and Integral anti-windup modes of operation. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 8935061..97bfd37 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.0.5 + QuickPID Library for Arduino - Version 3.0.6 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ From 44fa110dd8e64a4fbbafc4dfd9d36bde30275a2a Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 5 Jan 2022 09:53:09 -0500 Subject: [PATCH 082/101] Update - Since [ESP32 Arduino 2.0.1](https://github.com/espressif/arduino-esp32/releases/tag/2.0.1) or newer has analogWrite basic support based on LEDC, the included library [ESP32-ESP32S2-AnalogWrite](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) has been removed from QuickPID. Please Install SP32-ESP32S2-AnalogWrite separately if you require its advanced pin management or other unique features. - Uses dispKp = 0, dispKi = 0 and dispKd = 0 for both display purposes and defaults. Removed defKp, defKi and defKd. - [Autotune_QuickPID.ino](https://github.com/Dlloydev/sTune/blob/main/examples/Autotune_QuickPID/Autotune_QuickPID.ino) example is now at the [sTune](https://github.com/Dlloydev/sTune) repository. --- README.md | 13 +- examples/Autotune_PID_v1/Autotune_PID_v1.ino | 68 ----- .../Autotune_QuickPID/Autotune_QuickPID.ino | 71 ----- library.json | 4 +- library.properties | 6 +- src/QuickPID.cpp | 8 +- src/QuickPID.h | 14 +- src/analogWrite.cpp | 281 ------------------ src/analogWrite.h | 53 ---- 9 files changed, 18 insertions(+), 500 deletions(-) delete mode 100644 examples/Autotune_PID_v1/Autotune_PID_v1.ino delete mode 100644 examples/Autotune_QuickPID/Autotune_QuickPID.ino delete mode 100644 src/analogWrite.cpp delete mode 100644 src/analogWrite.h diff --git a/README.md b/README.md index e3cbe38..4237acb 100644 --- a/README.md +++ b/README.md @@ -2,12 +2,6 @@ QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library except for using a more advanced anti-windup mode. Integral anti-windup can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include timer, which allows external timer or ISR timing control. -### Need Autotune? - -#### Get [sTune](https://github.com/Dlloydev/sTune) [![arduino-library-badge](https://camo.githubusercontent.com/2f6685943640fc03f25d1851ccbb5dbcd5963d9e3c26341c9f2b1c0f564c9016/68747470733a2f2f7777772e617264752d62616467652e636f6d2f62616467652f7354756e652e7376673f)](https://www.ardu-badge.com/sTune) [![PlatformIO Registry](https://camo.githubusercontent.com/269bbd52c6be846bab52a8afe727604d3bce8e7f068e317e36042fe3c9330203/68747470733a2f2f6261646765732e72656769737472792e706c6174666f726d696f2e6f72672f7061636b616765732f646c6c6f796465762f6c6962726172792f7354756e652e737667)](https://registry.platformio.org/packages/libraries/dlloydev/sTune) - -A very fast autotuner that's capable of on-the-fly tunings. Example: [Autotune_QuickPID.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/Autotune_QuickPID/Autotune_QuickPID.ino) - ### Features Development began with a fork of the Arduino PID Library. Modifications and new features have been added as described in the [releases](https://github.com/Dlloydev/QuickPID/releases). @@ -16,7 +10,6 @@ Development began with a fork of the Arduino PID Library. Modifications and new - [x] New functions added: `SetProportionalMode`, `SetDerivativeMode` and `SetAntiWindupMode` - [x] `timer` mode for calling PID compute by an external timer function or ISR -- [x] `analogWrite()` support for ESP32 and ESP32-S2 - [x] Proportional on error `pOnError`, measurement `pOnMeas` or both `pOnErrorMeas` options - [x] Derivative on error `dOnError` and measurement `dOnMeas` options - [x] New PID Query Functions `GetPterm`, `GetIterm`, `GetDterm`, `GetPmode`, `GetDmode` and `GetAwMode` @@ -103,3 +96,9 @@ void SetDerivativeMode(dMode dMode); // Set the dTerm, based error or void SetAntiWindupMode(iAwMode iAwMode); // Set iTerm anti-windup to iAwCondition, iAwClamp or iAwOff ``` +### Need Autotune? + +#### Get [sTune](https://github.com/Dlloydev/sTune) [![arduino-library-badge](https://www.ardu-badge.com/badge/sTune.svg?)](https://www.ardu-badge.com/sTune) [![PlatformIO Registry](https://badges.registry.platformio.org/packages/dlloydev/library/sTune.svg)](https://registry.platformio.org/packages/libraries/dlloydev/sTune) + +A very fast autotuner capable of on-the-fly tunings and more. Example: [Autotune_QuickPID.ino](https://github.com/Dlloydev/sTune/blob/main/examples/Autotune_QuickPID/Autotune_QuickPID.ino) + diff --git a/examples/Autotune_PID_v1/Autotune_PID_v1.ino b/examples/Autotune_PID_v1/Autotune_PID_v1.ino deleted file mode 100644 index 3c93fba..0000000 --- a/examples/Autotune_PID_v1/Autotune_PID_v1.ino +++ /dev/null @@ -1,68 +0,0 @@ -/******************************************************************** - PID_v1 Autotune Example (using Temperature Control Lab) - http://apmonitor.com/pdc/index.php/Main/ArduinoTemperatureControl - ********************************************************************/ - -#include -#include - -const uint8_t inputPin = 0; -const uint8_t outputPin = 3; - -//user settings -const uint32_t testTimeSec = 300; -const float outputStart = 0; -const float outputStep = 20; -const uint16_t samples = 500; -const uint32_t settleTimeSec = 10; - -//input constants -const float mvResolution = 3300 / 1024.0f; -const float bias = 50; - -double Input = 0, Output = 0, Setpoint = 30; //myPID -float input = 0, output = 0, kp = 0, ki = 0, kd = 0; //tuner - -PID myPID(&Input, &Output, &Setpoint, 0, 0, 0, DIRECT); - -sTune tuner = sTune(&input, &output, tuner.zieglerNicholsPID, tuner.directIP, tuner.printALL); -/* zieglerNicholsPI directIP serialOFF - zieglerNicholsPID direct5T printALL - tyreusLuybenPI reverseIP printSUMMARY - tyreusLuybenPID reverse5T printDEBUG - cianconeMarlinPI printPIDTUNER - cianconeMarlinPID serialPLOTTER - amigofPID - pessenIntegralPID - someOvershootPID - noOvershootPID -*/ -void setup() { - analogReference(EXTERNAL); - Serial.begin(115200); - analogWrite(outputPin, outputStart); - tuner.Configure(outputStart, outputStep, testTimeSec, settleTimeSec, samples); -} - -void loop() { - - switch (tuner.Run()) { - case tuner.inOut: - input = (analogRead(inputPin) / mvResolution) - bias; - analogWrite(outputPin, output); - break; - - case tuner.tunings: - tuner.SetAutoTunings(&kp, &ki, &kd); - myPID.SetMode(AUTOMATIC); - myPID.SetSampleTime((testTimeSec * 1000) / samples); - myPID.SetTunings(kp, ki, kd); - break; - - case tuner.runPid: - Input = (analogRead(inputPin) / mvResolution) - bias; - myPID.Compute(); - analogWrite(outputPin, Output); - break; - } -} diff --git a/examples/Autotune_QuickPID/Autotune_QuickPID.ino b/examples/Autotune_QuickPID/Autotune_QuickPID.ino deleted file mode 100644 index 07a71f3..0000000 --- a/examples/Autotune_QuickPID/Autotune_QuickPID.ino +++ /dev/null @@ -1,71 +0,0 @@ -/******************************************************************** - QuickPID Autotune Example (using Temperature Control Lab) - http://apmonitor.com/pdc/index.php/Main/ArduinoTemperatureControl - ********************************************************************/ - -#include -#include - -const uint8_t inputPin = 0; -const uint8_t outputPin = 3; - -//user settings -const uint32_t testTimeSec = 300; -const float outputStart = 0; -const float outputStep = 20; -const uint16_t samples = 500; -const uint32_t settleTimeSec = 10; - -//input constants -const float mvResolution = 3300 / 1024.0f; -const float bias = 50; - -float Input = 0, Output = 0, Setpoint = 30, Kp = 0, Ki = 0, Kd = 0; - -QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, - myPID.pMode::pOnError, - myPID.dMode::dOnMeas, - myPID.iAwMode::iAwClamp, - myPID.Action::reverse); - -sTune tuner = sTune(&Input, &Output, tuner.zieglerNicholsPID, tuner.directIP, tuner.printALL); -/* zieglerNicholsPI directIP serialOFF - zieglerNicholsPID direct5T printALL - tyreusLuybenPI reverseIP printSUMMARY - tyreusLuybenPID reverse5T printDEBUG - cianconeMarlinPI printPIDTUNER - cianconeMarlinPID serialPLOTTER - amigofPID - pessenIntegralPID - someOvershootPID - noOvershootPID -*/ -void setup() { - analogReference(EXTERNAL); - Serial.begin(115200); - analogWrite(outputPin, outputStart); - tuner.Configure(outputStart, outputStep, testTimeSec, settleTimeSec, samples); -} - -void loop() { - - switch (tuner.Run()) { - case tuner.inOut: - Input = (analogRead(inputPin) / mvResolution) - bias; - analogWrite(outputPin, Output); - break; - - case tuner.tunings: - tuner.SetAutoTunings(&Kp, &Ki, &Kd); - myPID.SetMode(myPID.Control::automatic); - myPID.SetSampleTimeUs((testTimeSec * 1000000) / samples); - myPID.SetTunings(Kp, Ki, Kd); - break; - - case tuner.runPid: - Input = (analogRead(inputPin) / mvResolution) - bias; - myPID.Compute(); - analogWrite(outputPin, Output); - break; - } -} diff --git a/library.json b/library.json index 9023204..7221c10 100644 --- a/library.json +++ b/library.json @@ -1,7 +1,7 @@ { "name": "QuickPID", - "version": "3.0.6", - "description": "A fast PID controller with timer mode and multiple options for Proportional, Derivative and Integral anti-windup modes of operation. Includes analogWrite compatibility for ESP32 and ESP32-S2.", + "version": "3.1.0", + "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": { diff --git a/library.properties b/library.properties index e86073c..5239c9a 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=QuickPID -version=3.0.6 +version=3.1.0 author=David Lloyd maintainer=David Lloyd -sentence=A fast PID controller with timer mode and multiple options for Proportional, Derivative and Integral anti-windup modes of operation. -paragraph=Includes analogWrite compatibility for ESP32 and ESP32-S2. +sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. +paragraph=Optional external timer or ISR timing control. category=Signal Input/Output url=https://github.com/Dlloydev/QuickPID architectures=* diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 97bfd37..1af5f2b 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.0.6 + QuickPID Library for Arduino - Version 3.1.0 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ @@ -17,7 +17,7 @@ reliable defaults, so we need to have the user set them. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, + float Kp = 0, float Ki = 0, float Kd = 0, pMode pMode = pMode::pOnError, dMode dMode = dMode::dOnMeas, iAwMode iAwMode = iAwMode::iAwCondition, @@ -37,7 +37,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, } /* Constructor ********************************************************************* - To allow using proportional on error without explicitly saying so. + To allow using pOnError, dOnMeas and iAwCondition without explicitly saying so. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp, float Ki, float Kd, Action action) @@ -52,7 +52,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, Simplified constructor which uses defaults for remaining parameters. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint) - : QuickPID::QuickPID(Input, Output, Setpoint, defKp, defKi, defKd, + : QuickPID::QuickPID(Input, Output, Setpoint, dispKp, dispKi, dispKd, pmode = pMode::pOnError, dmode = dMode::dOnMeas, iawmode = iAwMode::iAwCondition, diff --git a/src/QuickPID.h b/src/QuickPID.h index 8f197ba..b4c750e 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -81,13 +81,9 @@ class QuickPID { void Initialize(); - static constexpr float defKp = 0; // default controller gains - static constexpr float defKi = 0; - static constexpr float defKd = 0; - - float dispKp; // tuning parameters for display purposes. - float dispKi; - float dispKd; + float dispKp = 0; // for defaults and display + float dispKi = 0; + float dispKd = 0; float pTerm; float iTerm; float dTerm; @@ -110,8 +106,4 @@ class QuickPID { float outputSum, outMin, outMax, error, lastError, lastInput; }; // class QuickPID - -#if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) -#include "analogWrite.h" -#endif #endif // QuickPID.h diff --git a/src/analogWrite.cpp b/src/analogWrite.cpp deleted file mode 100644 index 1f19088..0000000 --- a/src/analogWrite.cpp +++ /dev/null @@ -1,281 +0,0 @@ -/********************************************************************************** - AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 2.0.9 - by dlloydev https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite - This Library is licensed under the MIT License - **********************************************************************************/ -#include - -#if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) -#include "analogWrite.h" - -namespace aw { - -#if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) -pinStatus_t pinsStatus[8] = { - {0, -1, 980, 8, 0, 0 }, {2, -1, 980, 8, 0, 0 }, - {4, -1, 980, 8, 0, 0 }, {6, -1, 980, 8, 0, 0 }, - {1, -1, 980, 8, 0, 0 }, {3, -1, 980, 8, 0, 0 }, - {5, -1, 980, 8, 0, 0 }, {7, -1, 980, 8, 0, 0 } -}; -const uint8_t chd = 1; -#else //ESP32 - -pinStatus_t pinsStatus[8] = { - { 0, -1, 980, 8, 0, 0 }, { 2, -1, 980, 8, 0, 0 }, - { 4, -1, 980, 8, 0, 0 }, { 6, -1, 980, 8, 0, 0 }, - { 8, -1, 980, 8, 0, 0 }, {10, -1, 980, 8, 0, 0 }, - {12, -1, 980, 8, 0, 0 }, {14, -1, 980, 8, 0, 0 } -}; -const uint8_t chd = 2; -#endif - -void awDetachPin(uint8_t pin, uint8_t ch) { - pinsStatus[ch / chd].pin = -1; - pinsStatus[ch / chd].value = 0; - pinsStatus[ch / chd].frequency = 980; - pinsStatus[ch / chd].resolution = 8; - pinsStatus[ch / chd].phase = 0; - ledcWrite(ch / chd, 0); - ledcSetup(ch / chd, 0, 0); - ledcDetachPin(pinsStatus[ch / chd].pin); - REG_SET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL, GPIO_MODE_DEF_DISABLE); -} - -int8_t awGetChannel(int8_t pin) { - if (!((pinMask >> pin) & 1)) return -1; //not pwm pin - for (int8_t i = 0; i < 8; i++) { - int8_t ch = pinsStatus[i].channel; - if (pinsStatus[ch / chd].pin == pin) { - return ch; - break; - } - } - for (int8_t i = 0; i < 8; i++) { - int8_t ch = pinsStatus[i].channel; - if ((REG_GET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL)) == 0) { //free pin - if (pinsStatus[ch / chd].pin == -1) { //free channel - if ((ledcRead(ch) < 1) && (ledcReadFreq(ch) < 1)) { //free timer - pinsStatus[ch / chd].pin = pin; - ledcSetup(ch, pinsStatus[ch / chd].frequency, pinsStatus[ch / chd].resolution); - ledcAttachPin(pin, ch); - return ch; - break; - } else { - pinsStatus[ch / chd].pin = 88; //occupied timer - return -1; - break; - } - } - } else { - return -1; //occupied pin - break; - } - } - return -1; -} - -} //namespace aw - -float analogWrite(int8_t pin, int32_t value) { - if (pin == DAC1 || pin == DAC2) { //dac - if (value > 255) value = 255; - dacWrite(pin, value); - } else { - int8_t ch = aw::awGetChannel(pin); - if (ch >= 0) { - if (value == -1) aw::awDetachPin(pin, ch); - else { // write PWM - uint8_t bits = aw::pinsStatus[ch / aw::chd].resolution; - if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain - if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high - if (ledcRead(ch) != value) ledcWrite(ch, value); - aw::pinsStatus[ch / aw::chd].value = value; - } - } - return ledcReadFreq(ch); - } - return 0; -} - -float analogWrite(int8_t pin, int32_t value, float frequency) { - if (pin == DAC1 || pin == DAC2) { //dac - if (value > 255) value = 255; - dacWrite(pin, value); - } else { - int8_t ch = aw::awGetChannel(pin); - if (ch >= 0) { - if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; - if (value == -1) aw::awDetachPin(pin, ch); - else { // write PWM - uint8_t bits = aw::pinsStatus[ch / aw::chd].resolution; - if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain - if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high - if (aw::pinsStatus[ch / aw::chd].frequency != frequency) { - ledcSetup(ch, frequency, bits); - ledcWrite(ch, value); - aw::pinsStatus[ch / aw::chd].frequency = frequency; - } - if (aw::pinsStatus[ch / aw::chd].value != value) { - ledcWrite(ch, value); - aw::pinsStatus[ch / aw::chd].value = value; - } - } - } - return ledcReadFreq(ch); - } - return 0; -} - -float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution) { - if (pin == DAC1 || pin == DAC2) { //dac - if (value > 255) value = 255; - dacWrite(pin, value); - } else { - int8_t ch = aw::awGetChannel(pin); - if (ch >= 0) { - if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; - if (value == -1) aw::awDetachPin(pin, ch); - else { // write PWM - uint8_t bits = resolution & 0xF; - if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain - if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high - if ((aw::pinsStatus[ch / aw::chd].frequency != frequency) || (aw::pinsStatus[ch / aw::chd].resolution != bits)) { - ledcSetup(ch, frequency, bits); - ledcWrite(ch, value); - aw::pinsStatus[ch / aw::chd].frequency = frequency; - aw::pinsStatus[ch / aw::chd].resolution = bits; - } - if (aw::pinsStatus[ch / aw::chd].value != value) { - ledcWrite(ch, value); - aw::pinsStatus[ch / aw::chd].value = value; - } - } - } - return ledcReadFreq(ch); - } - return 0; -} - -float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution, uint32_t phase) { - if (pin == DAC1 || pin == DAC2) { //dac - if (value > 255) value = 255; - dacWrite(pin, value); - } else { - int8_t ch = aw::awGetChannel(pin); - if (ch >= 0) { - if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; - if (value == -1) aw::awDetachPin(pin, ch); - else { // write PWM - uint8_t bits = resolution & 0xF; - if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain - if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high - if ((aw::pinsStatus[ch / aw::chd].frequency != frequency) || (aw::pinsStatus[ch / aw::chd].resolution != bits)) { - ledcSetup(ch, frequency, bits); - ledcWrite(ch, value); - aw::pinsStatus[ch / aw::chd].frequency = frequency; - aw::pinsStatus[ch / aw::chd].resolution = bits; - } - if (aw::pinsStatus[ch / aw::chd].phase != phase) { - uint32_t group = (ch / 8), timer = ((ch / 2) % 4); - aw::ledc_channel_config_t ledc_channel { - (uint8_t)pin, - (aw::ledc_mode_t)group, - (aw::ledc_channel_t)ch, - aw::LEDC_INTR_DISABLE, - (aw::ledc_timer_t)timer, - (uint32_t)value, - (int)phase, - }; - ledc_channel_config(&ledc_channel); - ledc_set_duty_with_hpoint((aw::ledc_mode_t)group, (aw::ledc_channel_t)ch, value, phase); - aw::pinsStatus[ch / aw::chd].phase = phase; - } - if (aw::pinsStatus[ch / aw::chd].value != value) { - ledcWrite(ch, value); - aw::pinsStatus[ch / aw::chd].value = value; - } - } - } - return ledcReadFreq(ch); - } - return 0; -} - -float analogWriteFrequency(int8_t pin, float frequency) { - int8_t ch = aw::awGetChannel(pin); - if (ch >= 0) { - if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; - if (aw::pinsStatus[ch / aw::chd].frequency != frequency) { - ledcSetup(ch, frequency, aw::pinsStatus[ch / aw::chd].resolution); - ledcWrite(ch, aw::pinsStatus[ch / aw::chd].value); - aw::pinsStatus[ch / aw::chd].frequency = frequency; - } - } - return ledcReadFreq(ch); -} - -int32_t analogWriteResolution(int8_t pin, uint8_t resolution) { - int8_t ch = aw::awGetChannel(pin); - if (ch >= 0) { - if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; - if (aw::pinsStatus[ch / aw::chd].resolution != resolution) { - ledcDetachPin(pin); - ledcSetup(ch, aw::pinsStatus[ch / aw::chd].frequency, resolution & 0xF); - ledcAttachPin(pin, ch); - ledcWrite(ch, aw::pinsStatus[ch / aw::chd].value); - aw::pinsStatus[ch / aw::chd].resolution = resolution & 0xF; - } - } - return 1 << (resolution & 0xF); -} - -void setPinsStatusDefaults(int32_t value, float frequency, uint8_t resolution, uint32_t phase) { - for (int8_t i = 0; i < 8; i++) { - aw::pinsStatus[i].value = value; - aw::pinsStatus[i].frequency = frequency; - aw::pinsStatus[i].resolution = resolution; - aw::pinsStatus[i].phase = phase; - } -} - -void printPinsStatus() { - Serial.print(F("PWM pins: ")); - for (int i = 0; i < aw::muxSize; i++) { - if ((aw::pinMask >> i) & 1) { - Serial.print(i); Serial.print(F(", ")); - } - } - Serial.println(); - - Serial.println(); - for (int i = 0; i < 8; i++) { - int ch = aw::pinsStatus[i].channel; - Serial.print(F("ch: ")); - if (ch < 10) Serial.print(F(" ")); Serial.print(ch); Serial.print(F(" ")); - Serial.print(F("Pin: ")); - if ((aw::pinsStatus[ch / aw::chd].pin >= 0) && (aw::pinsStatus[ch / aw::chd].pin < 10)) Serial.print(F(" ")); - Serial.print(aw::pinsStatus[ch / aw::chd].pin); Serial.print(F(" ")); - Serial.print(F("Hz: ")); - if (ledcReadFreq(ch) < 10000) Serial.print(F(" ")); - if (ledcReadFreq(ch) < 1000) Serial.print(F(" ")); - if (ledcReadFreq(ch) < 100) Serial.print(F(" ")); - if (ledcReadFreq(ch) < 10) Serial.print(F(" ")); - Serial.print(ledcReadFreq(ch)); Serial.print(F(" ")); - Serial.print(F("Bits: ")); - if (aw::pinsStatus[ch / aw::chd].resolution < 10) Serial.print(F(" ")); - Serial.print(aw::pinsStatus[ch / aw::chd].resolution); Serial.print(F(" ")); - Serial.print(F("Duty: ")); - if (aw::pinsStatus[ch / aw::chd].value < 10000) Serial.print(F(" ")); - if (aw::pinsStatus[ch / aw::chd].value < 1000) Serial.print(F(" ")); - if (aw::pinsStatus[ch / aw::chd].value < 100) Serial.print(F(" ")); - if (aw::pinsStatus[ch / aw::chd].value < 10) Serial.print(F(" ")); - Serial.print(aw::pinsStatus[ch / aw::chd].value); Serial.print(F(" ")); - Serial.print(F("Ø: ")); - if (aw::pinsStatus[ch / aw::chd].phase < 1000) Serial.print(F(" ")); - if (aw::pinsStatus[ch / aw::chd].phase < 100) Serial.print(F(" ")); - if (aw::pinsStatus[ch / aw::chd].phase < 10) Serial.print(F(" ")); - Serial.print(aw::pinsStatus[ch / aw::chd].phase); - Serial.println(); - } -} -#endif //ESP32 diff --git a/src/analogWrite.h b/src/analogWrite.h deleted file mode 100644 index c971d4e..0000000 --- a/src/analogWrite.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once -#ifndef _ESP32_ESP32S2_ANALOG_WRITE_ -#define _ESP32_ESP32S2_ANALOG_WRITE_ - -#include - -#if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) - -namespace aw { - -#include "driver/ledc.h" - -#if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) - -#define NUM_OUTPUT_PINS 45 -#define DAC1 17 -#define DAC2 18 -const uint8_t muxSize = 48; -const uint64_t pinMask = 0x27FE00207FFE; //PWM - -#else //ESP32 -#define NUM_OUTPUT_PINS 34 -#define DAC1 25 -#define DAC2 26 -const uint8_t muxSize = 40; -const uint64_t pinMask = 0x308EFF034; //PWM -#endif - -typedef struct pinStatus { - int8_t channel; - int8_t pin; - float frequency; - uint8_t resolution; - uint32_t value; - uint32_t phase; -} pinStatus_t; - -void awDetachPin(uint8_t pin, uint8_t ch); -int8_t awGetChannel(int8_t pin); - -} //namespace aw - -float analogWriteFrequency(int8_t pin, float frequency = 980); -int32_t analogWriteResolution(int8_t pin, uint8_t resolution = 8); -float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution, uint32_t phase); -float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution); -float analogWrite(int8_t pin, int32_t value, float frequency); -float analogWrite(int8_t pin, int32_t value); -void setPinsStatusDefaults(int32_t value = 0, float frequency = 980, uint8_t resolution = 8, uint32_t phase = 0); -void printPinsStatus(void); - -#endif //ESP32 or ARDUINO_ARCH_ESP32 -#endif //_ESP32_ESP32S2_ANALOG_WRITE_ From d096e6d60d7215268f470c566fa55b1782686784 Mon Sep 17 00:00:00 2001 From: Guilherme Gonzaga Date: Thu, 20 Jan 2022 12:50:28 -0400 Subject: [PATCH 083/101] Fix bug in overload constructor --- src/QuickPID.cpp | 8 ++++---- src/QuickPID.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 1af5f2b..b218f1e 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -21,7 +21,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, pMode pMode = pMode::pOnError, dMode dMode = dMode::dOnMeas, iAwMode iAwMode = iAwMode::iAwCondition, - Action action = Action::direct) { + Action Action = Action::direct) { myOutput = Output; myInput = Input; @@ -30,7 +30,7 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, QuickPID::SetOutputLimits(0, 255); // same default as Arduino PWM limit sampleTimeUs = 100000; // 0.1 sec default - QuickPID::SetControllerDirection(action); + QuickPID::SetControllerDirection(Action); QuickPID::SetTunings(Kp, Ki, Kd, pMode, dMode, iAwMode); lastTime = micros() - sampleTimeUs; @@ -40,12 +40,12 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, To allow using pOnError, dOnMeas and iAwCondition without explicitly saying so. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, - float Kp, float Ki, float Kd, Action action) + float Kp, float Ki, float Kd, Action Action) : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pmode = pMode::pOnError, dmode = dMode::dOnMeas, iawmode = iAwMode::iAwCondition, - action = Action::direct) { + action = Action) { } /* Constructor ********************************************************************* diff --git a/src/QuickPID.h b/src/QuickPID.h index b4c750e..394b7f3 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -26,7 +26,7 @@ class QuickPID { QuickPID(float *Input, float *Output, float *Setpoint); // Sets PID mode to manual (0), automatic (1) or timer (2). - void SetMode(Control mode); + void SetMode(Control Mode); // Performs the PID calculation. It should be called every time loop() cycles ON/OFF and calculation frequency // can be set using SetMode and SetSampleTime respectively. From 299ce37b61ec2d2d3ee48f1344d25b541f6b721b Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 21 Jan 2022 12:01:19 -0500 Subject: [PATCH 084/101] Update PID_RelayOutput.ino --- examples/PID_RelayOutput/PID_RelayOutput.ino | 107 ++++++++++--------- 1 file changed, 54 insertions(+), 53 deletions(-) diff --git a/examples/PID_RelayOutput/PID_RelayOutput.ino b/examples/PID_RelayOutput/PID_RelayOutput.ino index 6ca75db..d7a9419 100644 --- a/examples/PID_RelayOutput/PID_RelayOutput.ino +++ b/examples/PID_RelayOutput/PID_RelayOutput.ino @@ -1,67 +1,68 @@ -/************************************************************* - PID Relay Output Example - Same as basic example, except that this time, the output - is going to a digital pin which (we presume) is controlling - a relay. The pid is designed to Output an analog value, - but the relay can only be On/Off. +/**************************************************************************** + PID Relay Output Example + https://github.com/Dlloydev/QuickPID/tree/master/examples/PID_RelayOutput - To connect them together we use "time proportioning - control", essentially a really slow version of PWM. - First we decide on a window size (5000mS for example). - We then set the pid to adjust its output between 0 and that - window size. Lastly, we add some logic that translates the - PID output into "Relay On Time" with the remainder of the - window being "Relay Off Time". The minWindow setting is a - floor (minimum time) the relay would be on. - *************************************************************/ + Similar to basic example, except the output is a digital pin controlling + a mechanical relay, SSR, MOSFET or other device. To interface the PID output + to a digital pin, we use "time proportioning control" (software PWM). + First we decide on a window size (5000mS for example). We then set the pid + to adjust its output between 0 and that window size and finally we set the + PID sample time to that same window size. -#include "QuickPID.h" + The digital output has the following features: + • The PID compute rate controls the rate of updating the digital output + • All transitions are debounced (rising and falling) + • Full control range (0 to windowSize) isn't limited by debounce + • Only one call to digitalWrite() per transition + *****************************************************************************/ -#define PIN_INPUT 0 -#define RELAY_PIN 6 +#include -//Define Variables we'll be connecting to -float Setpoint, Input, Output; +// pins +const byte inputPin = 0; +const byte relayPin = 3; -//Specify the links and initial tuning parameters -float Kp = 2, Ki = 5, Kd = 1; +// user settings +const unsigned long windowSize = 5000; +const byte debounce = 50; +float Input, Output, Setpoint = 30, Kp = 2, Ki = 5, Kd = 1; -QuickPID myPID(&Input, &Output, &Setpoint); +// status +unsigned long windowStartTime, nextSwitchTime; +boolean relayStatus = false; -unsigned int WindowSize = 5000; -unsigned int minWindow = 500; -unsigned long windowStartTime; +QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, + myPID.pMode::pOnError, + myPID.dMode::dOnMeas, + myPID.iAwMode::iAwClamp, + myPID.Action::direct); -void setup() -{ - pinMode(RELAY_PIN, OUTPUT); - windowStartTime = millis(); - - //initialize the variables we're linked to - Setpoint = 100; - - //tell the PID to range between 0 and the full window size - myPID.SetOutputLimits(0, WindowSize); - - //apply PID gains - myPID.SetTunings(Kp, Ki, Kd); - - //turn the PID on +void setup() { + pinMode(relayPin, OUTPUT); + pinMode(LED_BUILTIN, OUTPUT); + myPID.SetOutputLimits(0, windowSize); + myPID.SetSampleTimeUs(windowSize * 1000); myPID.SetMode(myPID.Control::automatic); } -void loop() -{ - Input = analogRead(PIN_INPUT); +void loop() { + unsigned long msNow = millis(); + Input = analogRead(inputPin); + if (myPID.Compute()) windowStartTime = msNow; - /************************************************ - turn the output pin on/off based on pid output - ************************************************/ - if (millis() - windowStartTime >= WindowSize) - { //time to shift the Relay Window - windowStartTime += WindowSize; - myPID.Compute(); + if (!relayStatus && Output > (msNow - windowStartTime)) { + if (msNow > nextSwitchTime) { + nextSwitchTime = msNow + debounce; + relayStatus = true; + digitalWrite(relayPin, HIGH); + digitalWrite(LED_BUILTIN, HIGH); + } + } else if (relayStatus && Output < (msNow - windowStartTime)) { + if (msNow > nextSwitchTime) { + nextSwitchTime = msNow + debounce; + relayStatus = false; + digitalWrite(relayPin, LOW); + digitalWrite(LED_BUILTIN, LOW); + } } - if (((unsigned int)Output > minWindow) && ((unsigned int)Output > (millis() - windowStartTime))) digitalWrite(RELAY_PIN, HIGH); - else digitalWrite(RELAY_PIN, LOW); } From a6cd1285a89f607cccca7ba5441fbc8b8b02a5bb Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 21 Jan 2022 14:45:56 -0500 Subject: [PATCH 085/101] Update - [Fix bug in overload constructor](https://github.com/Dlloydev/QuickPID/commit/d096e6d60d7215268f470c566fa55b1782686784) Thank you @guilhermgonzaga - Updated [PID_RelayOutput.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_RelayOutput/PID_RelayOutput.ino) example where now the PID compute rate controls the rate of updating the digital output (thanks to suggestion by @SinisterRj). Other features include debouncing all transitions, full window range control and only one call to digitalWrite() per transition. Use with controlling mechanical relays, SSRs, MOSFETs or other devices. --- README.md | 2 +- keywords.txt | 3 --- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 2 +- 5 files changed, 4 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 4237acb..136d41d 100644 --- a/README.md +++ b/README.md @@ -96,7 +96,7 @@ void SetDerivativeMode(dMode dMode); // Set the dTerm, based error or void SetAntiWindupMode(iAwMode iAwMode); // Set iTerm anti-windup to iAwCondition, iAwClamp or iAwOff ``` -### Need Autotune? +### Autotuner #### Get [sTune](https://github.com/Dlloydev/sTune) [![arduino-library-badge](https://www.ardu-badge.com/badge/sTune.svg?)](https://www.ardu-badge.com/sTune) [![PlatformIO Registry](https://badges.registry.platformio.org/packages/dlloydev/library/sTune.svg)](https://registry.platformio.org/packages/libraries/dlloydev/sTune) diff --git a/keywords.txt b/keywords.txt index 01d619f..b9d0871 100644 --- a/keywords.txt +++ b/keywords.txt @@ -33,9 +33,6 @@ GetDirection KEYWORD2 GetPmode KEYWORD2 GetDmode KEYWORD2 GetAwMode KEYWORD2 -analogWrite KEYWORD2 -analogWriteFrequency KEYWORD2 -analogWriteResolution KEYWORD2 ########################################## # Constants (LITERAL1) diff --git a/library.json b/library.json index 7221c10..23b664c 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.1.0", + "version": "3.1.1", "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": diff --git a/library.properties b/library.properties index 5239c9a..dff7f03 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.1.0 +version=3.1.1 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index b218f1e..1c38bed 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.1.0 + QuickPID Library for Arduino - Version 3.1.1 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ From d0611bc797d5d6c916809773397de968e3ff9fe6 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 3 Apr 2022 11:21:46 -0400 Subject: [PATCH 086/101] Update Updated SetTunings to clear outputSum variable if Ki parameter = 0. Suggested by @brN2k, issue#48. --- README.md | 3 +-- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 3 ++- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 136d41d..b751f47 100644 --- a/README.md +++ b/README.md @@ -100,5 +100,4 @@ void SetAntiWindupMode(iAwMode iAwMode); // Set iTerm anti-windup to iAwC #### Get [sTune](https://github.com/Dlloydev/sTune) [![arduino-library-badge](https://www.ardu-badge.com/badge/sTune.svg?)](https://www.ardu-badge.com/sTune) [![PlatformIO Registry](https://badges.registry.platformio.org/packages/dlloydev/library/sTune.svg)](https://registry.platformio.org/packages/libraries/dlloydev/sTune) -A very fast autotuner capable of on-the-fly tunings and more. Example: [Autotune_QuickPID.ino](https://github.com/Dlloydev/sTune/blob/main/examples/Autotune_QuickPID/Autotune_QuickPID.ino) - +A very fast autotuner capable of on-the-fly tunings and more. diff --git a/library.json b/library.json index 23b664c..de004bb 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.1.1", + "version": "3.1.2", "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": diff --git a/library.properties b/library.properties index dff7f03..f75d60f 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.1.1 +version=3.1.2 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 1c38bed..3dfdcca 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.1.1 + QuickPID Library for Arduino - Version 3.1.2 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ @@ -125,6 +125,7 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, iAwMode iAwMode = iAwMode::iAwCondition) { if (Kp < 0 || Ki < 0 || Kd < 0) return; + if (Ki == 0) outputSum = 0; pmode = pMode; dmode = dMode; iawmode = iAwMode; dispKp = Kp; dispKi = Ki; dispKd = Kd; float SampleTimeSec = (float)sampleTimeUs / 1000000; From 7e1d6675df5483d193b1f3d4f746e4efa6fb4887 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 6 Jan 2023 18:14:44 -0500 Subject: [PATCH 087/101] Update * Added default constructor --- LICENSE | 2 +- .../PID_AVR_Basic_Interrupt_TIMER.ino | 4 ++-- .../PID_AVR_Basic_Software_TIMER.ino | 4 ++-- examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino | 2 +- examples/PID_Basic/PID_Basic.ino | 2 +- examples/PID_Controller_Options/PID_Controller_Options.ino | 2 +- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 2 +- src/QuickPID.h | 3 +++ 10 files changed, 14 insertions(+), 11 deletions(-) diff --git a/LICENSE b/LICENSE index 5f44edf..3a20a5b 100644 --- a/LICENSE +++ b/LICENSE @@ -1,4 +1,4 @@ -Copyright 2022 David Lloyd +Copyright 2023 David Lloyd Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in diff --git a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino index 5577371..2b06d05 100644 --- a/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino +++ b/examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino @@ -2,8 +2,8 @@ PID AVR Basic Interrupt TIMER Example Reading analog input 0 to control analog PWM output 3 ********************************************************/ -#include "TimerOne.h" // https://github.com/PaulStoffregen/TimerOne -#include "QuickPID.h" +#include // https://github.com/PaulStoffregen/TimerOne +#include #define PIN_INPUT 0 #define PIN_OUTPUT 3 diff --git a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino index e64c3d5..89ce0a1 100644 --- a/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino +++ b/examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino @@ -2,8 +2,8 @@ PID AVR Basic Software TIMER Example Reading analog input 0 to control analog PWM output 3 ********************************************************/ -#include "Ticker.h" // https://github.com/sstaub/Ticker -#include "QuickPID.h" +#include // https://github.com/sstaub/Ticker +#include void runPid(); #define PIN_INPUT 0 diff --git a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino index 4908fcb..018c647 100644 --- a/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino +++ b/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino @@ -7,7 +7,7 @@ setpoint and more agressive tuning parameters when we're farther away. ******************************************************************************/ -#include "QuickPID.h" +#include #define PIN_INPUT 0 #define PIN_OUTPUT 3 diff --git a/examples/PID_Basic/PID_Basic.ino b/examples/PID_Basic/PID_Basic.ino index 73b2624..5afee18 100644 --- a/examples/PID_Basic/PID_Basic.ino +++ b/examples/PID_Basic/PID_Basic.ino @@ -3,7 +3,7 @@ Reading analog input 0 to control analog PWM output 3 ********************************************************/ -#include "QuickPID.h" +#include #define PIN_INPUT 0 #define PIN_OUTPUT 3 diff --git a/examples/PID_Controller_Options/PID_Controller_Options.ino b/examples/PID_Controller_Options/PID_Controller_Options.ino index f413dab..abbf9a8 100644 --- a/examples/PID_Controller_Options/PID_Controller_Options.ino +++ b/examples/PID_Controller_Options/PID_Controller_Options.ino @@ -3,7 +3,7 @@ Documentation (GitHub): https://github.com/Dlloydev/QuickPID **************************************************************************************/ -#include "QuickPID.h" +#include const byte inputPin = 0; const byte outputPin = 3; diff --git a/library.json b/library.json index de004bb..02c37c6 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.1.2", + "version": "3.1.3", "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": diff --git a/library.properties b/library.properties index f75d60f..a1ee3e8 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.1.2 +version=3.1.3 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 3dfdcca..536dd75 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.1.2 + QuickPID Library for Arduino - Version 3.1.3 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ diff --git a/src/QuickPID.h b/src/QuickPID.h index 394b7f3..35a786f 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -14,6 +14,9 @@ class QuickPID { // commonly used functions ************************************************************************************ + // Default constructor + QuickPID() {} + // Constructor. Links the PID to Input, Output, Setpoint, initial tuning parameters and control modes. QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, pMode pMode, dMode dMode, iAwMode iAwMode, Action Action); From 6adfc2c6a8245c36286bebe59dacd11971daaa88 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 6 Jan 2023 18:44:26 -0500 Subject: [PATCH 088/101] Update * Added default constructor --- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 5 ++++- src/QuickPID.h | 2 +- 4 files changed, 7 insertions(+), 4 deletions(-) diff --git a/library.json b/library.json index 02c37c6..847479d 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.1.3", + "version": "3.1.4", "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": diff --git a/library.properties b/library.properties index a1ee3e8..c80cfd4 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.1.3 +version=3.1.4 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 536dd75..f6841b3 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.1.3 + QuickPID Library for Arduino - Version 3.1.4 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ @@ -12,10 +12,13 @@ #include "QuickPID.h" +QuickPID::QuickPID() {} + /* Constructor ******************************************************************** The parameters specified here are those for for which we can't set up reliable defaults, so we need to have the user set them. **********************************************************************************/ + QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, float Kp = 0, float Ki = 0, float Kd = 0, pMode pMode = pMode::pOnError, diff --git a/src/QuickPID.h b/src/QuickPID.h index 35a786f..524194d 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -15,7 +15,7 @@ class QuickPID { // commonly used functions ************************************************************************************ // Default constructor - QuickPID() {} + QuickPID(); // Constructor. Links the PID to Input, Output, Setpoint, initial tuning parameters and control modes. QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, From 6074fa4c91e8f0f555337353bcfbe0f8b72e2f5f Mon Sep 17 00:00:00 2001 From: David R Forrest Date: Tue, 21 Mar 2023 10:04:07 -0400 Subject: [PATCH 089/101] Expose outputSum for monitoring and userspace anti-windup --- src/QuickPID.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/QuickPID.h b/src/QuickPID.h index 524194d..fa55a96 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -80,6 +80,7 @@ class QuickPID { uint8_t GetDmode(); // dOnError (0), dOnMeas (1) uint8_t GetAwMode(); // iAwCondition (0, iAwClamp (1), iAwOff (2) + float outputSum; // Internal integral sum private: void Initialize(); @@ -106,7 +107,7 @@ class QuickPID { iAwMode iawmode = iAwMode::iAwCondition; uint32_t sampleTimeUs, lastTime; - float outputSum, outMin, outMax, error, lastError, lastInput; + float outMin, outMax, error, lastError, lastInput; }; // class QuickPID #endif // QuickPID.h From 3d958e4a71847d7aed6544b938d63d7ea71930a0 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 22 Mar 2023 14:58:16 -0400 Subject: [PATCH 090/101] Resolves #66 --- src/QuickPID.cpp | 18 ++++++++++++++++++ src/QuickPID.h | 6 ++++++ 2 files changed, 24 insertions(+) diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index f6841b3..0996798 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -182,6 +182,12 @@ void QuickPID::SetMode(Control Mode) { } mode = Mode; } +void QuickPID::SetMode(uint8_t Mode) { + if (mode == Control::manual && Mode != 0) { // just went from manual to automatic or timer + QuickPID::Initialize(); + } + mode = (Control)Mode; +} /* Initialize()**************************************************************** Does all the things that need to happen to ensure a bumpless transfer @@ -200,6 +206,9 @@ void QuickPID::Initialize() { void QuickPID::SetControllerDirection(Action Action) { action = Action; } +void QuickPID::SetControllerDirection(uint8_t Direction) { + action = (Action)Direction; +} /* SetProportionalMode(.)***************************************************** Sets the computation method for the proportional term, to compute based @@ -208,6 +217,9 @@ void QuickPID::SetControllerDirection(Action Action) { void QuickPID::SetProportionalMode(pMode pMode) { pmode = pMode; } +void QuickPID::SetProportionalMode(uint8_t Pmode) { + pmode = (pMode)Pmode; +} /* SetDerivativeMode(.)******************************************************* Sets the computation method for the derivative term, to compute based @@ -216,6 +228,9 @@ void QuickPID::SetProportionalMode(pMode pMode) { void QuickPID::SetDerivativeMode(dMode dMode) { dmode = dMode; } +void QuickPID::SetDerivativeMode(uint8_t Dmode) { + dmode = (dMode)Dmode; +} /* SetAntiWindupMode(.)******************************************************* Sets the integral anti-windup mode to one of iAwClamp, which clamps @@ -227,6 +242,9 @@ void QuickPID::SetDerivativeMode(dMode dMode) { void QuickPID::SetAntiWindupMode(iAwMode iAwMode) { iawmode = iAwMode; } +void QuickPID::SetAntiWindupMode(uint8_t IawMode) { + iawmode = (iAwMode)IawMode; +} /* Status Functions************************************************************ These functions query the internal state of the PID. diff --git a/src/QuickPID.h b/src/QuickPID.h index fa55a96..0e87d64 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -30,6 +30,7 @@ class QuickPID { // Sets PID mode to manual (0), automatic (1) or timer (2). void SetMode(Control Mode); + void SetMode(uint8_t Mode); // Performs the PID calculation. It should be called every time loop() cycles ON/OFF and calculation frequency // can be set using SetMode and SetSampleTime respectively. @@ -50,6 +51,7 @@ class QuickPID { // Sets the controller direction or action. Direct means the output will increase when the error is positive. // Reverse means the output will decrease when the error is positive. void SetControllerDirection(Action Action); + void SetControllerDirection(uint8_t Direction); // Sets the sample time in microseconds with which each PID calculation is performed. Default is 100000 µs. void SetSampleTimeUs(uint32_t NewSampleTimeUs); @@ -57,15 +59,18 @@ class QuickPID { // Sets the computation method for the proportional term, to compute based either on error (default), // on measurement, or the average of both. void SetProportionalMode(pMode pMode); + void SetProportionalMode(uint8_t Pmode); // Sets the computation method for the derivative term, to compute based either on error or measurement (default). void SetDerivativeMode(dMode dMode); + void SetDerivativeMode(uint8_t Dmode); // Sets the integral anti-windup mode to one of iAwClamp, which clamps the output after // adding integral and proportional (on measurement) terms, or iAwCondition (default), which // provides some integral correction, prevents deep saturation and reduces overshoot. // Option iAwOff disables anti-windup altogether. void SetAntiWindupMode(iAwMode iAwMode); + void SetAntiWindupMode(uint8_t IawMode); // PID Query functions **************************************************************************************** float GetKp(); // proportional gain @@ -81,6 +86,7 @@ class QuickPID { uint8_t GetAwMode(); // iAwCondition (0, iAwClamp (1), iAwOff (2) float outputSum; // Internal integral sum + private: void Initialize(); From bb0b35e0adf068a61fa516d400477547a5eb769d Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 22 Mar 2023 15:47:01 -0400 Subject: [PATCH 091/101] Resolves #67 and adds new toggle mode Controller mode can now be toggled to/from manual/automatic. myPID.SetMode(myPID.Control::toggle); myPID.SetMode(myPID.Control::toggle); --- src/QuickPID.cpp | 10 +++++++--- src/QuickPID.h | 19 ++++++++++--------- 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 0996798..f15829f 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -177,16 +177,20 @@ void QuickPID::SetOutputLimits(float Min, float Max) { controller is automatically initialized. ******************************************************************************/ void QuickPID::SetMode(Control Mode) { - if (mode == Control::manual && Mode != Control::manual) { // just went from manual to automatic or timer + if (mode == Control::manual && Mode != Control::manual) { // just went from manual to automatic, timer or toggle QuickPID::Initialize(); } - mode = Mode; + if (Mode == Control::toggle) { + mode = (mode == Control::manual) ? Control::automatic : Control::manual; + } else mode = Mode; } void QuickPID::SetMode(uint8_t Mode) { if (mode == Control::manual && Mode != 0) { // just went from manual to automatic or timer QuickPID::Initialize(); } - mode = (Control)Mode; + if (Mode == 3) { // toggle + mode = (mode == Control::manual) ? Control::automatic : Control::manual; + } else mode = (Control)Mode; } /* Initialize()**************************************************************** diff --git a/src/QuickPID.h b/src/QuickPID.h index 0e87d64..c84f847 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -6,11 +6,11 @@ class QuickPID { public: - enum class Control : uint8_t {manual, automatic, timer}; // controller mode - enum class Action : uint8_t {direct, reverse}; // controller action - enum class pMode : uint8_t {pOnError, pOnMeas, pOnErrorMeas}; // proportional mode - enum class dMode : uint8_t {dOnError, dOnMeas}; // derivative mode - enum class iAwMode : uint8_t {iAwCondition, iAwClamp, iAwOff}; // integral anti-windup mode + enum class Control : uint8_t {manual, automatic, timer, toggle}; // controller mode + enum class Action : uint8_t {direct, reverse}; // controller action + enum class pMode : uint8_t {pOnError, pOnMeas, pOnErrorMeas}; // proportional mode + enum class dMode : uint8_t {dOnError, dOnMeas}; // derivative mode + enum class iAwMode : uint8_t {iAwCondition, iAwClamp, iAwOff}; // integral anti-windup mode // commonly used functions ************************************************************************************ @@ -28,7 +28,7 @@ class QuickPID { // Simplified constructor which uses defaults for remaining parameters. QuickPID(float *Input, float *Output, float *Setpoint); - // Sets PID mode to manual (0), automatic (1) or timer (2). + // Sets PID mode to manual (0), automatic (1), timer (2) or toggle manual/automatic (3). void SetMode(Control Mode); void SetMode(uint8_t Mode); @@ -72,6 +72,9 @@ class QuickPID { void SetAntiWindupMode(iAwMode iAwMode); void SetAntiWindupMode(uint8_t IawMode); + // Ensure a bumpless transfer from manual to automatic mode + void Initialize(); + // PID Query functions **************************************************************************************** float GetKp(); // proportional gain float GetKi(); // integral gain @@ -79,7 +82,7 @@ class QuickPID { float GetPterm(); // proportional component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output - uint8_t GetMode(); // manual (0), automatic (1) or timer (2) + uint8_t GetMode(); // manual (0), automatic (1), timer (2) or toggle manual/automatic (3) uint8_t GetDirection(); // direct (0), reverse (1) uint8_t GetPmode(); // pOnError (0), pOnMeas (1), pOnErrorMeas (2) uint8_t GetDmode(); // dOnError (0), dOnMeas (1) @@ -89,8 +92,6 @@ class QuickPID { private: - void Initialize(); - float dispKp = 0; // for defaults and display float dispKi = 0; float dispKd = 0; From 02b57096717a3d15757b0453cdc47ba9b577c54c Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Wed, 22 Mar 2023 22:46:44 -0400 Subject: [PATCH 092/101] Update Resolves #65, #66 and #67 Thanks @drf5n Also adds a new "toggle" controller mode that toggles between manual and automatic mode. --- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/library.json b/library.json index 847479d..f07a256 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.1.4", + "version": "3.1.5", "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": diff --git a/library.properties b/library.properties index c80cfd4..1d23fe4 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.1.4 +version=3.1.5 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index f15829f..373e720 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.1.4 + QuickPID Library for Arduino - Version 3.1.5 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ From 736b2b8c8b1bf1b36273cfadd74449bb701c2535 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 25 Mar 2023 10:25:01 -0400 Subject: [PATCH 093/101] Resolves #68 --- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 7 +++++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/library.json b/library.json index f07a256..ca1ae46 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.1.5", + "version": "3.1.6", "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": diff --git a/library.properties b/library.properties index 1d23fe4..297c268 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.1.5 +version=3.1.6 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 373e720..94c7fbf 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.1.5 + QuickPID Library for Arduino - Version 3.1.6 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ @@ -55,7 +55,10 @@ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, Simplified constructor which uses defaults for remaining parameters. **********************************************************************************/ QuickPID::QuickPID(float* Input, float* Output, float* Setpoint) - : QuickPID::QuickPID(Input, Output, Setpoint, dispKp, dispKi, dispKd, + : QuickPID::QuickPID(Input, Output, Setpoint, + dispKp = 0, + dispKi = 0, + dispKd = 0, pmode = pMode::pOnError, dmode = dMode::dOnMeas, iawmode = iAwMode::iAwCondition, From 97c5a1b83815075aef9c0efcd06985d5ff5ebfd8 Mon Sep 17 00:00:00 2001 From: Tarala Trilokesh <52102891+trilokeshtarala@users.noreply.github.com> Date: Sun, 9 Apr 2023 15:51:24 +0530 Subject: [PATCH 094/101] Update QuickPID.h --- src/QuickPID.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/QuickPID.h b/src/QuickPID.h index c84f847..38d1419 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -1,4 +1,5 @@ #pragma once +#include #ifndef QuickPID_h #define QuickPID_h From 5024ef5fef83c31cd25e4b9a9fcbe4a44319d5ea Mon Sep 17 00:00:00 2001 From: Tarala Trilokesh <52102891+trilokeshtarala@users.noreply.github.com> Date: Sun, 9 Apr 2023 21:46:02 +0530 Subject: [PATCH 095/101] added CMakeLists for comapatible with esp-idf --- CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..83f0c13 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,6 @@ +idf_component_register( + SRC_DIRS "src" + INCLUDE_DIRS "src" + REQUIRES arduino + PRIV_REQUIRES driver soc +) From c1ed035a7c6df961f4129541a9dee208531e10e5 Mon Sep 17 00:00:00 2001 From: Tarala Trilokesh <52102891+trilokeshtarala@users.noreply.github.com> Date: Sun, 9 Apr 2023 21:58:47 +0530 Subject: [PATCH 096/101] Update README.md added info to use this in the esp-idf toolkit to program esp32 --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index b751f47..7d42976 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,8 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) [![PlatformIO Registry](https://badges.registry.platformio.org/packages/dlloydev/library/QuickPID.svg)](https://registry.platformio.org/packages/libraries/dlloydev/QuickPID) - +``` +you can use this library in esp-idf tool to program esp32 by cloning this repo into your components folder. +then clean the build and rebuild. +``` QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library except for using a more advanced anti-windup mode. Integral anti-windup can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include timer, which allows external timer or ISR timing control. ### Features From efaec9ecde28a3bc856ef49a7a81da73836b8b16 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 9 Apr 2023 13:28:44 -0400 Subject: [PATCH 097/101] Update README.md --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 7d42976..8c2a796 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) [![PlatformIO Registry](https://badges.registry.platformio.org/packages/dlloydev/library/QuickPID.svg)](https://registry.platformio.org/packages/libraries/dlloydev/QuickPID) +QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library except for using a more advanced anti-windup mode. Integral anti-windup can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include timer, which allows external timer or ISR timing control. + ``` -you can use this library in esp-idf tool to program esp32 by cloning this repo into your components folder. -then clean the build and rebuild. +Note: You can use this library in esp-idf tool to program esp32 by cloning this repo into your components folder, then clean the build and rebuild. ``` -QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library except for using a more advanced anti-windup mode. Integral anti-windup can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include timer, which allows external timer or ISR timing control. ### Features From c89c67f63db6dfeb56c5c0a90eb3f58312f874f3 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 9 Apr 2023 13:34:50 -0400 Subject: [PATCH 098/101] Update README.md --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 8c2a796..9036a9a 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,8 @@ QuickPID is an updated implementation of the Arduino PID library with additional features for PID control. By default, this implementation closely follows the method of processing the p,i,d terms as in the PID_v1 library except for using a more advanced anti-windup mode. Integral anti-windup can be based on conditionally using PI terms to provide some integral correction, prevent deep saturation and reduce overshoot. Anti-windup can also be based on clamping only, or it can be turned completely off. Also, the proportional term can be based on error, measurement, or both. The derivative term can be based on error or measurement. PID controller modes include timer, which allows external timer or ISR timing control. ``` -Note: You can use this library in esp-idf tool to program esp32 by cloning this repo into your components folder, then clean the build and rebuild. +Note: You can use this library in esp-idf tool to program esp32 by cloning +this repo into your components folder, then clean the build and rebuild. ``` ### Features From f8e912fa5e827645036219f6cfe9050f7507c892 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Mon, 10 Apr 2023 10:16:18 -0400 Subject: [PATCH 099/101] Update --- library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/library.json b/library.json index ca1ae46..2625072 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.1.6", + "version": "3.1.7", "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": diff --git a/library.properties b/library.properties index 297c268..3ca8921 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.1.6 +version=3.1.7 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index 94c7fbf..e77c6ad 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.1.6 + QuickPID Library for Arduino - Version 3.1.7 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ From 96555896cb6a4e01acab0aaf0ef92aa59cb1e32e Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Fri, 5 May 2023 18:04:49 -0400 Subject: [PATCH 100/101] Update Add Reset() function to clears `pTerm`, `iTerm`, `dTerm` and `outputSum` values. --- README.md | 8 ++++++++ library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 14 +++++++++++++- src/QuickPID.h | 5 +++-- 5 files changed, 26 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 9036a9a..ba8c4ec 100644 --- a/README.md +++ b/README.md @@ -65,6 +65,14 @@ void QuickPID::Initialize(); Does all the things that need to happen to ensure a bump-less transfer from manual to automatic mode. +#### Reset + +```c++ +void QuickPID::Reset(); +``` + +Clears `pTerm`, `iTerm`, `dTerm` and `outputSum` values. + #### PID Query Functions These functions query the internal state of the PID. diff --git a/library.json b/library.json index 2625072..3a8ff31 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.1.7", + "version": "3.1.8", "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": diff --git a/library.properties b/library.properties index 3ca8921..cb41dbc 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.1.7 +version=3.1.8 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index e77c6ad..fd1792d 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.1.7 + QuickPID Library for Arduino - Version 3.1.8 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ @@ -253,6 +253,15 @@ void QuickPID::SetAntiWindupMode(uint8_t IawMode) { iawmode = (iAwMode)IawMode; } +void QuickPID::Reset() { + lastTime = micros() - sampleTimeUs; + lastInput = 0; + outputSum = 0; + pTerm = 0; + iTerm = 0; + dTerm = 0; +} + /* Status Functions************************************************************ These functions query the internal state of the PID. ******************************************************************************/ @@ -274,6 +283,9 @@ float QuickPID::GetIterm() { float QuickPID::GetDterm() { return dTerm; } +float QuickPID::GetOutputSum() { + return outputSum; +} uint8_t QuickPID::GetMode() { return static_cast(mode); } diff --git a/src/QuickPID.h b/src/QuickPID.h index 38d1419..5f327ac 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -73,8 +73,8 @@ class QuickPID { void SetAntiWindupMode(iAwMode iAwMode); void SetAntiWindupMode(uint8_t IawMode); - // Ensure a bumpless transfer from manual to automatic mode - void Initialize(); + void Initialize(); // Ensure a bumpless transfer from manual to automatic mode + void Reset(); // Clears pTerm, iTerm, dTerm and outputSum values // PID Query functions **************************************************************************************** float GetKp(); // proportional gain @@ -83,6 +83,7 @@ class QuickPID { float GetPterm(); // proportional component of output float GetIterm(); // integral component of output float GetDterm(); // derivative component of output + float GetOutputSum(); // summation of all pid term components uint8_t GetMode(); // manual (0), automatic (1), timer (2) or toggle manual/automatic (3) uint8_t GetDirection(); // direct (0), reverse (1) uint8_t GetPmode(); // pOnError (0), pOnMeas (1), pOnErrorMeas (2) From c3f64fa24bbd6c0ff00ae9ffa08e0bba0890dba0 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 10 Jun 2023 15:40:52 -0400 Subject: [PATCH 101/101] Update Adds the SetOutputSum(sum); function --- README.md | 1 + library.json | 2 +- library.properties | 2 +- src/QuickPID.cpp | 7 ++++++- src/QuickPID.h | 3 +++ 5 files changed, 12 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index ba8c4ec..c913885 100644 --- a/README.md +++ b/README.md @@ -106,6 +106,7 @@ void SetSampleTimeUs(uint32_t NewSampleTimeUs); // Set PID compute sample time, void SetProportionalMode(pMode pMode); // Set pTerm based on error (default), measurement, or both void SetDerivativeMode(dMode dMode); // Set the dTerm, based error or measurement (default). void SetAntiWindupMode(iAwMode iAwMode); // Set iTerm anti-windup to iAwCondition, iAwClamp or iAwOff +void SetOutputSum(float sum); // sets the output summation value ``` ### Autotuner diff --git a/library.json b/library.json index 3a8ff31..acf6266 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "QuickPID", - "version": "3.1.8", + "version": "3.1.9", "description": "A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.", "keywords": "PID, controller, signal, autotune, tuner, stune", "repository": diff --git a/library.properties b/library.properties index cb41dbc..f264393 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=QuickPID -version=3.1.8 +version=3.1.9 author=David Lloyd maintainer=David Lloyd sentence=A fast PID controller with multiple options. Various Integral anti-windup, Proportional and Derivative control modes. diff --git a/src/QuickPID.cpp b/src/QuickPID.cpp index fd1792d..3753611 100644 --- a/src/QuickPID.cpp +++ b/src/QuickPID.cpp @@ -1,5 +1,5 @@ /********************************************************************************** - QuickPID Library for Arduino - Version 3.1.8 + QuickPID Library for Arduino - Version 3.1.9 by dlloydev https://github.com/Dlloydev/QuickPID Based on the Arduino PID_v1 Library. Licensed under the MIT License. **********************************************************************************/ @@ -262,6 +262,11 @@ void QuickPID::Reset() { dTerm = 0; } +// sets the output summation value +void QuickPID::SetOutputSum(float sum) { + outputSum = sum; +} + /* Status Functions************************************************************ These functions query the internal state of the PID. ******************************************************************************/ diff --git a/src/QuickPID.h b/src/QuickPID.h index 5f327ac..66352e9 100644 --- a/src/QuickPID.h +++ b/src/QuickPID.h @@ -73,6 +73,9 @@ class QuickPID { void SetAntiWindupMode(iAwMode iAwMode); void SetAntiWindupMode(uint8_t IawMode); + // sets the output summation value + void SetOutputSum(float sum); + void Initialize(); // Ensure a bumpless transfer from manual to automatic mode void Reset(); // Clears pTerm, iTerm, dTerm and outputSum values