Skip to content

Commit

Permalink
Update
Browse files Browse the repository at this point in the history
    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
  • Loading branch information
Dlloydev committed May 28, 2021
1 parent daa7cb6 commit 2078421
Show file tree
Hide file tree
Showing 14 changed files with 141 additions and 124 deletions.
39 changes: 22 additions & 17 deletions README.md
Original file line number Diff line number Diff line change
@@ -1,30 +1,35 @@
# QuickPID ![arduino-library-badge](https://github.com/camo/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

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] 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)

Expand All @@ -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.
Expand All @@ -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.
Expand All @@ -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

Expand Down Expand Up @@ -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)
Expand Down
13 changes: 7 additions & 6 deletions examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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;

Expand Down
13 changes: 7 additions & 6 deletions examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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;

Expand Down
13 changes: 8 additions & 5 deletions examples/AutoTune_Interrupt_TIMER/AutoTune_Interrupt_TIMER.ino
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
}

Expand All @@ -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:
Expand Down
13 changes: 8 additions & 5 deletions examples/AutoTune_Software_TIMER/AutoTune_Software_TIMER.ino
Original file line number Diff line number Diff line change
Expand Up @@ -12,20 +12,22 @@ 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;

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();
Expand All @@ -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);
}

Expand All @@ -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:
Expand Down
12 changes: 7 additions & 5 deletions examples/QuickPID_AdaptiveTunings/QuickPID_AdaptiveTunings.ino
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand All @@ -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);
Expand Down
34 changes: 34 additions & 0 deletions examples/QuickPID_PDonM/QuickPID_PDonM.ino
Original file line number Diff line number Diff line change
@@ -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);
}
33 changes: 0 additions & 33 deletions examples/QuickPID_PonM/QuickPID_PonM.ino

This file was deleted.

28 changes: 14 additions & 14 deletions examples/QuickPID_RelayOutput/QuickPID_RelayOutput.ino
Original file line number Diff line number Diff line change
@@ -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"

Expand All @@ -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;
Expand Down
Loading

0 comments on commit 2078421

Please sign in to comment.