Dlloydev / QuickPID

A fast PID controller with multiple options. Various Integral anti-windup, Proportional, Derivative and timer control modes.
MIT License
195 stars 50 forks source link

Auto_Tune_Filter_Direct Issue / Explanation we are stuck ? #31

Closed REY-01 closed 2 years ago

REY-01 commented 2 years ago

Hello and thank you for your great Arduino library entry with quick PID.

We are currently using your PID program manually to drive a trailer with electric motor so that we no longer have a towing load on the towing vehicle. For this we have a sensor that gives us a 0-5v signal at the input for the towing force. Se these are the the 0-1023 input values at the input pin. Our output for the motor control is currently done with a mcp4725 DAC with12bit resolution so therefore we need the Output Min/max be set to 0-4096 and later can be narrowed down to 1200 - 2400 for the motor control.

This so far for our project now to our main issue. If we take the simple controller and give him the values your PID controller works great except that the determination of the values was very time consuming trail and error so far and we are not really satisfied now with some results so far..

We now wanted to use the auto-tuning to be able to respond to more or less load on the trailer and possibly tune the whole thing better but we need to understand your Programm better to archive that.

Now we have tired with your example that we have changed to our needs but the following problem occurred when we try to Autotune. The output values do not go higher than the 255 that were originally set for your PWM output. But Despite that we have entered the max and min values on the top they are fully ignored.

Now we have used the SetOutputLimits(outputMin, outputMax); To set the given output values again but the tuning process itself does not use this command after it is finished it does.

Are we doing something wrong? Or better where do we give these output values to the tuning process?

And could you please explain me again the logical process of your Auto Tuning example.

What do the inputs/outputs do for stabilizing for example? Step tuning is reached it should not then control the output and wait how the values are and then calculate? And a short explanation of the Autotuning values where nice like output 1/3 Step Hysteresis etc. we are not shure if we understand them correct. Thank you verry much !

include

include

include

const uint32_t sampleTimeUs = 10000; // 10ms const byte inputPin = A5; //const byte outputPin = 3; const int inputMax = 1023; const int outputMax = 4096; const int outputMin = 0;

bool printOrPlotter = 0; // on(1) monitor, off(0) plotter float POn = 1.3; // proportional on Error to Measurement ratio (0.0-1.0), default = 1.0 float DOn = 0.0; // derivative on Error to Measurement ratio (0.0-1.0), default = 0.0

byte outputStep = 50; byte hysteresis = 10; int setpoint = 341; // 1/3 of range for symetrical waveform int output = 1365; // 1/3 of range for symetrical waveform

float Input, Output, Setpoint; float Kp = 0, Ki = 0, Kd = 0; bool pidLoop = false;

QuickPID _myPID = QuickPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, POn, DOn, QuickPID::DIRECT);

//DAC init MCP4725 MCP(0x62);

void setup() { Serial.begin(115200); Serial.println(); //MCP Setup MCP.begin(); MCP.setValue(0);

if (constrain(output, outputMin, outputMax - outputStep - 5) < output) { Serial.println(F("AutoTune test exceeds outMax limit. Check output, hysteresis and outputStep values")); Serial.println("GO"); while (1); } // Select one, reference: https://github.com/Dlloydev/QuickPID/wiki //_myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PI); _myPID.AutoTune(tuningMethod::ZIEGLER_NICHOLS_PID); _myPID.SetOutputLimits(outputMin, outputMax); //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PI); //_myPID.AutoTune(tuningMethod::TYREUS_LUYBEN_PID); //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PI); //_myPID.AutoTune(tuningMethod::CIANCONE_MARLIN_PID); //_myPID.AutoTune(tuningMethod::AMIGOF_PID); //_myPID.AutoTune(tuningMethod::PESSEN_INTEGRAL_PID); //_myPID.AutoTune(tuningMethod::SOME_OVERSHOOT_PID); //_myPID.AutoTune(tuningMethod::NO_OVERSHOOT_PID);

_myPID.autoTune->autoTuneConfig(outputStep, hysteresis, inputMax - setpoint, output, QuickPID::DIRECT, printOrPlotter, sampleTimeUs);

//PID Limit _myPID.SetOutputLimits(outputMin, outputMax); }

void loop() {

if (_myPID.autoTune) // Avoid dereferencing nullptr after _myPID.clearAutoTune() { switch (_myPID.autoTune->autoTuneLoop()) { case _myPID.autoTune->AUTOTUNE: Input = inputMax - avg(_myPID.analogReadFast(inputPin)); // filtered, reverse acting _myPID.SetOutputLimits(outputMin, outputMax);//PID Limits MCP.setValue(Output); // Write Output to DAC break;

  case _myPID.autoTune->TUNINGS:
    _myPID.autoTune->setAutoTuneConstants(&Kp, &Ki, &Kd); // set new tunings
    _myPID.SetMode(QuickPID::AUTOMATIC); // setup PID  
    _myPID.SetOutputLimits(outputMin, outputMax);//PID Limit
    _myPID.SetSampleTimeUs(sampleTimeUs);
    _myPID.SetTunings(Kp, Ki, Kd, POn, DOn); // apply new tunings to PID
    Setpoint = 500;
    break;

  case _myPID.autoTune->CLR:
    if (!pidLoop) {
      _myPID.clearAutoTune(); // releases memory used by AutoTune object
      pidLoop = true;
    }
    break;
}

} if (pidLoop) { if (printOrPlotter == 0) { // plotter Serial.print("Setpoint:"); Serial.print(Setpoint); Serial.print(","); Serial.print("Input:"); Serial.print(Input); Serial.print(","); Serial.print("Output:"); Serial.print(Output); Serial.println(","); } Input = inputMax - _myPID.analogReadFast(inputPin); // reverse acting _myPID.Compute(); MCP.setValue(Output); // Write Output to DAC } }

float avg(int inputVal) { static int arrDat[16]; static int pos; static long sum; pos++; if (pos >= 16) pos = 0; sum = sum - arrDat[pos] + inputVal; arrDat[pos] = inputVal; return (float)sum / 16.0; }

Dlloydev commented 2 years ago

Hello and thanks for using QuickPID. That's an interesting project you're working on!

It looks like mostly you have everything setup correct in your modified Auto_Tune_Filter_Direct code except for a few things.

float outputStep = 50.0;
float hysteresis = 10.0;
float setpoint = 341.0; // 1/3 of range for symetrical waveform
float output = 1365.0; // 1/3 of range for symetrical waveform

Even with these changes, I don't think the current version of autotune will work well on your system, because it was designed on a system (PWM output→RC Filter→10-bit ADC input) that has a direct and linear relationship of output to input (process gain).

I've started some work on a newer version that'll improve autotune's compatibility, but it'll unfortunately be a while yet before I have anything ready.

Dlloydev commented 2 years ago

I've Removed AutoTune in preparation for a new AutoTune library (sTune) compatible with QuickPID, PID_v1 and others. Should have this ready in the next few weeks ... more details to follow.

Closing the previous AutoTune issues as these will be successfully addressed by the new library.