├── LICENSE ├── README.md ├── library.properties ├── pidautotuner.cpp └── pidautotuner.h /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # arduino-pid-autotuner 2 | Automated PID tuning using Ziegler-Nichols/relay method. 3 | 4 | Originally written for Arduino and compatible boards, but does not rely on the Arduino standard library. 5 | 6 | ## Disclaimer 7 | **Issues have been disabled on this repository due to too many off-topic questions about PID control in general or how to use this code. This project is a simple implementation of the algorithm described [here](https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method) and is not guaranteed to work in every use case. If you don't know what this code is intended to do, you probably don't need to use it.** 8 | 9 | ## How does it work? 10 | `pidautotuner.h` and `pidautotuner.cpp` are fully commented to explain how the algorithm works. 11 | 12 | ## What PID controller does this work with? 13 | This algorithm should work with any implementation of PID control if it is properly configured. 14 | 15 | ## Example code (Arduino) 16 | ```c 17 | #include 18 | 19 | void setup() { 20 | 21 | PIDAutotuner tuner = PIDAutotuner(); 22 | 23 | // Set the target value to tune to 24 | // This will depend on what you are tuning. This should be set to a value within 25 | // the usual range of the setpoint. For low-inertia systems, values at the lower 26 | // end of this range usually give better results. For anything else, start with a 27 | // value at the middle of the range. 28 | tuner.setTargetInputValue(targetInputValue); 29 | 30 | // Set the loop interval in microseconds 31 | // This must be the same as the interval the PID control loop will run at 32 | tuner.setLoopInterval(loopInterval); 33 | 34 | // Set the output range 35 | // These are the minimum and maximum possible output values of whatever you are 36 | // using to control the system (Arduino analogWrite, for example, is 0-255) 37 | tuner.setOutputRange(0, 255); 38 | 39 | // Set the Ziegler-Nichols tuning mode 40 | // Set it to either PIDAutotuner::ZNModeBasicPID, PIDAutotuner::ZNModeLessOvershoot, 41 | // or PIDAutotuner::ZNModeNoOvershoot. Defaults to ZNModeNoOvershoot as it is the 42 | // safest option. 43 | tuner.setZNMode(PIDAutotuner::ZNModeBasicPID); 44 | 45 | // This must be called immediately before the tuning loop 46 | // Must be called with the current time in microseconds 47 | tuner.startTuningLoop(micros()); 48 | 49 | // Run a loop until tuner.isFinished() returns true 50 | long microseconds; 51 | while (!tuner.isFinished()) { 52 | 53 | // This loop must run at the same speed as the PID control loop being tuned 54 | long prevMicroseconds = microseconds; 55 | microseconds = micros(); 56 | 57 | // Get input value here (temperature, encoder position, velocity, etc) 58 | double input = doSomethingToGetInput(); 59 | 60 | // Call tunePID() with the input value and current time in microseconds 61 | double output = tuner.tunePID(input, microseconds); 62 | 63 | // Set the output - tunePid() will return values within the range configured 64 | // by setOutputRange(). Don't change the value or the tuning results will be 65 | // incorrect. 66 | doSomethingToSetOutput(output); 67 | 68 | // This loop must run at the same speed as the PID control loop being tuned 69 | while (micros() - microseconds < loopInterval) delayMicroseconds(1); 70 | } 71 | 72 | // Turn the output off here. 73 | doSomethingToSetOutput(0); 74 | 75 | // Get PID gains - set your PID controller's gains to these 76 | double kp = tuner.getKp(); 77 | double ki = tuner.getKi(); 78 | double kd = tuner.getKd(); 79 | } 80 | 81 | void loop() { 82 | 83 | // ... 84 | } 85 | ``` 86 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=PID Autotuner 2 | version=1.0 3 | author=jackw01 4 | maintainer=jackw01 5 | sentence=PID autotuning library 6 | paragraph=Automatically tunes PID control loops using the Ziegler-Nichols method. 7 | category=Signal Input/Output 8 | url=https://github.com/jackw01/arduino-pid-autotuner 9 | architectures=* 10 | -------------------------------------------------------------------------------- /pidautotuner.cpp: -------------------------------------------------------------------------------- 1 | // PID automated tuning (Ziegler-Nichols/relay method) for Arduino and compatible boards 2 | // Copyright (c) 2016-2020 jackw01 3 | // This code is distrubuted under the MIT License, see LICENSE for details 4 | 5 | #include "pidautotuner.h" 6 | 7 | PIDAutotuner::PIDAutotuner() { 8 | } 9 | 10 | // Set target input for tuning 11 | void PIDAutotuner::setTargetInputValue(float target) { 12 | targetInputValue = target; 13 | } 14 | 15 | // Set loop interval 16 | void PIDAutotuner::setLoopInterval(long interval) { 17 | loopInterval = interval; 18 | } 19 | 20 | // Set output range 21 | void PIDAutotuner::setOutputRange(float min, float max) { 22 | minOutput = min; 23 | maxOutput = max; 24 | } 25 | 26 | // Set Ziegler-Nichols tuning mode 27 | void PIDAutotuner::setZNMode(ZNMode zn) { 28 | znMode = zn; 29 | } 30 | 31 | // Set tuning cycles 32 | void PIDAutotuner::setTuningCycles(int tuneCycles) { 33 | cycles = tuneCycles; 34 | } 35 | 36 | // Initialize all variables before loop 37 | void PIDAutotuner::startTuningLoop(unsigned long us) { 38 | i = 0; // Cycle counter 39 | output = true; // Current output state 40 | outputValue = maxOutput; 41 | t1 = t2 = us; // Times used for calculating period 42 | microseconds = tHigh = tLow = 0; // More time variables 43 | max = -1000000000000; // Max input 44 | min = 1000000000000; // Min input 45 | pAverage = iAverage = dAverage = 0; 46 | } 47 | 48 | // Run one cycle of the loop 49 | float PIDAutotuner::tunePID(float input, unsigned long us) { 50 | // Useful information on the algorithm used (Ziegler-Nichols method/Relay method) 51 | // http://www.processcontrolstuff.net/wp-content/uploads/2015/02/relay_autot-2.pdf 52 | // https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method 53 | // https://www.cds.caltech.edu/~murray/courses/cds101/fa04/caltech/am04_ch8-3nov04.pdf 54 | 55 | // Basic explanation of how this works: 56 | // * Turn on the output of the PID controller to full power 57 | // * Wait for the output of the system being tuned to reach the target input value 58 | // and then turn the controller output off 59 | // * Wait for the output of the system being tuned to decrease below the target input 60 | // value and turn the controller output back on 61 | // * Do this a lot 62 | // * Calculate the ultimate gain using the amplitude of the controller output and 63 | // system output 64 | // * Use this and the period of oscillation to calculate PID gains using the 65 | // Ziegler-Nichols method 66 | 67 | // Calculate time delta 68 | //long prevMicroseconds = microseconds; 69 | microseconds = us; 70 | //float deltaT = microseconds - prevMicroseconds; 71 | 72 | // Calculate max and min 73 | max = (max > input) ? max : input; 74 | min = (min < input) ? min : input; 75 | 76 | // Output is on and input signal has risen to target 77 | if (output && input > targetInputValue) { 78 | // Turn output off, record current time as t1, calculate tHigh, and reset maximum 79 | output = false; 80 | outputValue = minOutput; 81 | t1 = us; 82 | tHigh = t1 - t2; 83 | max = targetInputValue; 84 | } 85 | 86 | // Output is off and input signal has dropped to target 87 | if (!output && input < targetInputValue) { 88 | // Turn output on, record current time as t2, calculate tLow 89 | output = true; 90 | outputValue = maxOutput; 91 | t2 = us; 92 | tLow = t2 - t1; 93 | 94 | // Calculate Ku (ultimate gain) 95 | // Formula given is Ku = 4d / πa 96 | // d is the amplitude of the output signal 97 | // a is the amplitude of the input signal 98 | float ku = (4.0 * ((maxOutput - minOutput) / 2.0)) / (M_PI * (max - min) / 2.0); 99 | 100 | // Calculate Tu (period of output oscillations) 101 | float tu = tLow + tHigh; 102 | 103 | // How gains are calculated 104 | // PID control algorithm needs Kp, Ki, and Kd 105 | // Ziegler-Nichols tuning method gives Kp, Ti, and Td 106 | // 107 | // Kp = 0.6Ku = Kc 108 | // Ti = 0.5Tu = Kc/Ki 109 | // Td = 0.125Tu = Kd/Kc 110 | // 111 | // Solving these equations for Kp, Ki, and Kd gives this: 112 | // 113 | // Kp = 0.6Ku 114 | // Ki = Kp / (0.5Tu) 115 | // Kd = 0.125 * Kp * Tu 116 | 117 | // Constants 118 | // https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method 119 | 120 | float kpConstant, tiConstant, tdConstant; 121 | if (znMode == ZNModeBasicPID) { 122 | kpConstant = 0.6; 123 | tiConstant = 0.5; 124 | tdConstant = 0.125; 125 | } else if (znMode == ZNModeLessOvershoot) { 126 | kpConstant = 0.33; 127 | tiConstant = 0.5; 128 | tdConstant = 0.33; 129 | } else { // Default to No Overshoot mode as it is the safest 130 | kpConstant = 0.2; 131 | tiConstant = 0.5; 132 | tdConstant = 0.33; 133 | } 134 | 135 | // Calculate gains 136 | kp = kpConstant * ku; 137 | ki = (kp / (tiConstant * tu)) * loopInterval; 138 | kd = (tdConstant * kp * tu) / loopInterval; 139 | 140 | // Average all gains after the first two cycles 141 | if (i > 1) { 142 | pAverage += kp; 143 | iAverage += ki; 144 | dAverage += kd; 145 | } 146 | 147 | // Reset minimum 148 | min = targetInputValue; 149 | 150 | // Increment cycle count 151 | i ++; 152 | } 153 | 154 | // If loop is done, disable output and calculate averages 155 | if (i >= cycles) { 156 | output = false; 157 | outputValue = minOutput; 158 | kp = pAverage / (i - 1); 159 | ki = iAverage / (i - 1); 160 | kd = dAverage / (i - 1); 161 | } 162 | 163 | return outputValue; 164 | } 165 | 166 | // Get PID constants after tuning 167 | float PIDAutotuner::getKp() { return kp; }; 168 | float PIDAutotuner::getKi() { return ki; }; 169 | float PIDAutotuner::getKd() { return kd; }; 170 | 171 | // Is the tuning loop finished? 172 | bool PIDAutotuner::isFinished() { 173 | return (i >= cycles); 174 | } 175 | 176 | // return number of tuning cycle 177 | int PIDAutotuner::getCycle() { 178 | return i; 179 | } -------------------------------------------------------------------------------- /pidautotuner.h: -------------------------------------------------------------------------------- 1 | // PID automated tuning (Ziegler-Nichols/relay method) for Arduino and compatible boards 2 | // Copyright (c) 2016-2020 jackw01 3 | // This code is distrubuted under the MIT License, see LICENSE for details 4 | 5 | #ifndef PIDAUTOTUNER_H 6 | #define PIDAUTOTUNER_H 7 | 8 | #ifndef M_PI 9 | #define M_PI 3.14159265358979323846 10 | #endif 11 | 12 | class PIDAutotuner { 13 | public: 14 | // Constants for Ziegler-Nichols tuning mode 15 | typedef enum { 16 | ZNModeBasicPID, 17 | ZNModeLessOvershoot, 18 | ZNModeNoOvershoot 19 | } ZNMode; 20 | 21 | PIDAutotuner(); 22 | 23 | // Configure parameters for PID tuning 24 | // See README for more details - https://github.com/jackw01/arduino-pid-autotuner/blob/master/README.md 25 | // targetInputValue: the target value to tune to 26 | // loopInterval: PID loop interval in microseconds - must match whatever the PID loop being tuned runs at 27 | // outputRange: min and max values of the output that can be used to control the system (0, 255 for analogWrite) 28 | // znMode: Ziegler-Nichols tuning mode (znModeBasicPID, znModeLessOvershoot, znModeNoOvershoot) 29 | // tuningCycles: number of cycles that the tuning runs for (optional, default is 10) 30 | void setTargetInputValue(float target); 31 | void setLoopInterval(long interval); 32 | void setOutputRange(float min, float max); 33 | void setZNMode(ZNMode zn); 34 | void setTuningCycles(int tuneCycles); 35 | 36 | // Must be called immediately before the tuning loop starts 37 | void startTuningLoop(unsigned long us); 38 | 39 | // Automatically tune PID 40 | // This function must be run in a loop at the same speed as the PID loop being tuned 41 | // See README for more details - https://github.com/jackw01/arduino-pid-autotuner/blob/master/README.md 42 | float tunePID(float input, unsigned long us); 43 | 44 | // Get results of most recent tuning 45 | float getKp(); 46 | float getKi(); 47 | float getKd(); 48 | 49 | bool isFinished(); // Is the tuning finished? 50 | 51 | int getCycle(); // return tuning cycle 52 | 53 | private: 54 | float targetInputValue = 0; 55 | float loopInterval = 0; 56 | float minOutput, maxOutput; 57 | ZNMode znMode = ZNModeNoOvershoot; 58 | int cycles = 10; 59 | 60 | // See startTuningLoop() 61 | int i; 62 | bool output; 63 | float outputValue; 64 | long microseconds, t1, t2, tHigh, tLow; 65 | float max, min; 66 | float pAverage, iAverage, dAverage; 67 | 68 | float kp, ki, kd; 69 | }; 70 | 71 | #endif 72 | --------------------------------------------------------------------------------