├── library.properties ├── library.json ├── examples ├── PID_Basic │ └── PID_Basic.ino ├── PID_AVR_Basic_Software_TIMER │ └── PID_AVR_Basic_Software_TIMER.ino ├── PID_AVR_Basic_Interrupt_TIMER │ └── PID_AVR_Basic_Interrupt_TIMER.ino ├── PID_POn_DOn_Error_Measurement │ └── PID_POn_DOn_Error_Measurement.ino ├── PID_AdaptiveTunings │ └── PID_AdaptiveTunings.ino ├── PID_RelayOutput │ └── PID_RelayOutput.ino ├── AutoTune_Filter_DIRECT │ └── AutoTune_Filter_DIRECT.ino ├── AutoTune_Filter_REVERSE │ └── AutoTune_Filter_REVERSE.ino ├── AutoTune_AVR_Software_TIMER │ └── AutoTune_AVR_Software_TIMER.ino └── AutoTune_AVR_Interrupt_TIMER │ └── AutoTune_AVR_Interrupt_TIMER.ino ├── LICENSE ├── keywords.txt ├── src ├── analogWrite.h ├── QuickPID.h ├── analogWrite.cpp └── QuickPID.cpp └── README.md /library.properties: -------------------------------------------------------------------------------- 1 | name=QuickPID 2 | version=2.4.6 3 | author=David Lloyd 4 | maintainer=David Lloyd 5 | sentence=A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. 6 | paragraph=Also includes analogWrite compatibility for ESP32 and ESP32-S2. 7 | category=Signal Input/Output 8 | url=https://github.com/Dlloydev/QuickPID 9 | architectures=* 10 | -------------------------------------------------------------------------------- /library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "QuickPID", 3 | "version": "2.4.6", 4 | "description": "A fast PID controller with AutoTune dynamic object, 10 tuning rules, Integral anti-windup, TIMER Mode and mixing of Proportional and Derivative on Error to Measurement. Also includes analogWrite compatibility for ESP32 and ESP32-S2.", 5 | "keywords": "PID, controller, signal", 6 | "repository": 7 | { 8 | "type": "git", 9 | "url": "https://github.com/Dlloydev/QuickPID" 10 | }, 11 | "authors": 12 | [ 13 | { 14 | "name": "David Lloyd", 15 | "email": "dlloydev@testcor.ca", 16 | "url": "https://github.com/Dlloydev/QuickPID", 17 | "maintainer": true 18 | } 19 | ], 20 | "license": "MIT", 21 | "homepage": "https://github.com/Dlloydev/QuickPID", 22 | "dependencies": { 23 | "QuickPID": "~2.4.6" 24 | }, 25 | "frameworks": "*", 26 | "platforms": "*" 27 | } 28 | -------------------------------------------------------------------------------- /examples/PID_Basic/PID_Basic.ino: -------------------------------------------------------------------------------- 1 | /******************************************************** 2 | PID Basic Example 3 | Reading analog input 0 to control analog PWM output 3 4 | ********************************************************/ 5 | 6 | #include "QuickPID.h" 7 | 8 | #define PIN_INPUT 0 9 | #define PIN_OUTPUT 3 10 | 11 | //Define Variables we'll be connecting to 12 | float Setpoint, Input, Output; 13 | 14 | //Specify the links and initial tuning parameters 15 | float Kp = 2, Ki = 5, Kd = 1; 16 | 17 | QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); 18 | 19 | void setup() 20 | { 21 | //initialize the variables we're linked to 22 | Input = myQuickPID.analogReadFast(PIN_INPUT); 23 | Setpoint = 100; 24 | 25 | //turn the PID on 26 | myQuickPID.SetMode(QuickPID::AUTOMATIC); 27 | } 28 | 29 | void loop() 30 | { 31 | Input = myQuickPID.analogReadFast(PIN_INPUT); 32 | myQuickPID.Compute(); 33 | analogWrite(PIN_OUTPUT, Output); 34 | } 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2021 David Lloyd 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of 4 | this software and associated documentation files (the "Software"), to deal in 5 | the Software without restriction, including without limitation the rights to 6 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 7 | the Software, and to permit persons to whom the Software is furnished to do so, 8 | subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 15 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 16 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 17 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 18 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ########################################## 2 | # Syntax Coloring Map For QuickPID Library 3 | ########################################## 4 | 5 | ########################################## 6 | # Datatypes (KEYWORD1) 7 | ########################################## 8 | 9 | QuickPID KEYWORD1 10 | myQuickPID KEYWORD1 11 | 12 | ########################################## 13 | # Methods and Functions (KEYWORD2) 14 | ########################################## 15 | 16 | SetMode KEYWORD2 17 | Compute KEYWORD2 18 | AutoTune KEYWORD2 19 | SetOutputLimits KEYWORD2 20 | SetTunings KEYWORD2 21 | SetControllerDirection KEYWORD2 22 | SetSampleTimeUs KEYWORD2 23 | GetKp KEYWORD2 24 | GetKi KEYWORD2 25 | GetKd KEYWORD2 26 | GetPterm KEYWORD2 27 | GetIterm KEYWORD2 28 | GetDterm KEYWORD2 29 | GetMode KEYWORD2 30 | GetDirection KEYWORD2 31 | analogReadFast KEYWORD2 32 | analogReadAvg KEYWORD2 33 | analogWrite KEYWORD2 34 | analogWriteFrequency KEYWORD2 35 | analogWriteResolution KEYWORD2 36 | 37 | ########################################## 38 | # Constants (LITERAL1) 39 | ########################################## 40 | 41 | AUTOMATIC LITERAL1 42 | MANUAL LITERAL1 43 | TIMER LITERAL1 44 | DIRECT LITERAL1 45 | REVERSE LITERAL1 46 | mode_t LITERAL1 47 | direction_t LITERAL1 48 | analog_write_channel_t LITERAL1 49 | -------------------------------------------------------------------------------- /examples/PID_AVR_Basic_Software_TIMER/PID_AVR_Basic_Software_TIMER.ino: -------------------------------------------------------------------------------- 1 | /******************************************************** 2 | PID AVR Basic Software TIMER Example 3 | Reading analog input 0 to control analog PWM output 3 4 | ********************************************************/ 5 | #include "Ticker.h" // https://github.com/sstaub/Ticker 6 | #include "QuickPID.h" 7 | void runPid(); 8 | 9 | #define PIN_INPUT 0 10 | #define PIN_OUTPUT 3 11 | const uint32_t sampleTimeUs = 100000; // 100ms 12 | static boolean computeNow = false; 13 | 14 | //Define Variables we'll be connecting to 15 | float Setpoint, Input, Output; 16 | 17 | //Specify the links and initial tuning parameters 18 | float Kp = 2, Ki = 5, Kd = 1; 19 | 20 | Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); 21 | QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); 22 | 23 | void setup() { 24 | timer1.start(); 25 | 26 | //initialize the variables we're linked to 27 | Input = myQuickPID.analogReadFast(PIN_INPUT); 28 | Setpoint = 100; 29 | 30 | //turn the PID on 31 | myQuickPID.SetMode(QuickPID::AUTOMATIC); 32 | } 33 | 34 | void loop() { 35 | timer1.update(); 36 | if (computeNow) { 37 | Input = myQuickPID.analogReadFast(PIN_INPUT); 38 | myQuickPID.Compute(); 39 | analogWrite(PIN_OUTPUT, Output); 40 | computeNow = false; 41 | } 42 | } 43 | 44 | void runPid() { 45 | computeNow = true; 46 | } 47 | -------------------------------------------------------------------------------- /examples/PID_AVR_Basic_Interrupt_TIMER/PID_AVR_Basic_Interrupt_TIMER.ino: -------------------------------------------------------------------------------- 1 | /******************************************************** 2 | PID AVR Basic Interrupt TIMER Example 3 | Reading analog input 0 to control analog PWM output 3 4 | ********************************************************/ 5 | #include "TimerOne.h" // https://github.com/PaulStoffregen/TimerOne 6 | #include "QuickPID.h" 7 | 8 | #define PIN_INPUT 0 9 | #define PIN_OUTPUT 3 10 | const uint32_t sampleTimeUs = 100000; // 100ms 11 | volatile bool computeNow = false; 12 | 13 | //Define Variables we'll be connecting to 14 | float Setpoint, Input, Output; 15 | 16 | //Specify the links and initial tuning parameters 17 | float Kp = 2, Ki = 5, Kd = 1; 18 | 19 | QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, QuickPID::DIRECT); 20 | 21 | void setup() { 22 | Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval 23 | Timer1.attachInterrupt(runPid); // attaches runPid() as a timer overflow interrupt 24 | 25 | //initialize the variables we're linked to 26 | Input = myQuickPID.analogReadFast(PIN_INPUT); 27 | Setpoint = 100; 28 | 29 | //turn the PID on 30 | myQuickPID.SetMode(QuickPID::AUTOMATIC); 31 | } 32 | 33 | void loop() { 34 | if (computeNow) { 35 | Input = myQuickPID.analogReadFast(PIN_INPUT); 36 | myQuickPID.Compute(); 37 | analogWrite(PIN_OUTPUT, Output); 38 | computeNow = false; 39 | } 40 | } 41 | 42 | void runPid() { 43 | computeNow = true; 44 | } 45 | -------------------------------------------------------------------------------- /examples/PID_POn_DOn_Error_Measurement/PID_POn_DOn_Error_Measurement.ino: -------------------------------------------------------------------------------- 1 | /************************************************************************************** 2 | Proportional and Derivative on the ratio of Error to Measurement Example 3 | Increasing the proportional on measurement setting will make the output move more 4 | smoothly when the setpoint is changed. Also, it can eliminate overshoot. 5 | Decreasing the derivative on measurement adds more derivative on error. This reduces 6 | reduce overshoot but may increase output spikes. Adjust to suit your requirements. 7 | **************************************************************************************/ 8 | 9 | #include "QuickPID.h" 10 | 11 | //Define Variables we'll be connecting to 12 | float Setpoint, Input, Output; 13 | float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 14 | float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 15 | 16 | //Specify the links and initial tuning parameters 17 | QuickPID myQuickPID(&Input, &Output, &Setpoint, 2, 5, 1, POn, DOn, QuickPID::DIRECT); 18 | 19 | void setup() 20 | { 21 | //initialize the variables we're linked to 22 | Input = myQuickPID.analogReadFast(0); 23 | Setpoint = 100; 24 | 25 | //turn the PID on 26 | myQuickPID.SetMode(QuickPID::AUTOMATIC); 27 | } 28 | 29 | void loop() 30 | { 31 | Input = myQuickPID.analogReadFast(0); 32 | myQuickPID.Compute(); 33 | analogWrite(3, Output); 34 | } 35 | -------------------------------------------------------------------------------- /src/analogWrite.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _ESP32_ESP32S2_ANALOG_WRITE_ 3 | #define _ESP32_ESP32S2_ANALOG_WRITE_ 4 | 5 | #include 6 | 7 | #if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) 8 | 9 | namespace aw { 10 | 11 | #include "driver/ledc.h" 12 | 13 | #if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) 14 | 15 | #define NUM_OUTPUT_PINS 45 16 | #define DAC1 17 17 | #define DAC2 18 18 | const uint8_t muxSize = 48; 19 | const uint64_t pinMask = 0x27FE00207FFE; //PWM 20 | 21 | #else //ESP32 22 | #define NUM_OUTPUT_PINS 34 23 | #define DAC1 25 24 | #define DAC2 26 25 | const uint8_t muxSize = 40; 26 | const uint64_t pinMask = 0x308EFF034; //PWM 27 | #endif 28 | 29 | typedef struct pinStatus { 30 | int8_t channel; 31 | int8_t pin; 32 | float frequency; 33 | uint8_t resolution; 34 | uint32_t value; 35 | uint32_t phase; 36 | } pinStatus_t; 37 | 38 | float awLedcSetup(uint8_t ch, double frequency, uint8_t bits); 39 | void awDetachPin(uint8_t pin, uint8_t ch); 40 | float awLedcReadFreq(uint8_t ch); 41 | int8_t awGetChannel(int8_t pin); 42 | 43 | } //namespace aw 44 | 45 | float analogWriteFrequency(int8_t pin, float frequency = 980); 46 | int32_t analogWriteResolution(int8_t pin, uint8_t resolution = 8); 47 | float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution, uint32_t phase); 48 | float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution); 49 | float analogWrite(int8_t pin, int32_t value, float frequency); 50 | float analogWrite(int8_t pin, int32_t value); 51 | void setPinsStatusDefaults(int32_t value = 0, float frequency = 980, uint8_t resolution = 8, uint32_t phase = 0); 52 | void printPinsStatus(void); 53 | 54 | #endif //ESP32 or ARDUINO_ARCH_ESP32 55 | #endif //_ESP32_ESP32S2_ANALOG_WRITE_ 56 | -------------------------------------------------------------------------------- /examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino: -------------------------------------------------------------------------------- 1 | /******************************************************** 2 | PID Adaptive Tuning Example 3 | One of the benefits of the PID library is that you can 4 | change the tuning parameters at any time. this can be 5 | helpful if we want the controller to be agressive at some 6 | times, and conservative at others. in the example below 7 | we set the controller to use Conservative Tuning Parameters 8 | when we're near setpoint and more agressive Tuning 9 | Parameters when we're farther away. 10 | ********************************************************/ 11 | 12 | #include "QuickPID.h" 13 | 14 | #define PIN_INPUT 0 15 | #define PIN_OUTPUT 3 16 | 17 | //Define Variables we'll be connecting to 18 | float Setpoint, Input, Output; 19 | 20 | //Define the aggressive and conservative and POn Tuning Parameters 21 | float aggKp = 4, aggKi = 0.2, aggKd = 1; 22 | float consKp = 1, consKi = 0.05, consKd = 0.25; 23 | float aggPOn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0) 24 | float consPOn = 0.0; // proportional on Error to Measurement ratio (0.0-1.0) 25 | float aggDOn = 1.0; // derivative on Error to Measurement ratio (0.0-1.0) 26 | float consDOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0) 27 | 28 | //Specify the links and initial tuning parameters 29 | QuickPID myQuickPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, aggPOn, consDOn, QuickPID::DIRECT); 30 | 31 | void setup() 32 | { 33 | //initialize the variables we're linked to 34 | Input = myQuickPID.analogReadFast(PIN_INPUT); 35 | Setpoint = 100; 36 | 37 | //turn the PID on 38 | myQuickPID.SetMode(QuickPID::AUTOMATIC); 39 | } 40 | 41 | void loop() 42 | { 43 | Input = myQuickPID.analogReadFast(PIN_INPUT); 44 | 45 | float gap = abs(Setpoint - Input); //distance away from setpoint 46 | if (gap < 10) { //we're close to setpoint, use conservative tuning parameters 47 | myQuickPID.SetTunings(consKp, consKi, consKd, consPOn, consDOn); 48 | } else { 49 | //we're far from setpoint, use aggressive tuning parameters 50 | myQuickPID.SetTunings(aggKp, aggKi, aggKd, aggPOn, aggDOn); 51 | } 52 | myQuickPID.Compute(); 53 | analogWrite(PIN_OUTPUT, Output); 54 | } 55 | -------------------------------------------------------------------------------- /examples/PID_RelayOutput/PID_RelayOutput.ino: -------------------------------------------------------------------------------- 1 | /************************************************************* 2 | PID Relay Output Example 3 | Same as basic example, except that this time, the output 4 | is going to a digital pin which (we presume) is controlling 5 | a relay. The pid is designed to Output an analog value, 6 | but the relay can only be On/Off. 7 | 8 | To connect them together we use "time proportioning 9 | control", essentially a really slow version of PWM. 10 | First we decide on a window size (5000mS for example). 11 | We then set the pid to adjust its output between 0 and that 12 | window size. Lastly, we add some logic that translates the 13 | PID output into "Relay On Time" with the remainder of the 14 | window being "Relay Off Time". The minWindow setting is a 15 | floor (minimum time) the relay would be on. 16 | *************************************************************/ 17 | 18 | #include "QuickPID.h" 19 | 20 | #define PIN_INPUT 0 21 | #define RELAY_PIN 6 22 | 23 | //Define Variables we'll be connecting to 24 | float Setpoint, Input, Output; 25 | 26 | //Specify the links and initial tuning parameters 27 | float Kp = 2, Ki = 5, Kd = 1; 28 | float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 29 | float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 30 | 31 | QuickPID myQuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); 32 | 33 | unsigned int WindowSize = 5000; 34 | unsigned int minWindow = 500; 35 | unsigned long windowStartTime; 36 | 37 | void setup() 38 | { 39 | pinMode(RELAY_PIN, OUTPUT); 40 | windowStartTime = millis(); 41 | 42 | //initialize the variables we're linked to 43 | Setpoint = 100; 44 | 45 | //tell the PID to range between 0 and the full window size 46 | myQuickPID.SetOutputLimits(0, WindowSize); 47 | 48 | //turn the PID on 49 | myQuickPID.SetMode(QuickPID::AUTOMATIC); 50 | } 51 | 52 | void loop() 53 | { 54 | Input = analogRead(PIN_INPUT); 55 | 56 | /************************************************ 57 | turn the output pin on/off based on pid output 58 | ************************************************/ 59 | if (millis() - windowStartTime >= WindowSize) 60 | { //time to shift the Relay Window 61 | windowStartTime += WindowSize; 62 | myQuickPID.Compute(); 63 | } 64 | if (((unsigned int)Output > minWindow) && ((unsigned int)Output < (millis() - windowStartTime))) digitalWrite(RELAY_PIN, HIGH); 65 | else digitalWrite(RELAY_PIN, LOW); 66 | } 67 | -------------------------------------------------------------------------------- /examples/AutoTune_Filter_DIRECT/AutoTune_Filter_DIRECT.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | AutoTune Filter DIRECT Example 3 | Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter 4 | ******************************************************************************/ 5 | 6 | #include "QuickPID.h" 7 | 8 | const uint32_t sampleTimeUs = 10000; // 10ms 9 | const byte inputPin = 0; 10 | const byte outputPin = 3; 11 | const int outputMax = 255; 12 | const int outputMin = 0; 13 | 14 | bool printOrPlotter = 0; // on(1) monitor, off(0) plotter 15 | float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 16 | float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 17 | 18 | byte outputStep = 5; 19 | byte hysteresis = 1; 20 | int setpoint = 341; // 1/3 of range for symetrical waveform 21 | int output = 85; // 1/3 of range for symetrical waveform 22 | 23 | float Input, Output, Setpoint; 24 | float Kp = 0, Ki = 0, Kd = 0; 25 | bool pidLoop = false; 26 | 27 | QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); 28 | 29 | void setup() { 30 | Serial.begin(115200); 31 | Serial.println(); 32 | if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { 33 | Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); 34 | while (1); 35 | } 36 | // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki 37 | //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); 38 | _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); 39 | //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); 40 | //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); 41 | //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); 42 | //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); 43 | //_myPID.AutoTune(tuningMethod::AMIGOF_PID); 44 | //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); 45 | //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); 46 | //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); 47 | 48 | _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); 49 | } 50 | 51 | void loop() { 52 | 53 | if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() 54 | { 55 | switch (_myPID.autoTune->autoTuneLoop()) { 56 | case _myPID.autoTune->AUTOTUNE: 57 | Input = avg(_myPID.analogReadFast(inputPin)); 58 | analogWrite(outputPin, Output); 59 | break; 60 | 61 | case _myPID.autoTune->TUNINGS: 62 | _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings 63 | _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID 64 | _myPID.SetSampleTimeUs(sampleTimeUs); 65 | _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID 66 | Setpoint = 500; 67 | break; 68 | 69 | case _myPID.autoTune->CLR: 70 | if (!pidLoop) { 71 | _myPID.clearAutoTune(); // releases memory used by AutoTune object 72 | pidLoop = true; 73 | } 74 | break; 75 | } 76 | } 77 | if (pidLoop) { 78 | if (printOrPlotter == 0) { // plotter 79 | Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); 80 | Serial.print("Input:"); Serial.print(Input); Serial.print(","); 81 | Serial.print("Output:"); Serial.print(Output); Serial.println(","); 82 | } 83 | Input = _myPID.analogReadFast(inputPin); 84 | _myPID.Compute(); 85 | analogWrite(outputPin, Output); 86 | } 87 | } 88 | 89 | float avg(int inputVal) { 90 | static int arrDat[16]; 91 | static int pos; 92 | static long sum; 93 | pos++; 94 | if (pos >= 16) pos = 0; 95 | sum = sum - arrDat[pos] + inputVal; 96 | arrDat[pos] = inputVal; 97 | return (float)sum / 16.0; 98 | } 99 | -------------------------------------------------------------------------------- /examples/AutoTune_Filter_REVERSE/AutoTune_Filter_REVERSE.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | AutoTune Filter REVERSE Example 3 | Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter 4 | ******************************************************************************/ 5 | 6 | #include "QuickPID.h" 7 | 8 | const uint32_t sampleTimeUs = 10000; // 10ms 9 | const byte inputPin = 0; 10 | const byte outputPin = 3; 11 | const int inputMax = 1023; 12 | const int outputMax = 255; 13 | const int outputMin = 0; 14 | 15 | bool printOrPlotter = 0; // on(1) monitor, off(0) plotter 16 | float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 17 | float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 18 | 19 | byte outputStep = 5; 20 | byte hysteresis = 1; 21 | int setpoint = 341; // 1/3 of range for symetrical waveform 22 | int output = 85; // 1/3 of range for symetrical waveform 23 | 24 | float Input, Output, Setpoint; 25 | float Kp = 0, Ki = 0, Kd = 0; 26 | bool pidLoop = false; 27 | 28 | QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::REVERSE); 29 | 30 | void setup() { 31 | Serial.begin(115200); 32 | Serial.println(); 33 | if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { 34 | Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); 35 | while (1); 36 | } 37 | // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki 38 | //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); 39 | _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); 40 | //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); 41 | //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); 42 | //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); 43 | //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); 44 | //_myPID.AutoTune(tuningMethod::AMIGOF_PID); 45 | //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); 46 | //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); 47 | //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); 48 | 49 | _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, inputMax - setpoint, output, QuickPID::REVERSE, printOrPlotter, sampleTimeUs); 50 | } 51 | 52 | void loop() { 53 | 54 | if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() 55 | { 56 | switch (_myPID.autoTune->autoTuneLoop()) { 57 | case _myPID.autoTune->AUTOTUNE: 58 | Input = inputMax - avg(_myPID.analogReadFast(inputPin)); // filtered, reverse acting 59 | analogWrite(outputPin, Output); 60 | break; 61 | 62 | case _myPID.autoTune->TUNINGS: 63 | _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings 64 | _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID 65 | _myPID.SetSampleTimeUs(sampleTimeUs); 66 | _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID 67 | Setpoint = 500; 68 | break; 69 | 70 | case _myPID.autoTune->CLR: 71 | if (!pidLoop) { 72 | _myPID.clearAutoTune(); // releases memory used by AutoTune object 73 | pidLoop = true; 74 | } 75 | break; 76 | } 77 | } 78 | if (pidLoop) { 79 | if (printOrPlotter == 0) { // plotter 80 | Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); 81 | Serial.print("Input:"); Serial.print(Input); Serial.print(","); 82 | Serial.print("Output:"); Serial.print(Output); Serial.println(","); 83 | } 84 | Input = inputMax - _myPID.analogReadFast(inputPin); // reverse acting 85 | _myPID.Compute(); 86 | analogWrite(outputPin, Output); 87 | } 88 | } 89 | 90 | float avg(int inputVal) { 91 | static int arrDat[16]; 92 | static int pos; 93 | static long sum; 94 | pos++; 95 | if (pos >= 16) pos = 0; 96 | sum = sum - arrDat[pos] + inputVal; 97 | arrDat[pos] = inputVal; 98 | return (float)sum / 16.0; 99 | } 100 | -------------------------------------------------------------------------------- /examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | AutoTune AVR Software TIMER Example 3 | Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter 4 | ******************************************************************************/ 5 | #include "Ticker.h" // https://github.com/sstaub/Ticker 6 | #include "QuickPID.h" 7 | void runPid(); 8 | 9 | const uint32_t sampleTimeUs = 10000; // 10ms 10 | const byte inputPin = 0; 11 | const byte outputPin = 3; 12 | const int outputMax = 255; 13 | const int outputMin = 0; 14 | 15 | bool printOrPlotter = 0; // on(1) monitor, off(0) plotter 16 | float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 17 | float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 18 | 19 | byte outputStep = 5; 20 | byte hysteresis = 1; 21 | int setpoint = 341; // 1/3 of range for symetrical waveform 22 | int output = 85; // 1/3 of range for symetrical waveform 23 | 24 | float Input, Output, Setpoint; 25 | float Kp = 0, Ki = 0, Kd = 0; 26 | bool pidLoop = false; 27 | static boolean computeNow = false; 28 | 29 | Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS); 30 | QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); 31 | 32 | void setup() { 33 | timer1.start(); 34 | Serial.begin(115200); 35 | Serial.println(); 36 | if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { 37 | Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); 38 | while (1); 39 | } 40 | // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki 41 | //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); 42 | _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); 43 | //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); 44 | //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); 45 | //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); 46 | //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); 47 | //_myPID.AutoTune(tuningMethod::AMIGOF_PID); 48 | //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); 49 | //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); 50 | //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); 51 | 52 | _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); 53 | } 54 | 55 | void loop() { 56 | timer1.update(); 57 | if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() 58 | { 59 | switch (_myPID.autoTune->autoTuneLoop()) { 60 | case _myPID.autoTune->AUTOTUNE: 61 | Input = avg(_myPID.analogReadFast(inputPin)); 62 | analogWrite(outputPin, Output); 63 | break; 64 | case _myPID.autoTune->TUNINGS: 65 | _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings 66 | _myPID.SetMode(QuickPID::TIMER); // setup PID 67 | _myPID.SetSampleTimeUs(sampleTimeUs); 68 | _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID 69 | Setpoint = 500; 70 | break; 71 | case _myPID.autoTune->CLR: 72 | if (!pidLoop) { 73 | _myPID.clearAutoTune(); // releases memory used by AutoTune object 74 | pidLoop = true; 75 | } 76 | break; 77 | } 78 | } 79 | if (pidLoop) { 80 | if (printOrPlotter == 0) { // plotter 81 | Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); 82 | Serial.print("Input:"); Serial.print(Input); Serial.print(","); 83 | Serial.print("Output:"); Serial.print(Output); Serial.println(","); 84 | } 85 | if (computeNow) { 86 | Input = _myPID.analogReadFast(inputPin); 87 | _myPID.Compute(); 88 | analogWrite(outputPin, Output); 89 | computeNow = false; 90 | } 91 | } 92 | } 93 | 94 | void runPid() { 95 | computeNow = true; 96 | } 97 | 98 | float avg(int inputVal) { 99 | static int arrDat[16]; 100 | static int pos; 101 | static long sum; 102 | pos++; 103 | if (pos >= 16) pos = 0; 104 | sum = sum - arrDat[pos] + inputVal; 105 | arrDat[pos] = inputVal; 106 | return (float)sum / 16.0; 107 | } 108 | -------------------------------------------------------------------------------- /examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | AutoTune AVR Interrupt TIMER Example 3 | Circuit: https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter 4 | ******************************************************************************/ 5 | #include "TimerOne.h" // https://github.com/PaulStoffregen/TimerOne 6 | #include "QuickPID.h" 7 | void runPid(); 8 | 9 | const uint32_t sampleTimeUs = 10000; // 10ms 10 | const byte inputPin = 0; 11 | const byte outputPin = 3; 12 | const int outputMax = 255; 13 | const int outputMin = 0; 14 | 15 | bool printOrPlotter = 0; // on(1) monitor, off(0) plotter 16 | float POn = 1.0; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 17 | float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 18 | 19 | byte outputStep = 5; 20 | byte hysteresis = 1; 21 | int setpoint = 341; // 1/3 of range for symetrical waveform 22 | int output = 85; // 1/3 of range for symetrical waveform 23 | 24 | float Input, Output, Setpoint; 25 | float Kp = 0, Ki = 0, Kd = 0; 26 | bool pidLoop = false; 27 | volatile bool computeNow = false; 28 | 29 | QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT); 30 | 31 | void setup() { 32 | Timer1.initialize(sampleTimeUs); // initialize timer1, and set the time interval 33 | Timer1.attachInterrupt(runPid); // attaches runPid() as a timer overflow interrupt 34 | 35 | Serial.begin(115200); 36 | Serial.println(); 37 | if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { 38 | Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); 39 | while (1); 40 | } 41 | // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki 42 | //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); 43 | _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); 44 | //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); 45 | //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); 46 | //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); 47 | //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); 48 | //_myPID.AutoTune(tuningMethod::AMIGOF_PID); 49 | //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); 50 | //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); 51 | //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID); 52 | 53 | _myPID.autoTune->autoTuneConfig(outputStep, hysteresis, setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs); 54 | } 55 | 56 | void loop() { 57 | if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() 58 | { 59 | switch (_myPID.autoTune->autoTuneLoop()) { 60 | case _myPID.autoTune->AUTOTUNE: 61 | Input = avg(_myPID.analogReadFast(inputPin)); 62 | analogWrite(outputPin, Output); 63 | break; 64 | case _myPID.autoTune->TUNINGS: 65 | _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings 66 | _myPID.SetMode(QuickPID::TIMER); // setup PID 67 | _myPID.SetSampleTimeUs(sampleTimeUs); 68 | _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID 69 | Setpoint = 500; 70 | break; 71 | case _myPID.autoTune->CLR: 72 | if (!pidLoop) { 73 | _myPID.clearAutoTune(); // releases memory used by AutoTune object 74 | pidLoop = true; 75 | } 76 | break; 77 | } 78 | } 79 | if (pidLoop) { 80 | if (printOrPlotter == 0) { // plotter 81 | Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); 82 | Serial.print("Input:"); Serial.print(Input); Serial.print(","); 83 | Serial.print("Output:"); Serial.print(Output); Serial.println(","); 84 | } 85 | if (computeNow) { 86 | Input = _myPID.analogReadFast(inputPin); 87 | _myPID.Compute(); 88 | analogWrite(outputPin, Output); 89 | computeNow = false; 90 | } 91 | } 92 | } 93 | 94 | void runPid() { 95 | computeNow = true; 96 | } 97 | 98 | float avg(int inputVal) { 99 | static int arrDat[16]; 100 | static int pos; 101 | static long sum; 102 | pos++; 103 | if (pos >= 16) pos = 0; 104 | sum = sum - arrDat[pos] + inputVal; 105 | arrDat[pos] = inputVal; 106 | return (float)sum / 16.0; 107 | } 108 | -------------------------------------------------------------------------------- /src/QuickPID.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef QuickPID_h 3 | #define QuickPID_h 4 | 5 | enum class tuningMethod : uint8_t 6 | { 7 | ZIEGLER_NICHOLS_PI, 8 | ZIEGLER_NICHOLS_PID, 9 | TYREUS_LUYBEN_PI, 10 | TYREUS_LUYBEN_PID, 11 | CIANCONE_MARLIN_PI, 12 | CIANCONE_MARLIN_PID, 13 | AMIGOF_PID, 14 | PESSEN_INTEGRAL_PID, 15 | SOME_OVERSHOOT_PID, 16 | NO_OVERSHOOT_PID 17 | }; 18 | 19 | class AutoTunePID { 20 | public: 21 | AutoTunePID(); 22 | AutoTunePID(float *input, float *output, tuningMethod tuningRule); 23 | ~AutoTunePID() {}; 24 | 25 | void reset(); 26 | void autoTuneConfig(const float outputStep, const float hysteresis, const float setpoint, const float output, 27 | const bool dir = false, const bool printOrPlotter = false, uint32_t sampleTimeUs = 10000); 28 | byte autoTuneLoop(); 29 | void setAutoTuneConstants(float* kp, float* ki, float* kd); 30 | enum atStage : byte { AUTOTUNE, WAIT, STABILIZING, COARSE, FINE, TEST, T0, T1, T2, T3L, T3H, CALC, TUNINGS, CLR }; 31 | 32 | private: 33 | 34 | float *_input = nullptr; // Pointers to the Input, Output, and Setpoint variables. This creates a 35 | float *_output = nullptr; // hard link between the variables and the PID, freeing the user from having 36 | // float *mySetpoint = nullptr; // to constantly tell us what these values are. With pointers we'll just know. 37 | 38 | byte _autoTuneStage = 1; 39 | tuningMethod _tuningRule; 40 | bool _direction = false; 41 | bool _printOrPlotter = false; 42 | uint32_t _tLoop, _tLast, _t0, _t1, _t2, _t3; 43 | float _outputStep, _hysteresis, _atSetpoint, _atOutput; 44 | float _Ku, _Tu, _td, _kp, _ki, _kd, _rdAvg, _peakHigh, _peakLow, _inputLast; 45 | 46 | const uint16_t RulesContants[10][3] = 47 | { //ckp, cki, ckd x 1000 48 | { 450, 540, 0 }, // ZIEGLER_NICHOLS_PI 49 | { 600, 176, 75 }, // ZIEGLER_NICHOLS_PID 50 | { 313, 142, 0 }, // TYREUS_LUYBEN_PI 51 | { 454, 206, 72 }, // TYREUS_LUYBEN_PID 52 | { 303, 1212, 0 }, // CIANCONE_MARLIN_PI 53 | { 303, 1333, 37 }, // CIANCONE_MARLIN_PID 54 | { 0, 0, 0 }, // AMIGOF_PID 55 | { 700, 1750, 105 }, // PESSEN_INTEGRAL_PID 56 | { 333, 667, 111 }, // SOME_OVERSHOOT_PID 57 | { 333, 100, 67 } // NO_OVERSHOOT_PID 58 | }; 59 | 60 | }; // class AutoTunePID 61 | 62 | class QuickPID { 63 | 64 | public: 65 | 66 | // controller mode 67 | typedef enum { MANUAL = 0, AUTOMATIC = 1, TIMER = 2 } mode_t; 68 | 69 | // DIRECT: intput increases when the error is positive. REVERSE: intput decreases when the error is positive. 70 | typedef enum { DIRECT = 0, REVERSE = 1 } direction_t; 71 | 72 | // commonly used functions ************************************************************************************ 73 | 74 | // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters. 75 | QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, float POn, float DOn, direction_t ControllerDirection); 76 | 77 | // Overload constructor with proportional ratio. Links the PID to Input, Output, Setpoint and Tuning Parameters. 78 | QuickPID(float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, direction_t ControllerDirection); 79 | 80 | // Sets PID mode to MANUAL (0), AUTOMATIC (1) or TIMER (2). 81 | void SetMode(mode_t Mode); 82 | 83 | // Performs the PID calculation. It should be called every time loop() cycles ON/OFF and calculation frequency 84 | // can be set using SetMode and SetSampleTime respectively. 85 | bool Compute(); 86 | 87 | // Automatically determines and sets the tuning parameters Kp, Ki and Kd. Use this in the setup loop. 88 | void AutoTune(tuningMethod tuningRule); 89 | void clearAutoTune(); 90 | 91 | // Sets and clamps the output to a specific range (0-255 by default). 92 | void SetOutputLimits(int Min, int Max); 93 | 94 | // available but not commonly used functions ****************************************************************** 95 | 96 | // While most users will set the tunings once in the constructor, this function gives the user the option of 97 | // changing tunings during runtime for Adaptive control. 98 | void SetTunings(float Kp, float Ki, float Kd); 99 | 100 | // Overload for specifying proportional ratio. 101 | void SetTunings(float Kp, float Ki, float Kd, float POn, float DOn); 102 | 103 | // Sets the controller Direction or Action. DIRECT means the output will increase when the error is positive. 104 | // REVERSE means the output will decrease when the error is positive. 105 | void SetControllerDirection(direction_t ControllerDirection); 106 | 107 | // Sets the sample time in microseconds with which each PID calculation is performed. Default is 100000 µs. 108 | void SetSampleTimeUs(uint32_t NewSampleTimeUs); 109 | 110 | // PID Query functions *********************************************************************************** 111 | float GetKp(); // proportional gain 112 | float GetKi(); // integral gain 113 | float GetKd(); // derivative gain 114 | float GetPterm(); // proportional component of output 115 | float GetIterm(); // integral component of output 116 | float GetDterm(); // derivative component of output 117 | mode_t GetMode(); // MANUAL (0), AUTOMATIC (1) or TIMER (2) 118 | direction_t GetDirection(); // DIRECT (0) or REVERSE (1) 119 | 120 | int analogReadFast(int ADCpin); 121 | 122 | AutoTunePID *autoTune; 123 | 124 | private: 125 | 126 | void Initialize(); 127 | 128 | float dispKp; // tuning parameters for display purposes. 129 | float dispKi; 130 | float dispKd; 131 | float peTerm; 132 | float pmTerm; 133 | float iTerm; 134 | float deTerm; 135 | float dmTerm; 136 | 137 | float pOn; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 138 | float dOn; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0 139 | float kp; // (P)roportional Tuning Parameter 140 | float ki; // (I)ntegral Tuning Parameter 141 | float kd; // (D)erivative Tuning Parameter 142 | float kpe; // proportional on error amount 143 | float kpm; // proportional on measurement amount 144 | float kde; // derivative on error amount 145 | float kdm; // derivative on measurement amount 146 | 147 | float *myInput; // Pointers to the Input, Output, and Setpoint variables. This creates a 148 | float *myOutput; // hard link between the variables and the PID, freeing the user from having 149 | float *mySetpoint; // to constantly tell us what these values are. With pointers we'll just know. 150 | 151 | mode_t mode = MANUAL; 152 | direction_t controllerDirection; 153 | uint32_t sampleTimeUs, lastTime; 154 | int outMin, outMax, error; 155 | int outputSum; 156 | float lastInput; 157 | bool inAuto; 158 | 159 | }; // class QuickPID 160 | 161 | #if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) 162 | #include "analogWrite.h" 163 | #endif 164 | #endif // QuickPID.h 165 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # QuickPID [![arduino-library-badge](https://www.ardu-badge.com/badge/QuickPID.svg?)](https://www.ardu-badge.com/QuickPID) 2 | 3 | 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. 4 | 5 | #### [QuickPID WiKi ...](https://github.com/Dlloydev/QuickPID/wiki) 6 | 7 | ### Features 8 | 9 | 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). 10 | 11 | #### New feature Summary 12 | 13 | - [x] Fast PID read-compute-write cycle (Arduino UNO): QuickPID = **51µs**, PID_v1 = **128µs** 14 | - [x] `TIMER` mode for calling PID compute by an external timer function or ISR 15 | - [x] `analogReadFast()` support for AVR (4x faster) 16 | - [x] `analogWrite()` support for ESP32 and ESP32-S2 17 | - [x] Variable Proportional on Error to Proportional on Measurement parameter `POn` 18 | - [x] Variable Derivative on Error to Derivative on Measurement parameter `DOn` 19 | - [x] New PID Query Functions: `GetPterm();` `GetIterm();` `GetDterm();` 20 | - [x] 2-stage Integral windup prevention when output exceeds limits 21 | - [x] New REVERSE mode only changes sign of `error` and `dInput` 22 | - [x] Uses `float` instead of `double` 23 | 24 | #### AutoTune Features 25 | 26 | - [x] New AutoTune class added as a dynamic object and includes 10 tuning methods 27 | - [x] Compatible with reverse acting controllers 28 | - [x] Fast, non-blocking tuning sequence completes in only 1.5 oscillations 29 | - [x] Determines how easy the process is to control 30 | - [x] Determines ultimate period `Tu`, dead time `td`, ultimate gain `Ku`, and tuning parameters `Kp, Ki, Kd` 31 | 32 | ### [AutoTune](https://github.com/Dlloydev/QuickPID/wiki/AutoTune) 33 | 34 | #### [AutoTune Filter Examples](https://github.com/Dlloydev/QuickPID/wiki/AutoTune-Filter-Examples) 35 | 36 | 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. 37 | 38 | #### Direct and Reverse Controller Action 39 | 40 | If a positive error increases the controller's output, the controller is said to be direct acting (i.e. heating process). When a positive error decreases the controller's output, the controller is said to be reverse acting (i.e. cooling process). When the controller is set to `REVERSE` acting, the sign of the `error` and `dInput` (derivative of Input) is internally changed. All operating ranges and limits remain the same. To simulate a `REVERSE` acting process from a process that's `DIRECT` acting, the Input value needs to be "flipped". That is, if your reading from a 10-bit ADC with 0-1023 range, the input value used is (1023 - reading). 41 | 42 | ### Functions 43 | 44 | #### QuickPID_Constructor 45 | 46 | ```c++ 47 | QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, 48 | float Kp, float Ki, float Kd, float POn, float DOn, uint8_t ControllerDirection); 49 | ``` 50 | 51 | - `Input`, `Output`, and `Setpoint` are pointers to the variables holding these values. 52 | - `Kp`, `Ki`, and `Kd` are the PID proportional, integral, and derivative gains. 53 | - `POn` controls the mix of Proportional on Error to Proportional on Measurement. Range is 0.0-1.0, default = 1.0 54 | - `DOn` controls the mix of Derivative on Error to Derivative on Measurement. Range is 0.0-1.0, default = 0.0 55 | 56 | ![POnDOn](https://user-images.githubusercontent.com/63488701/120000053-68de3e00-bfa0-11eb-9db2-04c2f4be76a2.png) 57 | 58 | - `ControllerDirection` Either DIRECT or REVERSE determines which direction the output will move for a given error. 59 | 60 | ```c++ 61 | QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, 62 | float Kp, float Ki, float Kd, uint8_t ControllerDirection); 63 | ``` 64 | 65 | This allows you to use Proportional on Error without explicitly saying so. 66 | 67 | #### Compute 68 | 69 | ```c++ 70 | bool QuickPID::Compute(); 71 | ``` 72 | 73 | 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. 74 | 75 | #### SetTunings 76 | 77 | ```c++ 78 | void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn, float DOn); 79 | ``` 80 | 81 | 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. 82 | 83 | ```c++ 84 | void QuickPID::SetTunings(float Kp, float Ki, float Kd); 85 | ``` 86 | 87 | Set Tunings using the last remembered POn and DOn settings. See example [PID_AdaptiveTunings.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/PID_AdaptiveTunings/PID_AdaptiveTunings.ino) 88 | 89 | #### SetSampleTime 90 | 91 | ```c++ 92 | void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs); 93 | ``` 94 | 95 | Sets the period, in microseconds, at which the calculation is performed. The default is 100ms. 96 | 97 | #### SetOutputLimits 98 | 99 | ```c++ 100 | void QuickPID::SetOutputLimits(int Min, int Max); 101 | ``` 102 | 103 | The PID controller is designed to vary its output within a given range. By default this range is 0-255, the Arduino PWM range. 104 | 105 | #### SetMode 106 | 107 | ```c++ 108 | void QuickPID::SetMode(uint8_t Mode); 109 | ``` 110 | 111 | 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. 112 | 113 | `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: 114 | 115 | - [AutoTune_AVR_Interrupt_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_AVR_Interrupt_TIMER/AutoTune_AVR_Interrupt_TIMER.ino) 116 | - [AutoTune_AVR_Software_TIMER.ino](https://github.com/Dlloydev/QuickPID/blob/master/examples/AutoTune_AVR_Software_TIMER/AutoTune_AVR_Software_TIMER.ino) 117 | - [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) 118 | - [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) 119 | 120 | #### Initialize 121 | 122 | ```c++ 123 | void QuickPID::Initialize(); 124 | ``` 125 | 126 | Does all the things that need to happen to ensure a bump-less transfer from manual to automatic mode. 127 | 128 | #### SetControllerDirection 129 | 130 | ```c++ 131 | void QuickPID::SetControllerDirection(uint8_t Direction) 132 | ``` 133 | 134 | 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. 135 | 136 | #### PID Query Functions 137 | 138 | ```c++ 139 | float GetKp(); // proportional gain 140 | float GetKi(); // integral gain 141 | float GetKd(); // derivative gain 142 | float GetPterm(); // proportional component of output 143 | float GetIterm(); // integral component of output 144 | float GetDterm(); // derivative component of output 145 | mode_t GetMode(); // MANUAL (0) or AUTOMATIC (1) or TIMER (2) 146 | direction_t GetDirection(); // DIRECT (0) or REVERSE (1) 147 | ``` 148 | 149 | These functions query the internal state of the PID. 150 | 151 | #### Utility Functions 152 | 153 | ```c++ 154 | int QuickPID::analogReadFast(int ADCpin) 155 | ``` 156 | 157 | 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. 158 | 159 | #### [AnalogWrite (PWM and DAC) for ESP32](https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite) 160 | 161 | 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. 162 | 163 | ### Original README (Arduino PID) 164 | 165 | ``` 166 | *************************************************************** 167 | * Arduino PID Library - Version 1.2.1 168 | * by Brett Beauregard brettbeauregard.com 169 | * 170 | * This Library is licensed under the MIT License 171 | *************************************************************** 172 | ``` 173 | 174 | - For an ultra-detailed explanation of why the code is the way it is, please visit: 175 | http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ 176 | 177 | - For function documentation see: http://playground.arduino.cc/Code/PIDLibrary 178 | 179 | ------ 180 | 181 | -------------------------------------------------------------------------------- /src/analogWrite.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************** 2 | AnalogWrite Library for ESP32-ESP32S2 Arduino core - Version 2.0.8 3 | by dlloydev https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite 4 | This Library is licensed under the MIT License 5 | **********************************************************************************/ 6 | #include 7 | 8 | #if (defined(ESP32) || defined(ARDUINO_ARCH_ESP32)) 9 | #include "analogWrite.h" 10 | 11 | namespace aw { 12 | 13 | #if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) 14 | pinStatus_t pinsStatus[8] = { 15 | {0, -1, 980, 8, 0, 0 }, {2, -1, 980, 8, 0, 0 }, 16 | {4, -1, 980, 8, 0, 0 }, {6, -1, 980, 8, 0, 0 }, 17 | {1, -1, 980, 8, 0, 0 }, {3, -1, 980, 8, 0, 0 }, 18 | {5, -1, 980, 8, 0, 0 }, {7, -1, 980, 8, 0, 0 } 19 | }; 20 | const uint8_t chd = 1; 21 | #else //ESP32 22 | 23 | pinStatus_t pinsStatus[8] = { 24 | { 0, -1, 980, 8, 0, 0 }, { 2, -1, 980, 8, 0, 0 }, 25 | { 4, -1, 980, 8, 0, 0 }, { 6, -1, 980, 8, 0, 0 }, 26 | { 8, -1, 980, 8, 0, 0 }, {10, -1, 980, 8, 0, 0 }, 27 | {12, -1, 980, 8, 0, 0 }, {14, -1, 980, 8, 0, 0 } 28 | }; 29 | const uint8_t chd = 2; 30 | #endif 31 | 32 | float awLedcSetup(uint8_t ch, double frequency, uint8_t bits) { 33 | #if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) 34 | frequency *= 80; //workaround for issue 5050 35 | return ledcSetup(ch, frequency, bits) / 80; 36 | #else //ESP32 37 | return ledcSetup(ch, frequency, bits); 38 | #endif 39 | } 40 | 41 | void awDetachPin(uint8_t pin, uint8_t ch) { 42 | pinsStatus[ch / chd].pin = -1; 43 | pinsStatus[ch / chd].value = 0; 44 | pinsStatus[ch / chd].frequency = 980; 45 | pinsStatus[ch / chd].resolution = 8; 46 | pinsStatus[ch / chd].phase = 0; 47 | ledcWrite(ch / chd, 0); 48 | ledcSetup(ch / chd, 0, 0); 49 | ledcDetachPin(pinsStatus[ch / chd].pin); 50 | REG_SET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL, GPIO_MODE_DEF_DISABLE); 51 | } 52 | 53 | float awLedcReadFreq(uint8_t ch) { 54 | #if (CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3) 55 | return ledcReadFreq(ch) / 80; //workaround for issue 5050 56 | #else //ESP32 57 | return ledcReadFreq(ch); 58 | #endif 59 | } 60 | 61 | int8_t awGetChannel(int8_t pin) { 62 | if (!((pinMask >> pin) & 1)) return -1; //not pwm pin 63 | for (int8_t i = 0; i < 8; i++) { 64 | int8_t ch = pinsStatus[i].channel; 65 | if (pinsStatus[ch / chd].pin == pin) { 66 | return ch; 67 | break; 68 | } 69 | } 70 | for (int8_t i = 0; i < 8; i++) { 71 | int8_t ch = pinsStatus[i].channel; 72 | if ((REG_GET_FIELD(GPIO_PIN_MUX_REG[pin], MCU_SEL)) == 0) { //free pin 73 | if (pinsStatus[ch / chd].pin == -1) { //free channel 74 | if ((ledcRead(ch) < 1) && (ledcReadFreq(ch) < 1)) { //free timer 75 | pinsStatus[ch / chd].pin = pin; 76 | aw::awLedcSetup(ch, pinsStatus[ch / chd].frequency, pinsStatus[ch / chd].resolution); 77 | ledcAttachPin(pin, ch); 78 | return ch; 79 | break; 80 | } else { 81 | pinsStatus[ch / chd].pin = 88; //occupied timer 82 | return -1; 83 | break; 84 | } 85 | } 86 | } else { 87 | return -1; //occupied pin 88 | break; 89 | } 90 | } 91 | return -1; 92 | } 93 | 94 | } //namespace aw 95 | 96 | float analogWrite(int8_t pin, int32_t value) { 97 | if (pin == DAC1 || pin == DAC2) { //dac 98 | if (value > 255) value = 255; 99 | dacWrite(pin, value); 100 | } else { 101 | int8_t ch = aw::awGetChannel(pin); 102 | if (ch >= 0) { 103 | if (value == -1) aw::awDetachPin(pin, ch); 104 | else { // write PWM 105 | uint8_t bits = aw::pinsStatus[ch / aw::chd].resolution; 106 | if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain 107 | if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high 108 | if (ledcRead(ch) != value) ledcWrite(ch, value); 109 | aw::pinsStatus[ch / aw::chd].value = value; 110 | } 111 | } 112 | return aw::awLedcReadFreq(ch); 113 | } 114 | return 0; 115 | } 116 | 117 | float analogWrite(int8_t pin, int32_t value, float frequency) { 118 | if (pin == DAC1 || pin == DAC2) { //dac 119 | if (value > 255) value = 255; 120 | dacWrite(pin, value); 121 | } else { 122 | int8_t ch = aw::awGetChannel(pin); 123 | if (ch >= 0) { 124 | if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; 125 | if (value == -1) aw::awDetachPin(pin, ch); 126 | else { // write PWM 127 | uint8_t bits = aw::pinsStatus[ch / aw::chd].resolution; 128 | if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain 129 | if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high 130 | if (aw::pinsStatus[ch / aw::chd].frequency != frequency) { 131 | aw::awLedcSetup(ch, frequency, bits); 132 | ledcWrite(ch, value); 133 | aw::pinsStatus[ch / aw::chd].frequency = frequency; 134 | } 135 | if (aw::pinsStatus[ch / aw::chd].value != value) { 136 | ledcWrite(ch, value); 137 | aw::pinsStatus[ch / aw::chd].value = value; 138 | } 139 | } 140 | } 141 | return aw::awLedcReadFreq(ch); 142 | } 143 | return 0; 144 | } 145 | 146 | float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution) { 147 | if (pin == DAC1 || pin == DAC2) { //dac 148 | if (value > 255) value = 255; 149 | dacWrite(pin, value); 150 | } else { 151 | int8_t ch = aw::awGetChannel(pin); 152 | if (ch >= 0) { 153 | if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; 154 | if (value == -1) aw::awDetachPin(pin, ch); 155 | else { // write PWM 156 | uint8_t bits = resolution & 0xF; 157 | if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain 158 | if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high 159 | if ((aw::pinsStatus[ch / aw::chd].frequency != frequency) || (aw::pinsStatus[ch / aw::chd].resolution != bits)) { 160 | aw::awLedcSetup(ch, frequency, bits); 161 | ledcWrite(ch, value); 162 | aw::pinsStatus[ch / aw::chd].frequency = frequency; 163 | aw::pinsStatus[ch / aw::chd].resolution = bits; 164 | } 165 | if (aw::pinsStatus[ch / aw::chd].value != value) { 166 | ledcWrite(ch, value); 167 | aw::pinsStatus[ch / aw::chd].value = value; 168 | } 169 | } 170 | } 171 | return aw::awLedcReadFreq(ch); 172 | } 173 | return 0; 174 | } 175 | 176 | float analogWrite(int8_t pin, int32_t value, float frequency, uint8_t resolution, uint32_t phase) { 177 | if (pin == DAC1 || pin == DAC2) { //dac 178 | if (value > 255) value = 255; 179 | dacWrite(pin, value); 180 | } else { 181 | int8_t ch = aw::awGetChannel(pin); 182 | if (ch >= 0) { 183 | if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; 184 | if (value == -1) aw::awDetachPin(pin, ch); 185 | else { // write PWM 186 | uint8_t bits = resolution & 0xF; 187 | if (value > ((1 << bits) - 1)) value = (1 << bits); //constrain 188 | if ((bits > 7) && (value == ((1 << bits) - 1))) value = (1 << bits); //keep PWM high 189 | if ((aw::pinsStatus[ch / aw::chd].frequency != frequency) || (aw::pinsStatus[ch / aw::chd].resolution != bits)) { 190 | aw::awLedcSetup(ch, frequency, bits); 191 | ledcWrite(ch, value); 192 | aw::pinsStatus[ch / aw::chd].frequency = frequency; 193 | aw::pinsStatus[ch / aw::chd].resolution = bits; 194 | } 195 | if (aw::pinsStatus[ch / aw::chd].phase != phase) { 196 | uint32_t group = (ch / 8), timer = ((ch / 2) % 4); 197 | aw::ledc_channel_config_t ledc_channel { 198 | (uint8_t)pin, 199 | (aw::ledc_mode_t)group, 200 | (aw::ledc_channel_t)ch, 201 | aw::LEDC_INTR_DISABLE, 202 | (aw::ledc_timer_t)timer, 203 | (uint32_t)value, 204 | (int)phase, 205 | }; 206 | ledc_channel_config(&ledc_channel); 207 | ledc_set_duty_with_hpoint((aw::ledc_mode_t)group, (aw::ledc_channel_t)ch, value, phase); 208 | aw::pinsStatus[ch / aw::chd].phase = phase; 209 | } 210 | if (aw::pinsStatus[ch / aw::chd].value != value) { 211 | ledcWrite(ch, value); 212 | aw::pinsStatus[ch / aw::chd].value = value; 213 | } 214 | } 215 | } 216 | return aw::awLedcReadFreq(ch); 217 | } 218 | return 0; 219 | } 220 | 221 | float analogWriteFrequency(int8_t pin, float frequency) { 222 | int8_t ch = aw::awGetChannel(pin); 223 | if (ch >= 0) { 224 | if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; 225 | if (aw::pinsStatus[ch / aw::chd].frequency != frequency) { 226 | aw::awLedcSetup(ch, frequency, aw::pinsStatus[ch / aw::chd].resolution); 227 | ledcWrite(ch, aw::pinsStatus[ch / aw::chd].value); 228 | aw::pinsStatus[ch / aw::chd].frequency = frequency; 229 | } 230 | } 231 | return aw::awLedcReadFreq(ch); 232 | } 233 | 234 | int32_t analogWriteResolution(int8_t pin, uint8_t resolution) { 235 | int8_t ch = aw::awGetChannel(pin); 236 | if (ch >= 0) { 237 | if ((aw::pinsStatus[ch / aw::chd].pin) > 47) return -1; 238 | if (aw::pinsStatus[ch / aw::chd].resolution != resolution) { 239 | ledcDetachPin(pin); 240 | aw::awLedcSetup(ch, aw::pinsStatus[ch / aw::chd].frequency, resolution & 0xF); 241 | ledcAttachPin(pin, ch); 242 | ledcWrite(ch, aw::pinsStatus[ch / aw::chd].value); 243 | aw::pinsStatus[ch / aw::chd].resolution = resolution & 0xF; 244 | } 245 | } 246 | return 1 << (resolution & 0xF); 247 | } 248 | 249 | void setPinsStatusDefaults(int32_t value, float frequency, uint8_t resolution, uint32_t phase) { 250 | for (int8_t i = 0; i < 8; i++) { 251 | aw::pinsStatus[i].value = value; 252 | aw::pinsStatus[i].frequency = frequency; 253 | aw::pinsStatus[i].resolution = resolution; 254 | aw::pinsStatus[i].phase = phase; 255 | } 256 | } 257 | 258 | void printPinsStatus() { 259 | Serial.print("PWM pins: "); 260 | for (int i = 0; i < aw::muxSize; i++) { 261 | if ((aw::pinMask >> i) & 1) { 262 | Serial.print(i); Serial.print(", "); 263 | } 264 | } 265 | Serial.println(); 266 | 267 | Serial.println(); 268 | for (int i = 0; i < 8; i++) { 269 | int ch = aw::pinsStatus[i].channel; 270 | Serial.print("ch: "); 271 | if (ch < 10) Serial.print(" "); Serial.print(ch); Serial.print(" "); 272 | Serial.print("Pin: "); 273 | if ((aw::pinsStatus[ch / aw::chd].pin >= 0) && (aw::pinsStatus[ch / aw::chd].pin < 10)) Serial.print(" "); 274 | Serial.print(aw::pinsStatus[ch / aw::chd].pin); Serial.print(" "); 275 | Serial.print("Hz: "); 276 | if (aw::awLedcReadFreq(ch) < 10000) Serial.print(" "); 277 | if (aw::awLedcReadFreq(ch) < 1000) Serial.print(" "); 278 | if (aw::awLedcReadFreq(ch) < 100) Serial.print(" "); 279 | if (aw::awLedcReadFreq(ch) < 10) Serial.print(" "); 280 | Serial.print(aw::awLedcReadFreq(ch)); Serial.print(" "); 281 | Serial.print("Bits: "); 282 | if (aw::pinsStatus[ch / aw::chd].resolution < 10) Serial.print(" "); 283 | Serial.print(aw::pinsStatus[ch / aw::chd].resolution); Serial.print(" "); 284 | Serial.print("Duty: "); 285 | if (aw::pinsStatus[ch / aw::chd].value < 10000) Serial.print(" "); 286 | if (aw::pinsStatus[ch / aw::chd].value < 1000) Serial.print(" "); 287 | if (aw::pinsStatus[ch / aw::chd].value < 100) Serial.print(" "); 288 | if (aw::pinsStatus[ch / aw::chd].value < 10) Serial.print(" "); 289 | Serial.print(aw::pinsStatus[ch / aw::chd].value); Serial.print(" "); 290 | Serial.print("Ø: "); 291 | if (aw::pinsStatus[ch / aw::chd].phase < 1000) Serial.print(" "); 292 | if (aw::pinsStatus[ch / aw::chd].phase < 100) Serial.print(" "); 293 | if (aw::pinsStatus[ch / aw::chd].phase < 10) Serial.print(" "); 294 | Serial.print(aw::pinsStatus[ch / aw::chd].phase); 295 | Serial.println(); 296 | } 297 | } 298 | #endif //ESP32 299 | -------------------------------------------------------------------------------- /src/QuickPID.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************** 2 | QuickPID Library for Arduino - Version 2.4.6 3 | by dlloydev https://github.com/Dlloydev/QuickPID 4 | Based on the Arduino PID Library and work on AutoTunePID class 5 | by gnalbandian (Gonzalo). Licensed under the MIT License. 6 | **********************************************************************************/ 7 | 8 | #if ARDUINO >= 100 9 | #include "Arduino.h" 10 | #else 11 | #include "WProgram.h" 12 | #endif 13 | 14 | #include "QuickPID.h" 15 | 16 | /* Constructor ******************************************************************** 17 | The parameters specified here are those for for which we can't set up 18 | reliable defaults, so we need to have the user set them. 19 | **********************************************************************************/ 20 | QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, 21 | float Kp, float Ki, float Kd, float POn = 1.0, float DOn = 0.0, 22 | QuickPID::direction_t ControllerDirection = DIRECT) { 23 | 24 | myOutput = Output; 25 | myInput = Input; 26 | mySetpoint = Setpoint; 27 | mode = MANUAL; 28 | 29 | QuickPID::SetOutputLimits(0, 255); // same default as Arduino PWM limit 30 | sampleTimeUs = 100000; // 0.1 sec default 31 | QuickPID::SetControllerDirection(ControllerDirection); 32 | QuickPID::SetTunings(Kp, Ki, Kd, POn, DOn); 33 | 34 | lastTime = micros() - sampleTimeUs; 35 | } 36 | 37 | /* Constructor ********************************************************************* 38 | To allow using Proportional on Error without explicitly saying so. 39 | **********************************************************************************/ 40 | QuickPID::QuickPID(float* Input, float* Output, float* Setpoint, 41 | float Kp, float Ki, float Kd, direction_t ControllerDirection = DIRECT) 42 | : QuickPID::QuickPID(Input, Output, Setpoint, Kp, Ki, Kd, pOn = 1.0, dOn = 0.0, ControllerDirection = DIRECT) { 43 | } 44 | 45 | /* Compute() *********************************************************************** 46 | This function should be called every time "void loop()" executes. The function 47 | will decide whether a new PID Output needs to be computed. Returns true 48 | when the output is computed, false when nothing has been done. 49 | **********************************************************************************/ 50 | bool QuickPID::Compute() { 51 | if (mode == MANUAL) return false; 52 | uint32_t now = micros(); 53 | uint32_t timeChange = (now - lastTime); 54 | if (mode == TIMER || timeChange >= sampleTimeUs) { 55 | float input = *myInput; 56 | float dInput = input - lastInput; 57 | error = *mySetpoint - input; 58 | if (controllerDirection == REVERSE) { 59 | error = -error; 60 | dInput = -dInput; 61 | } 62 | pmTerm = kpm * dInput; 63 | peTerm = kpe * error; 64 | iTerm = ki * error; 65 | dmTerm = kdm * dInput; 66 | deTerm = -kde * error; 67 | 68 | outputSum += iTerm; // include integral amount 69 | if (outputSum > outMax) outputSum -= outputSum - outMax; // early integral anti-windup at outMax 70 | else if (outputSum < outMin) outputSum += outMin - outputSum; // early integral anti-windup at outMin 71 | outputSum = constrain(outputSum, outMin, outMax); // integral anti-windup clamp 72 | outputSum = constrain(outputSum - pmTerm, outMin, outMax); // include pmTerm and clamp 73 | *myOutput = constrain(outputSum + peTerm + dmTerm - deTerm, outMin, outMax); // totalize, clamp and drive the output 74 | 75 | lastInput = input; 76 | lastTime = now; 77 | return true; 78 | } 79 | else return false; 80 | } 81 | 82 | /* SetTunings(....)************************************************************ 83 | This function allows the controller's dynamic performance to be adjusted. 84 | it's called automatically from the constructor, but tunings can also 85 | be adjusted on the fly during normal operation. 86 | ******************************************************************************/ 87 | void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1.0, float DOn = 0.0) { 88 | if (Kp < 0 || Ki < 0 || Kd < 0) return; 89 | pOn = POn; 90 | dOn = DOn; 91 | dispKp = Kp; dispKi = Ki; dispKd = Kd; 92 | float SampleTimeSec = (float)sampleTimeUs / 1000000; 93 | kp = Kp; 94 | ki = Ki * SampleTimeSec; 95 | kd = Kd / SampleTimeSec; 96 | kpe = kp * pOn; 97 | kpm = kp * (1 - pOn); 98 | kde = kd * dOn; 99 | kdm = kd * (1 - dOn); 100 | } 101 | 102 | /* SetTunings(...)************************************************************ 103 | Set Tunings using the last remembered POn and DOn setting. 104 | ******************************************************************************/ 105 | void QuickPID::SetTunings(float Kp, float Ki, float Kd) { 106 | SetTunings(Kp, Ki, Kd, pOn, dOn); 107 | } 108 | 109 | /* SetSampleTime(.)*********************************************************** 110 | Sets the period, in microseconds, at which the calculation is performed. 111 | ******************************************************************************/ 112 | void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs) { 113 | if (NewSampleTimeUs > 0) { 114 | float ratio = (float)NewSampleTimeUs / (float)sampleTimeUs; 115 | ki *= ratio; 116 | kd /= ratio; 117 | sampleTimeUs = NewSampleTimeUs; 118 | } 119 | } 120 | 121 | /* SetOutputLimits(..)******************************************************** 122 | The PID controller is designed to vary its output within a given range. 123 | By default this range is 0-255, the Arduino PWM range. 124 | ******************************************************************************/ 125 | void QuickPID::SetOutputLimits(int Min, int Max) { 126 | if (Min >= Max) return; 127 | outMin = Min; 128 | outMax = Max; 129 | 130 | if (mode != MANUAL) { 131 | *myOutput = constrain(*myOutput, outMin, outMax); 132 | outputSum = constrain(outputSum, outMin, outMax); 133 | } 134 | } 135 | 136 | /* SetMode(.)***************************************************************** 137 | Sets the controller mode to MANUAL (0), AUTOMATIC (1) or TIMER (2) 138 | when the transition from MANUAL to AUTOMATIC or TIMER occurs, the 139 | controller is automatically initialized. 140 | ******************************************************************************/ 141 | void QuickPID::SetMode(mode_t Mode) { 142 | if (mode == MANUAL && Mode != MANUAL) { // just went from MANUAL to AUTOMATIC or TIMER 143 | QuickPID::Initialize(); 144 | } 145 | mode = Mode; 146 | } 147 | 148 | /* Initialize()**************************************************************** 149 | Does all the things that need to happen to ensure a bumpless transfer 150 | from manual to automatic mode. 151 | ******************************************************************************/ 152 | void QuickPID::Initialize() { 153 | outputSum = *myOutput; 154 | lastInput = *myInput; 155 | outputSum = constrain(outputSum, outMin, outMax); 156 | } 157 | 158 | /* SetControllerDirection(.)************************************************** 159 | The PID will either be connected to a DIRECT acting process (+Output leads 160 | to +Input) or a REVERSE acting process(+Output leads to -Input). 161 | ******************************************************************************/ 162 | void QuickPID::SetControllerDirection(direction_t ControllerDirection) { 163 | controllerDirection = ControllerDirection; 164 | } 165 | 166 | /* Status Functions************************************************************ 167 | These functions query the internal state of the PID. They're here for display 168 | purposes. These are the functions the PID Front-end uses for example. 169 | ******************************************************************************/ 170 | float QuickPID::GetKp() { 171 | return dispKp; 172 | } 173 | float QuickPID::GetKi() { 174 | return dispKi; 175 | } 176 | float QuickPID::GetKd() { 177 | return dispKd; 178 | } 179 | float QuickPID::GetPterm() { 180 | return peTerm + pmTerm; 181 | } 182 | float QuickPID::GetIterm() { 183 | return iTerm; 184 | } 185 | float QuickPID::GetDterm() { 186 | return deTerm + dmTerm; 187 | } 188 | QuickPID::mode_t QuickPID::GetMode() { 189 | return mode; 190 | } 191 | QuickPID::direction_t QuickPID::GetDirection() { 192 | return controllerDirection; 193 | } 194 | 195 | /* AutoTune Functions*********************************************************/ 196 | 197 | void QuickPID::AutoTune(tuningMethod tuningRule) { 198 | autoTune = new AutoTunePID(myInput, myOutput, tuningRule); 199 | } 200 | 201 | void QuickPID::clearAutoTune() { 202 | if (autoTune) 203 | delete autoTune; 204 | } 205 | 206 | AutoTunePID::AutoTunePID() { 207 | _input = nullptr; 208 | _output = nullptr; 209 | reset(); 210 | } 211 | 212 | AutoTunePID::AutoTunePID(float *input, float *output, tuningMethod tuningRule) { 213 | AutoTunePID(); 214 | _input = input; 215 | _output = output; 216 | _tuningRule = tuningRule; 217 | } 218 | 219 | void AutoTunePID::reset() { 220 | _tLast = 0; 221 | _t0 = 0; 222 | _t1 = 0; 223 | _t2 = 0; 224 | _t3 = 0; 225 | _Ku = 0.0; 226 | _Tu = 0.0; 227 | _td = 0.0; 228 | _kp = 0.0; 229 | _ki = 0.0; 230 | _kd = 0.0; 231 | _rdAvg = 0.0; 232 | _peakHigh = 0.0; 233 | _peakLow = 0.0; 234 | _autoTuneStage = 0; 235 | } 236 | 237 | void AutoTunePID::autoTuneConfig(const float outputStep, const float hysteresis, const float atSetpoint, 238 | const float atOutput, const bool dir, const bool printOrPlotter, uint32_t sampleTimeUs) 239 | { 240 | _outputStep = outputStep; 241 | _hysteresis = hysteresis; 242 | _atSetpoint = atSetpoint; 243 | _atOutput = atOutput; 244 | _direction = dir; 245 | _printOrPlotter = printOrPlotter; 246 | _tLoop = constrain((sampleTimeUs / 8), 500, 16383); 247 | _autoTuneStage = STABILIZING; 248 | } 249 | 250 | byte AutoTunePID::autoTuneLoop() { 251 | if ((micros() - _tLast) <= _tLoop) return WAIT; 252 | else _tLast = micros(); 253 | switch (_autoTuneStage) { 254 | case AUTOTUNE: 255 | return AUTOTUNE; 256 | break; 257 | case WAIT: 258 | return WAIT; 259 | break; 260 | case STABILIZING: 261 | if (_printOrPlotter == 1) Serial.print(F("Stabilizing →")); 262 | _t0 = millis(); 263 | _peakHigh = _atSetpoint; 264 | _peakLow = _atSetpoint; 265 | (!_direction) ? *_output = 0 : *_output = _atOutput + (_outputStep * 2); 266 | _autoTuneStage = COARSE; 267 | return AUTOTUNE; 268 | break; 269 | case COARSE: // coarse adjust 270 | if (millis() - _t0 < 2000) { 271 | return AUTOTUNE; 272 | break; 273 | } 274 | if (*_input < (_atSetpoint - _hysteresis)) { 275 | (!_direction) ? *_output = _atOutput + (_outputStep * 2) : *_output = _atOutput - (_outputStep * 2); 276 | _autoTuneStage = FINE; 277 | } 278 | return AUTOTUNE; 279 | break; 280 | case FINE: // fine adjust 281 | if (*_input > _atSetpoint) { 282 | (!_direction) ? *_output = _atOutput - _outputStep : *_output = _atOutput + _outputStep; 283 | _autoTuneStage = TEST; 284 | } 285 | return AUTOTUNE; 286 | break; 287 | case TEST: // run AutoTune relay method 288 | if (*_input < _atSetpoint) { 289 | if (_printOrPlotter == 1) Serial.print(F(" AutoTune →")); 290 | (!_direction) ? *_output = _atOutput + _outputStep : *_output = _atOutput - _outputStep; 291 | _autoTuneStage = T0; 292 | } 293 | return AUTOTUNE; 294 | break; 295 | case T0: // get t0 296 | if (*_input > _atSetpoint) { 297 | _t0 = micros(); 298 | if (_printOrPlotter == 1) Serial.print(F(" t0 →")); 299 | _inputLast = *_input; 300 | _autoTuneStage = T1; 301 | } 302 | return AUTOTUNE; 303 | break; 304 | case T1: // get t1 305 | if ((*_input > _atSetpoint) && (*_input > _inputLast)) { 306 | _t1 = micros(); 307 | if (_printOrPlotter == 1) Serial.print(F(" t1 →")); 308 | _autoTuneStage = T2; 309 | } 310 | return AUTOTUNE; 311 | break; 312 | case T2: // get t2 313 | _rdAvg = *_input; 314 | if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; 315 | if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; 316 | if (_rdAvg > _atSetpoint + _hysteresis) { 317 | _t2 = micros(); 318 | if (_printOrPlotter == 1) Serial.print(F(" t2 →")); 319 | (!_direction) ? *_output = _atOutput - _outputStep : *_output = _atOutput + _outputStep; 320 | _autoTuneStage = T3L; 321 | } 322 | return AUTOTUNE; 323 | break; 324 | case T3L: // t3 low cycle 325 | _rdAvg = *_input; 326 | if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; 327 | if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; 328 | if (_rdAvg < _atSetpoint - _hysteresis) { 329 | (!_direction) ? *_output = _atOutput + _outputStep : *_output = _atOutput - _outputStep; 330 | _autoTuneStage = T3H; 331 | } 332 | return AUTOTUNE; 333 | break; 334 | case T3H: // t3 high cycle, relay test done 335 | _rdAvg = *_input; 336 | if (_rdAvg > _peakHigh) _peakHigh = _rdAvg; 337 | if ((_rdAvg < _peakLow) && (_peakHigh >= (_atSetpoint + _hysteresis))) _peakLow = _rdAvg; 338 | if (_rdAvg > _atSetpoint + _hysteresis) { 339 | _t3 = micros(); 340 | if (_printOrPlotter == 1) Serial.println(F(" t3 → done.")); 341 | _autoTuneStage = CALC; 342 | } 343 | return AUTOTUNE; 344 | break; 345 | case CALC: // calculations 346 | _td = (float)(_t1 - _t0) / 1000000.0; // dead time (seconds) 347 | _Tu = (float)(_t3 - _t2) / 1000000.0; // ultimate period (seconds) 348 | _Ku = (float)(4 * _outputStep * 2) / (float)(3.14159 * sqrt (sq (_peakHigh - _peakLow) - sq (_hysteresis))); // ultimate gain 349 | if (_tuningRule == tuningMethod::AMIGOF_PID) { 350 | (_td < 0.1) ? _td = 0.1 : _td = _td; 351 | _kp = (0.2 + 0.45 * (_Tu / _td)) / _Ku; 352 | float Ti = (((0.4 * _td) + (0.8 * _Tu)) / (_td + (0.1 * _Tu)) * _td); 353 | float Td = (0.5 * _td * _Tu) / ((0.3 * _td) + _Tu); 354 | _ki = _kp / Ti; 355 | _kd = Td * _kp; 356 | } else { //other rules 357 | _kp = (float)(RulesContants[static_cast(_tuningRule)][0] / 1000.0) * _Ku; 358 | _ki = (float)(RulesContants[static_cast(_tuningRule)][1] / 1000.0) * (_Ku / _Tu); 359 | _kd = (float)(RulesContants[static_cast(_tuningRule)][2] / 1000.0) * (_Ku * _Tu); 360 | } 361 | if (_printOrPlotter == 1) { 362 | // Controllability https://blog.opticontrols.com/wp-content/uploads/2011/06/td-versus-tau.png 363 | if ((_Tu / _td + 0.0001) > 0.75) Serial.println(F("This process is easy to control.")); 364 | else if ((_Tu / _td + 0.0001) > 0.25) Serial.println(F("This process has average controllability.")); 365 | else Serial.println(F("This process is difficult to control.")); 366 | Serial.print(F("Tu: ")); Serial.print(_Tu); // Ultimate Period (sec) 367 | Serial.print(F(" td: ")); Serial.print(_td); // Dead Time (sec) 368 | Serial.print(F(" Ku: ")); Serial.print(_Ku); // Ultimate Gain 369 | Serial.print(F(" Kp: ")); Serial.print(_kp); 370 | Serial.print(F(" Ki: ")); Serial.print(_ki); 371 | Serial.print(F(" Kd: ")); Serial.println(_kd); 372 | Serial.println(); 373 | } 374 | _autoTuneStage = TUNINGS; 375 | return AUTOTUNE; 376 | break; 377 | case TUNINGS: 378 | _autoTuneStage = CLR; 379 | return TUNINGS; 380 | break; 381 | case CLR: 382 | return CLR; 383 | break; 384 | default: 385 | return CLR; 386 | break; 387 | } 388 | return CLR; 389 | } 390 | 391 | void AutoTunePID::setAutoTuneConstants(float * kp, float * ki, float * kd) { 392 | *kp = _kp; 393 | *ki = _ki; 394 | *kd = _kd; 395 | } 396 | 397 | /* Utility************************************************************/ 398 | // https://github.com/avandalen/avdweb_AnalogReadFast 399 | int QuickPID::analogReadFast(int ADCpin) { 400 | #if defined(__AVR_ATmega328P__) 401 | byte ADCregOriginal = ADCSRA; 402 | ADCSRA = (ADCSRA & B11111000) | 5; // 32 prescaler 403 | int adc = analogRead(ADCpin); 404 | ADCSRA = ADCregOriginal; 405 | return adc; 406 | #else 407 | return analogRead(ADCpin); 408 | # endif 409 | } 410 | --------------------------------------------------------------------------------