New Version with AutoTune

Dlloydev · Jan 30, 2021 · a55f341 · a55f341
1 parent 6be0fbe
commit a55f341
Show file tree

Hide file tree

Showing 8 changed files with 235 additions and 118 deletions.
diff --git a/QuickPID.cpp b/QuickPID.cpp
@@ -1,5 +1,5 @@
 /**********************************************************************************
-   QuickPID Library for Arduino - Version 2.0.5
+   QuickPID Library for Arduino - Version 2.1.0
    by dlloydev https://github.com/Dlloydev/QuickPID
    Based on the Arduino PID Library by Brett Beauregard
 
@@ -27,12 +27,12 @@ QuickPID::QuickPID(int16_t* Input, int16_t* Output, int16_t* Setpoint,
   inAuto = false;
 
   QuickPID::SetOutputLimits(0, 255);  // default is same as the arduino PWM limit
-  SampleTimeUs = 100000;              // default is 0.1 seconds
+  sampleTimeUs = 100000;              // default is 0.1 seconds
 
   QuickPID::SetControllerDirection(ControllerDirection);
   QuickPID::SetTunings(Kp, Ki, Kd, POn);
 
-  lastTime = micros() - SampleTimeUs;
+  lastTime = micros() - sampleTimeUs;
 }
 
 /* Constructor ********************************************************************
@@ -58,29 +58,100 @@ bool QuickPID::Compute()
   if (!inAuto) return false;
   uint32_t now = micros();
   uint32_t timeChange = (now - lastTime);
-  if (timeChange >= SampleTimeUs)
-  {
-    /*Compute all the working error variables*/
+
+  if (timeChange >= sampleTimeUs) {  // Compute the working error variables
     int16_t input = *myInput;
     int16_t dInput = input - lastInput;
     error = *mySetpoint - input;
 
-    /*Working error, Proportional distribution and Remaining PID output*/
-    if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput);
-    else outputSum += (kpi * error) - (kpd * dInput);
+    if (kpi < 31 && kpd < 31) outputSum += FX_MUL(FL_FX(kpi) , error) - FX_MUL(FL_FX(kpd), dInput); // fixed-point
+    else outputSum += (kpi * error) - (kpd * dInput); // floating-point
 
-    if (outputSum > outMax) outputSum = outMax;
-    if (outputSum < outMin) outputSum = outMin;
+    outputSum = QuickPID::Saturate(outputSum);
     *myOutput = outputSum;
 
-    /*Remember some variables for next time*/
     lastInput = input;
     lastTime = now;
     return true;
   }
   else return false;
 }
 
+/* AutoTune(...)***************************************************************************
+   This function uses the Relay Method to tune the loop without operator intervention.
+   It determines the critical gain (Ku) and critical period (Tu) which is used in the
+   Ziegler-Nichols tuning rules to compute Kp, Ki and Kd.
+ ******************************************************************************************/
+void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 30) {
+  const byte outputStep = 2;                  // ±2 = span of 4
+  const byte hysteresis = 1;                  // ±1 = span of 2
+
+  static uint32_t  stepPrevTime, stepTime;
+  int peakHigh = 341, peakLow = 341;          // Why 341? Because its exacly 1/3 of the 10-bit
+  *myInput = 341;                             // ADC max and this perfectly matches the 8-bit
+  *mySetpoint = 341;                          // PWM value (85/255 = 1/3). We need 0% digital bias
+  *myOutput = 85;                             // for a symetrical waveform over the setpoint.
+
+  float Ku, Tu;
+  bool stepDir = true;
+
+  analogWrite(outputPin, 85);
+  if (Print == 1) Serial.print("Settling at 33% for 7 sec ");
+  for (int i = 0; i < 7; i++) {
+    delay(1000);
+    if (Print == 1) Serial.print(".");
+  }
+  if (Print == 1) Serial.println();
+  if (Print == 1) Serial.print("AutoTune:");
+
+  for (int i = 0; i < 16; i++) {                        // fill the rolling average
+    *myInput = analogReadAvg(inputPin);
+  }
+
+  timeout *= 1000;
+
+  do { //oscillate the output based on the input's relation to the setpoint
+    if (*myInput > *mySetpoint + hysteresis) {          // input passes +'ve hysteresis
+      *myOutput = 85 - outputStep;                      // step down
+      if (*myInput > peakHigh) peakHigh = *myInput;     // update peakHigh
+      if (!stepDir) {                                   // if previous direction was down
+        stepPrevTime = stepTime;                        // update step time variables
+        stepTime = micros();
+        stepDir = true;
+      }
+    }
+    else if (*myInput < *mySetpoint - hysteresis) {     // input passes -'ve hysteresis
+      *myOutput = 85 + outputStep;                      // step up
+      if (stepPrevTime && (*myInput < peakLow)) peakLow = *myInput; // update peakLow only if prev peakHigh timing > 0
+      stepDir = false;
+    }
+    *myInput = analogReadAvg(inputPin);                 // update input (rolling average)
+    analogWrite(outputPin, *myOutput);                  // update output
+    Ku = (float)(4 * outputStep * 2) / (float)(3.14159 * sqrt (sq (peakHigh - peakLow) - sq (hysteresis * 2))); // critical gain
+    Tu = (float)(stepTime - stepPrevTime) / 1000000.0;  // critical period
+    delay(2);                                           // allow some iteration time
+  } while ((isinf(Ku) || isnan(Ku)) && (millis() <= timeout));
+
+  if (tuningRule == 0) {   // Ziegler Nichols PID
+    kp = 0.6 * Ku;
+    ki = 1.2 * Ku / Tu;
+    kd = 0.075 * Ku * Tu;
+  } else {                 // Ziegler Nichols PI
+    kp = 0.45 * Ku;
+    ki = 0.54 * Ku / Tu;
+    kd = 0.0;
+  }
+
+  if (Print == 1) {
+    Serial.print("  Ku: "); Serial.print(Ku);
+    Serial.print("  Tu: "); Serial.print(Tu);
+    Serial.print("  Kp: "); Serial.print(kp);
+    Serial.print("  Ki: "); Serial.print(ki);
+    Serial.print("  Kd: "); Serial.println(kd);
+  }
+  SetTunings(kp, ki, kd);
+}
+
 /* SetTunings(....)************************************************************
    This function allows the controller's dynamic performance to be adjusted.
    it's called automatically from the constructor, but tunings can also
@@ -93,12 +164,12 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd, float POn = 1)
   pOn = POn;
   dispKp = Kp; dispKi = Ki; dispKd = Kd;
 
-  float SampleTimeSec = (float)SampleTimeUs / 1000000;
+  float SampleTimeSec = (float)sampleTimeUs / 1000000;
   kp = Kp;
   ki = Ki * SampleTimeSec;
   kd = Kd / SampleTimeSec;
-  kpi = kp * pOn + ki;
-  kpd = kp * (1 - pOn) + kd;
+  kpi = (kp * pOn) + ki;
+  kpd = (kp * (1 - pOn)) + kd;
 
   if (controllerDirection == REVERSE)
   {
@@ -120,16 +191,20 @@ void QuickPID::SetTunings(float Kp, float Ki, float Kd) {
  ******************************************************************************/
 void QuickPID::SetSampleTimeUs(uint32_t NewSampleTimeUs)
 {
-  if (NewSampleTimeUs > 0)
-  {
-    float ratio  = (float)NewSampleTimeUs / (float)SampleTimeUs;
+  if (NewSampleTimeUs > 0) {
+    float ratio  = (float)NewSampleTimeUs / (float)sampleTimeUs;
     ki *= ratio;
     kd /= ratio;
-
-    SampleTimeUs = NewSampleTimeUs;
+    sampleTimeUs = NewSampleTimeUs;
   }
 }
 
+int16_t QuickPID::Saturate(int16_t Out) {
+  if (Out > outMax) Out = outMax;
+  else if (Out < outMin) Out = outMin;
+  return Out;
+}
+
 /* SetOutputLimits(...)********************************************************
    The PID controller is designed to vary its output within a given range.
    By default this range is 0-255, the Arduino PWM range.
@@ -142,11 +217,8 @@ void QuickPID::SetOutputLimits(int16_t Min, int16_t Max)
 
   if (inAuto)
   {
-    if (*myOutput > outMax) *myOutput = outMax;
-    else if (*myOutput < outMin) *myOutput = outMin;
-
-    if (outputSum > outMax) outputSum = outMax;
-    else if (outputSum < outMin) outputSum = outMin;
+    *myOutput = QuickPID::Saturate(*myOutput);
+    outputSum = QuickPID::Saturate(outputSum);
   }
 }
 
@@ -173,8 +245,7 @@ void QuickPID::Initialize()
 {
   outputSum = *myOutput;
   lastInput = *myInput;
-  if (outputSum > outMax) outputSum = outMax;
-  else if (outputSum < outMin) outputSum = outMin;
+  outputSum = QuickPID::Saturate(outputSum);
 }
 
 /* SetControllerDirection(...)*************************************************
@@ -243,3 +314,17 @@ int QuickPID::analogReadFast(int ADCpin) {
   return analogRead(ADCpin);
 # endif
 }
+
+int QuickPID::analogReadAvg(int ADCpin)
+{
+  static int arrDat[16];
+  static int dat;
+  static int pos;
+  static long sum;
+  dat = QuickPID::analogReadFast(ADCpin);
+  pos++;
+  if (pos >= 16) pos = 0;
+  sum = sum - arrDat[pos] + dat;
+  arrDat[pos] = dat;
+  return sum / 16;
+}
diff --git a/QuickPID.h b/QuickPID.h
@@ -7,15 +7,14 @@ class QuickPID
   public:
 
     //Constants used in some of the functions below
-#define AUTOMATIC	1
-#define MANUAL	0
+#define AUTOMATIC 1
+#define MANUAL  0
 #define DIRECT  0
 #define REVERSE 1
 
 #define FL_FX(a) (int32_t)(a*256.0)  // float to fixed point
 #define FX_MUL(a,b) ((a*b)>>8)       // fixed point multiply
 
-
     // commonly used functions ************************************************************************************
 
     // Constructor. Links the PID to Input, Output, Setpoint and initial Tuning Parameters.
@@ -31,8 +30,13 @@ class QuickPID
     // can be set using SetMode and SetSampleTime respectively.
     bool Compute();
 
-    // Clamps the output to a specific range. 0-255 by default, but it's likely the user will want to change this
-    // depending on the application.
+    // Automatically determines and sets the tuning parameters Kp, Ki and Kd. Use this in the setup loop.
+    void AutoTune(int, int, int, int, uint32_t);
+
+    // Clamps the output to its pre-determined limits.
+    int16_t Saturate(int16_t);
+
+    // Sets and clamps the output to a specific range (0-255 by default).
     void SetOutputLimits(int16_t, int16_t);
 
     // available but not commonly used functions ******************************************************************
@@ -61,6 +65,7 @@ class QuickPID
 
     // Utility functions ******************************************************************************************
     int analogReadFast(int);
+    int analogReadAvg(int);
 
   private:
     void Initialize();
@@ -82,7 +87,7 @@ class QuickPID
     int16_t *myOutput;     // hard link between the variables and the PID, freeing the user from having
     int16_t *mySetpoint;   // to constantly tell us what these values are. With pointers we'll just know.
 
-    uint32_t SampleTimeUs, lastTime;
+    uint32_t sampleTimeUs, lastTime;
     int16_t outMin, outMax, error;
     int16_t lastInput, outputSum;
     bool inAuto;

diff --git a/README.md b/README.md
@@ -1,6 +1,6 @@
 # QuickPID
 
-A fast hybrid fixed-point and floating-point PID controller for Arduino. 
+QuickPID is a fast fixed/floating point implementation of the Arduino PID library with built-in AutoTune function. This controller can automatically determine and set parameters (Kp, Ki, Kd). The POn setting controls the mix of Proportional on Errror to Proportional on Measurement and can be used to set the desired amount of overshoot.
 
 ### About
 
@@ -11,7 +11,8 @@ This PID controller provides a faster *read-compute-write* cycle than alternativ
 Development began with a fork of the Arduino PID Library. Some modifications and new features have been added as follows:
 
 - Quicker hybrid fixed/floating point math in compute function
-- `POn` parameter now controls the setpoint weighting of Proportional on Error and Proportional on Measurement
+- Built-in `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. Also determines critical gain `Ku` and critical period `Tu` of the control system
+- `POn` parameter now controls the setpoint weighting and mix of Proportional on Error to Proportional on Measurement
 - Reorganized and more efficient PID algorithm
 - micros() timing resolution
 - Faster analog read function
@@ -30,15 +31,15 @@ Development began with a fork of the Arduino PID Library. Some modifications and
 
 ### Self Test Example (RC Filter):
 
-[This example](https://github.com/Dlloydev/QuickPID/wiki/QuickPID_RC_Filter) allows you to experiment with the four tuning parameters.
+[This example](https://github.com/Dlloydev/QuickPID/wiki/AutoTune_RC_Filter) allows you to experiment with the AutoTune and POn control on an RC filter.
 
 ### Simplified PID Algorithm
 
 ```c++
 outputSum += (kpi * error) - (kpd * dInput);
 ```
 
-The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required The pOn variable controls the setpoint weighting of Proportional on Error and Proportional on Measurement. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated as follows:
+The new `kpi` and `kpd` parameters are calculated in the `SetTunings()` function. This results in a simple and fast algorithm with only two multiply operations required. The pOn variable controls the setpoint weighting of Proportional on Error and Proportional on Measurement. The gains for `error` (`kpi`) and measurement `dInput` (`kpd`) are calculated as follows:
 
 ```c++
  kpi = kp * pOn + ki;
@@ -77,6 +78,33 @@ bool QuickPID::Compute()
 
 This function contains the PID algorithm and it should be called once every loop(). Most of the time it will just return false without doing anything. However, at a  frequency specified by `SetSampleTime` it will calculate a new Output and return true.
 
+#### AutoTune
+
+```c++
+void QuickPID::AutoTune(int inputPin, int outputPin, int tuningRule, int Print = 0, uint32_t timeout = 30)
+```
+
+The `AutoTune()` function automatically determines and sets `Kp`, `Ki` and `Kd`. It also determines the critical gain `Ku` and critical period `Tu` of the control system. 
+
+`int tuningRule = 0; // PID(0), PI(1)`
+
+Selects the appropriate Ziegler–Nichols tuning rule for the PID or PI type controller.
+
+| Controller | Kp          | Ki               | Kd                |
+| ---------- | ----------- | ---------------- | ----------------- |
+| PI         | `0.45 * Ku` | `0.54 * Ku / Tu` | `0`               |
+| PID        | `0.6 * Ku`  | `1.2 * Ku / Tu`  | `0.075 * Ku * Tu` |
+
+`int Print = 0; // on(1), off(0)`
+
+When using Serial Monitor, turn on serial print output to view the AutoTune status and results. When using the Serial Plotter, turn off the AutoTune print output to prevent plot labels from being overwritten.
+
+`uint32_t timeout = 30`
+
+Sets the AutoTune timeout where the default is 30 seconds.
+
+For more inormation, see [QuickPID AutoTune.](https://github.com/Dlloydev/QuickPID/wiki/AutoTune)
+
 #### SetTunings
 
 ```c++
@@ -150,21 +178,16 @@ These functions query the internal state of the PID. They're here for display pu
 int QuickPID::analogReadFast(int ADCpin)
 ```
 
-A faster configuration of `analogRead()`where a preset of 32 is used. Works with the following defines:
-
-`__AVR_ATmega328P__`
+A faster configuration of `analogRead()`where a preset of 32 is used.  If the architecture definition isn't found, normal `analogRead()`is used to return a value.
 
-`__AVR_ATtiny_Zero_One__`
-
-`__AVR_ATmega_Zero__`
-
-`__AVR_DA__`
+### Change Log
 
- If the definition isn't found, normal `analogRead()`is used to return a value.
+#### Version 2.1.0 (latest)
 
-### Change Log
+- Added AutoTune function and documentation
+- Added AutoTune_RC_Filter example and documentation
 
-#### Version 2.0.5 (latest)
+#### Version 2.0.5
 
 - Added MIT license text file
 - POn defaults to 1