From 576d7143dcb82eed3a456cbf2da6b3cfe825aecf Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sun, 12 Dec 2021 11:08:53 -0500 Subject: [PATCH] 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