From 936204e07467e4dc8a65636e3f8b3f519d6fc30e Mon Sep 17 00:00:00 2001 From: Dlloydev Date: Sat, 22 Jan 2022 09:50:41 -0500 Subject: [PATCH] Update - Added emergency stop function where the test is stopped and output cleared to 0 if the input exceeds 10% (default) over the input span. This function allows the user to adjust the eStop limit. - `inOut` switch case renamed to `sample` as it now is active once per sample period - Updated `Autotune_PID_Digital_Out` example for software PWM relay control - Updated examples and documentation --- README.md | 13 ++- .../Autotune_PID_Digital_Out.ino | 108 ++++++++++-------- examples/Autotune_PID_v1/Autotune_PID_v1.ino | 57 +++++---- .../Autotune_Plotter/Autotune_Plotter.ino | 49 ++++---- .../Autotune_QuickPID/Autotune_QuickPID.ino | 45 ++++---- examples/Get_All_Tunings/Get_All_Tunings.ino | 21 ++-- keywords.txt | 3 +- library.json | 2 +- library.properties | 2 +- src/sTune.cpp | 38 ++++-- src/sTune.h | 7 +- 11 files changed, 185 insertions(+), 160 deletions(-) diff --git a/README.md b/README.md index 08517ac..4dd507c 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ This tuning method determines the process gain, dead time, time constant and mor See [**WiKi**](https://github.com/Dlloydev/sTune/wiki) for test results and more: -- [Autotune Digital Output](https://github.com/Dlloydev/sTune/wiki/Autotune_PID_Digital_Out_Reference) ![image](https://user-images.githubusercontent.com/63488701/149647496-f4459516-a483-469e-af1e-90a54fbd909e.png) +- [Autotune Digital Output](https://github.com/Dlloydev/sTune/wiki/Autotune_PID_Digital_Out_Reference) - [Get_All_Tunings](https://github.com/Dlloydev/sTune/wiki/Get_All_Tunings) - [plotter function reference](https://github.com/Dlloydev/sTune/wiki/plotter-function-reference) @@ -102,11 +102,11 @@ sTune(float *input, float *output, TuningRule tuningRule, Action action, SerialM #### Instantiate sTune ```c++ -sTune tuner = sTune(&Input, &Output, tuner.Mixed_PID, tuner.directIP, tuner.printALL); -/* ZN_PID directIP serialOFF - DampedOsc_PID direct5T printALL - NoOvershoot_PID reverseIP printSUMMARY - CohenCoon_PID reverse5T printDEBUG +sTune tuner = sTune(&Input, &Output, tuner.ZN_PID, tuner.directIP, tuner.printALL); +/* ZN_PID directIP serialOFF + DampedOsc_PID direct5T printALL + NoOvershoot_PID reverseIP printSUMMARY + CohenCoon_PID reverse5T printDEBUG Mixed_PID ZN_PI DampedOsc_PI @@ -128,6 +128,7 @@ This function applies the sTune test settings. #### Set Functions ```c++ +void SetEmergencyStop(float e_Stop); void SetControllerAction(Action Action); void SetSerialMode(SerialMode SerialMode); void SetTuningMethod(TuningMethod TuningMethod); diff --git a/examples/Autotune_PID_Digital_Out/Autotune_PID_Digital_Out.ino b/examples/Autotune_PID_Digital_Out/Autotune_PID_Digital_Out.ino index c93d0a4..12bdb51 100644 --- a/examples/Autotune_PID_Digital_Out/Autotune_PID_Digital_Out.ino +++ b/examples/Autotune_PID_Digital_Out/Autotune_PID_Digital_Out.ino @@ -1,6 +1,16 @@ /**************************************************************************** - Autotune QuickPID Digital Output Example - https://github.com/Dlloydev/sTune/wiki/Autotune_PID_Digital_Out_Reference + Autotune QuickPID Digital Output Example + https://github.com/Dlloydev/sTune/wiki/Autotune_PID_Digital_Out_Reference + + The output is a digital pin controlling a mechanical relay, SSR, MOSFET or + other device. To interface the PID output to the digital pin, we use + software PWM. The PID compute rate controls the output update rate. All + transitions are debounced and there's only one call to digitalWrite() + per transition. + + This sketch runs sTune then applies the tunings to QuickPID. Open the + serial plotter to view the input response through the complete tuning and + PID control process. ****************************************************************************/ #include @@ -8,27 +18,28 @@ // pins const uint8_t inputPin = 0; -const uint8_t outputPin = 3; -const uint8_t ledPin = 9; - -// test setup -uint32_t testTimeSec = 600; // testTimeSec / samples = sample interval -const uint16_t samples = 300; +const uint8_t relayPin = 3; +// user settings (sTune) uint32_t settleTimeSec = 15; +uint32_t testTimeSec = 300; +const uint16_t samples = 300; const float inputSpan = 80; -const float outputSpan = 2000; // window size for sTune and PID -const float minSpan = 50; +const float outputSpan = (testTimeSec / samples) * 1000; float outputStart = 0; -float outputStep = 200; -bool clearPidOutput = false; // false: "on the fly" testing, true: PID starts at 0 output +float outputStep = 100; -// temperature -const float mvResolution = 3300 / 1024.0f; -const float bias = 50; +// user settings (PID) +float Setpoint = 30; +const float windowSize = 5000; +const byte debounce = 50; + +// status +unsigned long windowStartTime, nextSwitchTime, msNow; +boolean relayStatus = false; // variables -float Input, Output, Setpoint = 30, Kp, Ki, Kd; +float Input, Output, Kp, Ki, Kd; QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.pMode::pOnError, @@ -36,7 +47,7 @@ QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.iAwMode::iAwClamp, myPID.Action::direct); -sTune tuner = sTune(&Input, &Output, tuner.ZN_PID, tuner.directIP, tuner.printALL); +sTune tuner = sTune(&Input, &Output, tuner.ZN_PID, tuner.directIP, tuner.printOFF); /* ZN_PID directIP printOFF DampedOsc_PID direct5T printALL NoOvershoot_PID reverseIP printSUMMARY @@ -49,49 +60,54 @@ sTune tuner = sTune(&Input, &Output, tuner.ZN_PID, tuner.directIP, tuner.printAL Mixed_PI */ void setup() { - pinMode(outputPin, OUTPUT); - pinMode(ledPin, OUTPUT); - digitalWrite(outputPin, LOW); - analogReference(EXTERNAL); // used by TCLab Serial.begin(115200); + pinMode(relayPin, OUTPUT); + pinMode(LED_BUILTIN, OUTPUT); tuner.Configure(inputSpan, outputSpan, outputStart, outputStep, testTimeSec, settleTimeSec, samples); } void loop() { - softPwmOutput(); switch (tuner.Run()) { - case tuner.inOut: // active while sTune is testing - Input = (analogRead(inputPin) / mvResolution) - bias; + case tuner.sample: // active once per sample during test + windowStartTime = msNow; + Input = analogRead(inputPin); + tuner.plotter(Setpoint, 0.05, 5, 1); // scaled output, every 5th sample, averaged input break; - case tuner.tunings: // active just once when sTune is done - tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune - myPID.SetOutputLimits(0, outputSpan); // PID output spans from 0 to the full window size - myPID.SetSampleTimeUs(outputSpan * 1000); // PID sample rate matches sTune - if (clearPidOutput) Output = 0; - myPID.SetMode(myPID.Control::automatic); // the PID is turned on (automatic) - myPID.SetTunings(Kp, Ki, Kd); // update PID with the new tunings + + case tuner.tunings: // active just once when sTune is done + tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune + myPID.SetOutputLimits(0, windowSize); + myPID.SetSampleTimeUs(windowSize * 1000); + // Output = 0; // optional output preset value + myPID.SetMode(myPID.Control::automatic); // the PID is turned on + myPID.SetTunings(Kp, Ki, Kd); // update PID with the new tunings break; - case tuner.runPid: // active once per sample period after case "tunings" - Input = (analogRead(inputPin) / mvResolution) - bias; - myPID.Compute(); - tuner.plotter(Setpoint, 0.1, 5, 1); // scaled output, every 5th sample, averaged input + + case tuner.runPid: // active once per sample after tunings + tuner.plotter(Setpoint, 0.05, 5, 1); break; } // put your main code here, to run repeatedly + msNow = millis(); + softPwmOutput(); + Input = analogRead(inputPin); + if (myPID.Compute()) windowStartTime = msNow; } void softPwmOutput() { - static uint32_t tPrev; - uint32_t tNow = millis(); - uint32_t tElapsed = (tNow - tPrev); - if (tElapsed >= outputSpan) tPrev = tNow; - if (tElapsed > minSpan && tElapsed < outputSpan - minSpan) { // in range? - if (tElapsed <= Output) { - digitalWrite(outputPin, HIGH); - digitalWrite(ledPin, HIGH); - } else { - digitalWrite(outputPin, LOW); - digitalWrite(ledPin, LOW); + if (!relayStatus && Output > (msNow - windowStartTime)) { + if (msNow > nextSwitchTime) { + nextSwitchTime = msNow + debounce; + relayStatus = true; + digitalWrite(relayPin, HIGH); + digitalWrite(LED_BUILTIN, HIGH); + } + } else if (relayStatus && Output < (msNow - windowStartTime)) { + if (msNow > nextSwitchTime) { + nextSwitchTime = msNow + debounce; + relayStatus = false; + digitalWrite(relayPin, LOW); + digitalWrite(LED_BUILTIN, LOW); } } } diff --git a/examples/Autotune_PID_v1/Autotune_PID_v1.ino b/examples/Autotune_PID_v1/Autotune_PID_v1.ino index bf91838..caf7938 100644 --- a/examples/Autotune_PID_v1/Autotune_PID_v1.ino +++ b/examples/Autotune_PID_v1/Autotune_PID_v1.ino @@ -1,6 +1,8 @@ /******************************************************************** - Autotune PID_v1 Example (using Temperature Control Lab) - http://apmonitor.com/pdc/index.php/Main/ArduinoTemperatureControl + Autotune PID_v1 Example + + This sketch runs sTune then applies the tunings to PID_v1. Open + the serial printer to view the test progress and results. ********************************************************************/ #include @@ -10,27 +12,25 @@ const uint8_t inputPin = 0; const uint8_t outputPin = 3; -// test setup +// user settings (sTune) +uint32_t settleTimeSec = 15; uint32_t testTimeSec = 300; const uint16_t samples = 500; -uint32_t settleTimeSec = 10; const float inputSpan = 80; const float outputSpan = 255; float outputStart = 0; float outputStep = 25; -bool clearPidOutput = false; // false: "on the fly" testing, true: PID starts at 0 output -// temperature -const float mvResolution = 3300 / 1024.0f; -const float bias = 50; +// user settings (PID) +double Setpoint = 30; -// test variables -double Input = 0, Output = 0, Setpoint = 30; // myPID -float input = 0, output = 0, kp = 0, ki = 0, kd = 0; // tuner +// variables +double Input, Output; // PID +float input, output, kp, ki, kd; // sTune PID myPID(&Input, &Output, &Setpoint, 0, 0, 0, DIRECT); -sTune tuner = sTune(&input, &output, tuner.Mixed_PID, tuner.directIP, tuner.printALL); +sTune tuner = sTune(&input, &output, tuner.ZN_PID, tuner.directIP, tuner.printALL); /* ZN_PID directIP serialOFF DampedOsc_PID direct5T printALL NoOvershoot_PID reverseIP printSUMMARY @@ -43,7 +43,6 @@ sTune tuner = sTune(&input, &output, tuner.Mixed_PID, tuner.directIP, tuner.prin Mixed_PI */ void setup() { - analogReference(EXTERNAL); // used by TCLab Serial.begin(115200); analogWrite(outputPin, outputStart); tuner.Configure(inputSpan, outputSpan, outputStart, outputStep, testTimeSec, settleTimeSec, samples); @@ -51,25 +50,25 @@ void setup() { void loop() { - switch (tuner.Run()) { // active while sTune is testing - case tuner.inOut: - input = (analogRead(inputPin) / mvResolution) - bias; + switch (tuner.Run()) { + case tuner.sample: // active once per sample during test + Input = analogRead(inputPin); analogWrite(outputPin, output); break; - case tuner.tunings: // active just once when sTune is done - tuner.GetAutoTunings(&kp, &ki, &kd); // sketch variables updated by sTune - myPID.SetSampleTime((testTimeSec * 1000) / samples); // PID sample rate - if (clearPidOutput) Output = 0; - myPID.SetMode(AUTOMATIC); // the PID is turned on (automatic) - myPID.SetTunings(kp, ki, kd); // update PID with the new tunings + case tuner.tunings: // active just once when sTune is done + tuner.GetAutoTunings(&kp, &ki, &kd); // sketch variables updated by sTune + myPID.SetSampleTime((testTimeSec * 1000) / samples); // PID sample rate + // Output = 0; // optional output preset value + myPID.SetMode(AUTOMATIC); // the PID is turned on + myPID.SetTunings(kp, ki, kd); // update PID with the new tunings break; - case tuner.runPid: // this case runs once per sample period after case "tunings" - Input = (analogRead(inputPin) / mvResolution) - bias; - myPID.Compute(); - analogWrite(outputPin, Output); - break; - } - // put your main code here, to run repeatedly + case tuner.runPid: // active once per sample after tunings + Input = analogRead(inputPin); + myPID.Compute(); + analogWrite(outputPin, Output); + break; + } +// put your main code here, to run repeatedly } diff --git a/examples/Autotune_Plotter/Autotune_Plotter.ino b/examples/Autotune_Plotter/Autotune_Plotter.ino index a64b09a..91028c8 100644 --- a/examples/Autotune_Plotter/Autotune_Plotter.ino +++ b/examples/Autotune_Plotter/Autotune_Plotter.ino @@ -1,6 +1,9 @@ /******************************************************************** - Autotune Plotter Example (using Temperature Control Lab) - http://apmonitor.com/pdc/index.php/Main/ArduinoTemperatureControl + Autotune Plotter Example + + This sketch runs sTune then applies the tunings to QuickPID. Open + the serial plotter to view the input response through the complete + tuning and PID control process. ********************************************************************/ #include @@ -10,22 +13,20 @@ const uint8_t inputPin = 0; const uint8_t outputPin = 3; -// test setup +// user settings (sTune) +uint32_t settleTimeSec = 10; uint32_t testTimeSec = 300; const uint16_t samples = 500; -uint32_t settleTimeSec = 10; const float inputSpan = 80; const float outputSpan = 255; float outputStart = 0; float outputStep = 25; -bool clearPidOutput = false; // false: "on the fly" testing, true: PID starts at 0 output -// temperature -const float mvResolution = 3300 / 1024.0f; -const float bias = 50; +// user settings (PID) +float Setpoint = 30; -// test variables -float Input = 0, Output = 0, Setpoint = 30, Kp = 0, Ki = 0, Kd = 0; +// variables +float Input, Output, Kp, Ki, Kd; QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.pMode::pOnError, @@ -33,7 +34,7 @@ QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.iAwMode::iAwClamp, myPID.Action::direct); -sTune tuner = sTune(&Input, &Output, tuner.Mixed_PID, tuner.directIP, tuner.printALL); +sTune tuner = sTune(&Input, &Output, tuner.ZN_PID, tuner.directIP, tuner.printOFF); /* ZN_PID directIP serialOFF DampedOsc_PID direct5T printALL NoOvershoot_PID reverseIP printSUMMARY @@ -46,32 +47,32 @@ sTune tuner = sTune(&Input, &Output, tuner.Mixed_PID, tuner.directIP, tuner.prin Mixed_PI */ void setup() { - analogReference(EXTERNAL); // used by TCLab Serial.begin(115200); analogWrite(outputPin, outputStart); tuner.Configure(inputSpan, outputSpan, outputStart, outputStep, testTimeSec, settleTimeSec, samples); } void loop() { - switch (tuner.Run()) { // active while sTune is testing - case tuner.inOut: - Input = (analogRead(inputPin) / mvResolution) - bias; + switch (tuner.Run()) { + case tuner.sample: // active once per sample during test + Input = analogRead(inputPin); analogWrite(outputPin, Output); + tuner.plotter(Setpoint, 0.5, 6, 1); // output scaled 0.5, plot every 6th sample, averaged input break; - case tuner.tunings: // active just once when sTune is done - tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune - myPID.SetSampleTimeUs((testTimeSec * 1000000) / samples); // PID sample rate (same as sTune) - if (clearPidOutput) Output = 0; - myPID.SetMode(myPID.Control::automatic); // the PID is turned on (automatic) - myPID.SetTunings(Kp, Ki, Kd); // update PID with the new tunings + case tuner.tunings: // active just once when sTune is done + tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune + myPID.SetSampleTimeUs((testTimeSec / samples) * 1000000); + // Output = 0; // optional output preset value + myPID.SetMode(myPID.Control::automatic); // the PID is turned on + myPID.SetTunings(Kp, Ki, Kd); // update PID with the new tunings break; - case tuner.runPid: // this case runs once per sample period after case "tunings" - Input = (analogRead(inputPin) / mvResolution) - bias; + case tuner.runPid: // active once per sample after tunings + Input = analogRead(inputPin); myPID.Compute(); analogWrite(outputPin, Output); - tuner.plotter(Setpoint, 0.5, 6, 1); // plot output scaled 0.5, plot every 6th sample, plot averaged input + tuner.plotter(Setpoint, 0.5, 6, 1); break; } // put your main code here, to run repeatedly diff --git a/examples/Autotune_QuickPID/Autotune_QuickPID.ino b/examples/Autotune_QuickPID/Autotune_QuickPID.ino index 1dd80d4..7dc7337 100644 --- a/examples/Autotune_QuickPID/Autotune_QuickPID.ino +++ b/examples/Autotune_QuickPID/Autotune_QuickPID.ino @@ -1,6 +1,8 @@ /******************************************************************** - Autotune QuickPID Example (using Temperature Control Lab) - http://apmonitor.com/pdc/index.php/Main/ArduinoTemperatureControl + Autotune QuickPID Example + + This sketch runs sTune then applies the tunings to QuickPID. Open + the serial printer to view the test progress and results. ********************************************************************/ #include @@ -10,22 +12,20 @@ const uint8_t inputPin = 0; const uint8_t outputPin = 3; -// test setup +// user settings (sTune) +uint32_t settleTimeSec = 15; uint32_t testTimeSec = 300; const uint16_t samples = 500; -uint32_t settleTimeSec = 10; const float inputSpan = 80; const float outputSpan = 255; float outputStart = 0; float outputStep = 25; -bool clearPidOutput = false; // false: "on the fly" testing, true: PID starts at 0 output -// temperature -const float mvResolution = 3300 / 1024.0f; -const float bias = 50; +// user settings (PID) +float Setpoint = 30; -// test variables -float Input = 0, Output = 0, Setpoint = 30, Kp = 0, Ki = 0, Kd = 0; +// variables +float Input, Output, Kp, Ki, Kd; QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.pMode::pOnError, @@ -33,7 +33,7 @@ QuickPID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, myPID.iAwMode::iAwClamp, myPID.Action::direct); -sTune tuner = sTune(&Input, &Output, tuner.Mixed_PID, tuner.directIP, tuner.printALL); +sTune tuner = sTune(&Input, &Output, tuner.ZN_PID, tuner.directIP, tuner.printALL); /* ZN_PID directIP serialOFF DampedOsc_PID direct5T printALL NoOvershoot_PID reverseIP printSUMMARY @@ -46,29 +46,28 @@ sTune tuner = sTune(&Input, &Output, tuner.Mixed_PID, tuner.directIP, tuner.prin Mixed_PI */ void setup() { - analogReference(EXTERNAL); // used by TCLab Serial.begin(115200); analogWrite(outputPin, outputStart); tuner.Configure(inputSpan, outputSpan, outputStart, outputStep, testTimeSec, settleTimeSec, samples); } void loop() { - switch (tuner.Run()) { // active while sTune is testing - case tuner.inOut: - Input = (analogRead(inputPin) / mvResolution) - bias; + switch (tuner.Run()) { + case tuner.sample: // active once per sample during test + Input = analogRead(inputPin); analogWrite(outputPin, Output); break; - case tuner.tunings: // active just once when sTune is done - tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune - myPID.SetSampleTimeUs((testTimeSec * 1000000) / samples); // PID sample rate (same as sTune) - if (clearPidOutput) Output = 0; - myPID.SetMode(myPID.Control::automatic); // the PID is turned on (automatic) - myPID.SetTunings(Kp, Ki, Kd); // update PID with the new tunings + case tuner.tunings: // active just once when sTune is done + tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune + myPID.SetSampleTimeUs((testTimeSec * 1000000) / samples); // PID sample rate + // Output = 0; // optional output preset value + myPID.SetMode(myPID.Control::automatic); // the PID is turned on + myPID.SetTunings(Kp, Ki, Kd); // update PID with the new tunings break; - case tuner.runPid: // this case runs once per sample period after case "tunings" - Input = (analogRead(inputPin) / mvResolution) - bias; + case tuner.runPid: // active once per sample after tunings + Input = analogRead(inputPin); myPID.Compute(); analogWrite(outputPin, Output); break; diff --git a/examples/Get_All_Tunings/Get_All_Tunings.ino b/examples/Get_All_Tunings/Get_All_Tunings.ino index 9e7bfb0..bf23d2f 100644 --- a/examples/Get_All_Tunings/Get_All_Tunings.ino +++ b/examples/Get_All_Tunings/Get_All_Tunings.ino @@ -1,6 +1,5 @@ /******************************************************************** - sTune Get All Tunings Example (using Temperature Control Lab) - http://apmonitor.com/pdc/index.php/Main/ArduinoTemperatureControl + sTune Get All Tunings Example ********************************************************************/ #include @@ -9,23 +8,19 @@ const uint8_t inputPin = 0; const uint8_t outputPin = 3; -// test setup +// user settings +uint32_t settleTimeSec = 15; uint32_t testTimeSec = 300; const uint16_t samples = 500; -uint32_t settleTimeSec = 10; const float inputSpan = 80; const float outputSpan = 255; float outputStart = 0; float outputStep = 25; -// temperature -const float mvResolution = 3300 / 1024.0f; -const float bias = 50; - // test variables -float Input = 0, Output = 0; +float Input, Output; -sTune tuner = sTune(&Input, &Output, tuner.Mixed_PID, tuner.directIP, tuner.printALL); +sTune tuner = sTune(&Input, &Output, tuner.ZN_PID, tuner.directIP, tuner.printALL); /* ZN_PID directIP serialOFF DampedOsc_PID direct5T printALL NoOvershoot_PID reverseIP printSUMMARY @@ -38,9 +33,7 @@ sTune tuner = sTune(&Input, &Output, tuner.Mixed_PID, tuner.directIP, tuner.prin Mixed_PI */ void setup() { - analogReference(EXTERNAL); // used by TCLab Serial.begin(115200); - delay(5000); analogWrite(outputPin, outputStart); tuner.Configure(inputSpan, outputSpan, outputStart, outputStep, testTimeSec, settleTimeSec, samples); } @@ -48,8 +41,8 @@ void setup() { void loop() { switch (tuner.Run()) { - case tuner.inOut: - Input = (analogRead(inputPin) / mvResolution) - bias; + case tuner.sample: + Input = analogRead(inputPin); analogWrite(outputPin, Output); break; diff --git a/keywords.txt b/keywords.txt index 5585cd1..42b55f4 100644 --- a/keywords.txt +++ b/keywords.txt @@ -20,6 +20,7 @@ printTestRun KEYWORD2 printResults KEYWORD2 printPidTuner KEYWORD2 plotter KEYWORD2 +SetEmergencyStop KEYWORD2 SetControllerAction KEYWORD2 SetSerialMode KEYWORD2 SetTuningMethod KEYWORD2 @@ -53,7 +54,7 @@ printOFF LITERAL1 printALL LITERAL1 printSUMMARY LITERAL1 printDEBUG LITERAL1 -inOut LITERAL1 +sample LITERAL1 test LITERAL1 tunings LITERAL1 runPid LITERAL1 diff --git a/library.json b/library.json index be861a9..c648c09 100644 --- a/library.json +++ b/library.json @@ -1,6 +1,6 @@ { "name": "sTune", - "version": "2.1.1", + "version": "2.2.0", "description": "Open loop PID autotuner using a novel s-curve inflection point test method. Tuning parameters are determined in about ½Tau on a first-order system with time delay. Full 5Tau testing and multiple serial output options are provided.", "keywords": "PID, autotune, autotuner, tuner, tune, stune, inflection, step", "repository": diff --git a/library.properties b/library.properties index a92ad1a..839d8c5 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=sTune -version=2.1.1 +version=2.2.0 author=David Lloyd maintainer=David Lloyd sentence=Open loop PID autotuner using a novel s-curve inflection point test method. diff --git a/src/sTune.cpp b/src/sTune.cpp index 7301265..3d803fe 100644 --- a/src/sTune.cpp +++ b/src/sTune.cpp @@ -1,5 +1,5 @@ /**************************************************************************************** - sTune Library for Arduino - Version 2.1.1 + sTune Library for Arduino - Version 2.2.0 by dlloydev https://github.com/Dlloydev/sTune Licensed under the MIT License. @@ -35,7 +35,7 @@ sTune::sTune(float *input, float *output, TuningMethod tuningMethod, Action acti } void sTune::Reset() { - _tunerStatus = inOut; + _tunerStatus = test; *_output = _outputStart; usPrev = micros(); settlePrev = usPrev; @@ -67,13 +67,13 @@ void sTune::Configure(const float inputSpan, const float outputSpan, float outpu uint32_t testTimeSec, uint32_t settleTimeSec, const uint16_t samples) { sTune::Reset(); _inputSpan = inputSpan; + eStop = inputSpan * 1.1f; _outputSpan = outputSpan; _outputStart = outputStart; _outputStep = outputStep; _testTimeSec = testTimeSec; _settleTimeSec = settleTimeSec; _samples = samples; - _tunerStatus = inOut; _bufferSize = (uint16_t)(_samples * 0.1); _samplePeriodUs = (float)(_testTimeSec * 1000000.0f) / _samples; _tangentPeriodUs = _samplePeriodUs * (_bufferSize - 1); @@ -90,12 +90,19 @@ uint8_t sTune::Run() { switch (_tunerStatus) { - case inOut: + case sample: _tunerStatus = test; return test; break; case test: // run inflection point test method + if (pvInst > eStop && !eStopAbort) { + sTune::Reset(); + sampleCount = _samples + 1; + eStopAbort = 1; + Serial.println(F(" ABORT: pvInst > eStop")); + break; + } if (settleElapsed >= _settlePeriodUs) { // if settling period has expired if (sampleCount == 8) *_output = _outputStep; // provides additional period using _outputStart if (usElapsed >= _samplePeriodUs) { // ready to process a sample @@ -199,9 +206,12 @@ uint8_t sTune::Run() { pvTangentPrev = pvTangent; } else _tunerStatus = tunings; sampleCount++; + _tunerStatus = sample; + return sample; } + } else { // settling - if (usElapsed >= _samplePeriodUs) { + if (usElapsed >= _samplePeriodUs && !eStopAbort) { *_output = _outputStart; usPrev = usNow; pvInst = *_input; @@ -210,17 +220,15 @@ uint8_t sTune::Run() { Serial.print(F(" pvInst: ")); Serial.print(pvInst, 4); Serial.println(F(" Settling ⤳⤳⤳⤳")); } + _tunerStatus = sample; + return sample; } - _tunerStatus = inOut; - return inOut; } - _tunerStatus = inOut; - return inOut; break; case tunings: - _tunerStatus = runPid; - return runPid; + _tunerStatus = timerPid; + return timerPid; break; case runPid: @@ -247,6 +255,10 @@ uint8_t sTune::Run() { return timerPid; } +void sTune::SetEmergencyStop(float e_Stop) { + eStop = e_Stop; +} + void sTune::SetControllerAction(Action Action) { _action = Action; } @@ -277,7 +289,8 @@ void sTune::plotter(float setpoint, float outputScale, uint8_t everyNth, bool av plotCount = 1; Serial.print(F("Setpoint:")); Serial.print(setpoint); Serial.print(F(", ")); Serial.print(F("Input:")); Serial.print((average) ? pvAvg : pvInst); Serial.print(F(", ")); - Serial.print(F("Output:")); Serial.print(*_output * outputScale); Serial.println(F(",")); + Serial.print(F("Output:")); Serial.print(*_output * outputScale); Serial.print(F(",")); + Serial.println(); } else plotCount++; } @@ -297,6 +310,7 @@ void sTune::printTestRun() { Serial.print(F(" ipCount: ")); Serial.print(ipCount); } Serial.print(F(" pvTangent: ")); Serial.print(pvTangent, 4); + if (pvInst > 0.9f * eStop) Serial.print(F(" ⚠")); if (pvTangent - pvTangentPrev > 0 + epsilon) Serial.println(F(" ↗")); else if (pvTangent - pvTangentPrev < 0 - epsilon) Serial.println(F(" ↘")); else Serial.println(F(" →")); diff --git a/src/sTune.h b/src/sTune.h index bd6d9b9..ece369d 100644 --- a/src/sTune.h +++ b/src/sTune.h @@ -8,7 +8,7 @@ class sTune { // Inflection Point Autotuner enum Action : uint8_t {directIP, direct5T, reverseIP, reverse5T}; enum SerialMode : uint8_t {printOFF, printALL, printSUMMARY, printDEBUG}; - enum TunerStatus : uint8_t {inOut, test, tunings, runPid, timerPid}; + enum TunerStatus : uint8_t {sample, test, tunings, runPid, timerPid}; enum TuningMethod : uint8_t { ZN_PID, DampedOsc_PID, NoOvershoot_PID, CohenCoon_PID, Mixed_PID, ZN_PI, DampedOsc_PI, NoOvershoot_PI, CohenCoon_PI, Mixed_PI }; @@ -26,6 +26,7 @@ class sTune { // Inflection Point Autotuner void plotter(float setpoint, float outputScale = 1, uint8_t everyNth = 1, bool average = false); // Set functions + void SetEmergencyStop(float e_Stop); void SetControllerAction(Action Action); void SetSerialMode(SerialMode SerialMode); void SetTuningMethod(TuningMethod TuningMethod); @@ -53,10 +54,10 @@ class sTune { // Inflection Point Autotuner float *_input, *_output, _settlePeriodUs, _samplePeriodUs, _tangentPeriodUs; float _inputSpan, _outputSpan, _outputStart, _outputStep; - float pvInst, pvAvg, pvIp, pvMax, pvPk, pvRes, slopeIp, pvTangent, pvTangentPrev = 0, pvStart; + float eStop, pvInst, pvAvg, pvIp, pvMax, pvPk, pvRes, slopeIp, pvTangent, pvTangentPrev = 0, pvStart; float _kp, _ki, _kd, _Ku, _Tu, _td, _R, _Ko, _TuMin, _tdMin; - uint8_t ipCount = 0, plotCount = 0; + uint8_t ipCount = 0, plotCount = 0, eStopAbort = 0; uint16_t _bufferSize, _samples, sampleCount = 0, pvPkCount = 0, dtCount = 0; uint32_t _settleTimeSec, _testTimeSec, usPrev = 0, settlePrev = 0, usStart, us, ipUs;