From 02c0508b20731b33c6f1f52637aa08270c9ec5e9 Mon Sep 17 00:00:00 2001 From: rusefi Date: Sun, 18 Nov 2018 23:28:18 -0500 Subject: [PATCH] moving to nicer implementation --- firmware/controllers/math/pid_auto_tune.cpp | 60 +++--- firmware/controllers/math/pid_auto_tune.h | 221 ++++++++++++++++---- 2 files changed, 215 insertions(+), 66 deletions(-) diff --git a/firmware/controllers/math/pid_auto_tune.cpp b/firmware/controllers/math/pid_auto_tune.cpp index f0b170bb6f..d050e23740 100644 --- a/firmware/controllers/math/pid_auto_tune.cpp +++ b/firmware/controllers/math/pid_auto_tune.cpp @@ -1,6 +1,10 @@ /* * pid_auto_tune.cpp * + * See https://github.com/br3ttb/Arduino-PID-AutoTune-Library/blob/master/PID_AutoTune_v0/PID_AutoTune_v0.cpp + * See https://github.com/t0mpr1c3/Arduino-PID-AutoTune-Library/blob/master/PID_AutoTune_v0/PID_AutoTune_v0.cpp + * + * * Created on: Sep 13, 2017 * @author Andrey Belomutskiy, (c) 2012-2018 */ @@ -13,35 +17,33 @@ PID_AutoTune::PID_AutoTune() { nLookBack = 50; oStep = 10; noiseBand = 0.5; - Ku = NAN; - Pu = NAN; -} - -void PID_AutoTune::FinishUp() { - Ku = 4 * (2 * oStep) / ((absMax - absMin) * 3.14159); - Pu = (float) (currentPeakTime - prevPeakTime) / 1000.0; // converting ms to seconds } -int PID_AutoTune::Runtime(Logging *logging) { - justevaled = false; +//void PID_AutoTune::FinishUp() { +// Ku = 4 * (2 * oStep) / ((absMax - absMin) * 3.14159); +// Pu = (float) (currentPeakTime - prevPeakTime) / 1000.0; // converting ms to seconds +// +//} + +bool PID_AutoTune::Runtime(Logging *logging) { + if (peakCount > 9 && running) { running = false; - FinishUp(); +// FinishUp(); return 1; } - justevaled = true; if (!running) { //initialize working variables the first time around - peakType = 0; + peakType = NOT_A_PEAK; peakCount = 0; absMax = input; absMin = input; setpoint = input; running = true; - dataPointsCount = 0; + inputCount = 0; outputStart = output; output = outputStart + oStep; @@ -55,7 +57,7 @@ int PID_AutoTune::Runtime(Logging *logging) { absMax = input; if (input < absMin) absMin = input; - dataPointsCount++; + inputCount ++; } //oscillate the output base on the input's relation to the setpoint @@ -83,7 +85,7 @@ int PID_AutoTune::Runtime(Logging *logging) { lastInputs[i + 1] = lastInputs[i]; } lastInputs[0] = input; - if (dataPointsCount < 9) { //we don't want to trust the maxes or mins until the inputs array has been filled + if (inputCount < 9) { //we don't want to trust the maxes or mins until the inputs array has been filled return 0; } @@ -95,27 +97,27 @@ int PID_AutoTune::Runtime(Logging *logging) { bool directionJustChanged = false; if (isMax) { - if (peakType == 0) - peakType = 1; - if (peakType == -1) { - peakType = 1; + if (peakType == NOT_A_PEAK ) + peakType = MAXIMUM ; + if (peakType == MINIMUM ) { + peakType = MAXIMUM ; directionJustChanged = true; } // peaks[peakCount] = input; we are not using max peak values } else if (isMin) { - if (peakType == 0) - peakType = -1; - if (peakType == 1) { - peakType = -1; - prevPeakTime = currentPeakTime; - currentPeakTime = currentTimeMillis(); + if (peakType == NOT_A_PEAK ) + peakType = MINIMUM ; + if (peakType == MAXIMUM ) { + peakType = MINIMUM; + lastPeakTime[1] = lastPeakTime[0]; + lastPeakTime[0] = currentTimeMillis(); directionJustChanged = true; if (peakCount < 10) { peakCount++; - peaks[peakCount] = input; + lastPeaks[peakCount] = input; } else { // todo: reset peak counter maybe? } @@ -125,10 +127,10 @@ int PID_AutoTune::Runtime(Logging *logging) { } if (directionJustChanged && peakCount > 2) { //we've transitioned. check if we can autotune based on the last peaks - float avgSeparation = (absF(peaks[peakCount - 0] - peaks[peakCount - 1]) - + absF(peaks[peakCount - 1] - peaks[peakCount - 2])) / 2; + float avgSeparation = (absF(lastPeaks[peakCount - 0] - lastPeaks[peakCount - 1]) + + absF(lastPeaks[peakCount - 1] - lastPeaks[peakCount - 2])) / 2; if (avgSeparation < 0.05 * (absMax - absMin)) { - FinishUp(); + //FinishUp(); running = false; return 1; diff --git a/firmware/controllers/math/pid_auto_tune.h b/firmware/controllers/math/pid_auto_tune.h index fca715d0ee..451d84916d 100644 --- a/firmware/controllers/math/pid_auto_tune.h +++ b/firmware/controllers/math/pid_auto_tune.h @@ -15,43 +15,190 @@ #include "global.h" #include "rusefi_types.h" -class PID_AutoTune { -public: - PID_AutoTune(); - void reset(); - void FinishUp(); - int Runtime(Logging *logging); -// bool isMax, isMin; - /** - * sensor position - */ - float input; - /** - * actuator duty cycle - */ - float output; - /** - * trigger line - */ - float setpoint; - float noiseBand; - //int controlType = 1; - bool running; - efitimems_t currentPeakTime, prevPeakTime; -// unsigned int peak1, peak2, lastTime; - //int sampleTime; - int nLookBack; - int peakType; // todo: convert to enum - float lastInputs[101]; - float peaks[10]; - int peakCount; - int dataPointsCount; - //bool justchanged; // - bool justevaled; - float absMax, absMin; - float oStep; - float outputStart; - float Ku, Pu; +#define byte uint8_t + + // irrational constants +#define CONST_PI 3.14159265358979323846 +#define CONST_SQRT2_DIV_2 0.70710678118654752440 + + + +// verbose debug option +// requires open Serial port +#undef AUTOTUNE_DEBUG + +#define USE_SIMULATION + +// defining this option implements relay bias +// this is useful to adjust the relay output values +// during the auto tuning to recover symmetric +// oscillations +// this can compensate for load disturbance +// and equivalent signals arising from nonlinear +// or non-stationary processes +// any improvement in the tunings seems quite modest +// but sometimes unbalanced oscillations can be +// persuaded to converge where they might not +// otherwise have done so +#undef AUTOTUNE_RELAY_BIAS + +// average amplitude of successive peaks must differ by no more than this proportion +// relative to half the difference between maximum and minimum of last 2 cycles +#define AUTOTUNE_PEAK_AMPLITUDE_TOLERANCE 0.05 + +// ratio of up/down relay step duration should differ by no more than this tolerance +// biasing the relay con give more accurate estimates of the tuning parameters but +// setting the tolerance too low will prolong the autotune procedure unnecessarily +// this parameter also sets the minimum bias in the relay as a proportion of its amplitude +#define AUTOTUNE_STEP_ASYMMETRY_TOLERANCE 0.20 + +// auto tune terminates if waiting too long between peaks or relay steps +// set larger value for processes with long delays or time constants +#define AUTOTUNE_MAX_WAIT_MINUTES 5 + +// Ziegler-Nichols type auto tune rules +// in tabular form +struct Tuning +{ + byte _divisor[3]; + + bool PI_controller() + { + return _divisor[2] == 0; + } + + double divisor(byte index) + { + return (double)(_divisor[index] * 0.05); + } }; +class PID_AutoTune +{ + +public: + + // constants *********************************************************************************** + + // auto tune method + enum + { + ZIEGLER_NICHOLS_PI = 0, + ZIEGLER_NICHOLS_PID = 1, + TYREUS_LUYBEN_PI, + TYREUS_LUYBEN_PID, + CIANCONE_MARLIN_PI, + CIANCONE_MARLIN_PID, + AMIGOF_PI, + PESSEN_INTEGRAL_PID, + SOME_OVERSHOOT_PID, + NO_OVERSHOOT_PID + }; + + // peak type + enum Peak + { + MINIMUM = -1, + NOT_A_PEAK = 0, + MAXIMUM = 1 + }; + + // auto tuner state + enum AutoTunerState + { + AUTOTUNER_OFF = 0, + STEADY_STATE_AT_BASELINE = 1, + STEADY_STATE_AFTER_STEP_UP = 2, + RELAY_STEP_UP = 4, + RELAY_STEP_DOWN = 8, + CONVERGED = 16, + FAILED = 128 + }; + + // tuning rule divisor + enum + { + KP_DIVISOR = 0, + TI_DIVISOR = 1, + TD_DIVISOR = 2 + }; + + + // commonly used methods *********************************************************************** + PID_AutoTune(); // * Constructor. links the Autotune to a given PID + bool Runtime(Logging *logging); // * Similar to the PID Compute function, + // returns true when done, otherwise returns false + void Cancel(); // * Stops the AutoTune + + void SetOutputStep(double); // * how far above and below the starting value will + // the output step? + double GetOutputStep(); // + + void SetControlType(byte); // * Determines tuning algorithm + byte GetControlType(); // * Returns tuning algorithm + + void SetLookbackSec(int); // * how far back are we looking to identify peaks + int GetLookbackSec(); // + + void SetNoiseBand(double); // * the autotune will ignore signal chatter smaller + // than this value + double GetNoiseBand(); // this should be accurately set + + double GetKp(); // * once autotune is complete, these functions contain the + double GetKi(); // computed tuning parameters. + double GetKd(); // + + double oStep; + byte peakCount; + double input; + double output; + + double absMax; // todo: remove this + double absMin; // todo: remove this + double outputStart; + + +private: + + double processValueOffset(double, // * returns an estimate of the process value offset + double); // as a proportion of the amplitude + + double setpoint; + + bool running; // todo: remove this + + double noiseBand; + byte nLookBack; + byte controlType; // * selects autotune algorithm + + enum AutoTunerState state; // * state of autotuner finite state machine + unsigned long lastTime; + unsigned long sampleTime; + enum Peak peakType; + unsigned long lastPeakTime[5]; // * peak time, most recent in array element 0 + double lastPeaks[5]; // * peak value, most recent in array element 0 + double lastInputs[101]; // * process values, most recent in array element 0 + byte inputCount; + double workingNoiseBand; + double workingOstep; + double inducedAmplitude; + double Kp, Ti, Td; + + // used by AMIGOf tuning rule + double calculatePhaseLag(double); // * calculate phase lag from noiseBand and inducedAmplitude + double fastArcTan(double); + double newWorkingNoiseBand; + double K_process; + +#if defined AUTOTUNE_RELAY_BIAS + double relayBias; + unsigned long lastStepTime[5]; // * step time, most recent in array element 0 + double sumInputSinceLastStep[5]; // * integrated process values, most recent in array element 0 + byte stepCount; +#endif + +}; + + + #endif /* CONTROLLERS_MATH_PID_AUTO_TUNE_H_ */