From a55f3413be7a289620d37d491fdb13706b495e90 Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 30 Jan 2021 14:11:31 -0500 Subject: [PATCH] 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