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

myPID.SetSampleTimeUs not working as expected. #41

Closed SinisterRj closed 2 years ago

SinisterRj commented 2 years ago

PID recalculates faster than my window time (in a relay output configuration). It runs once in 60-50 ms when my window is 5000. Changed micros() to millis() in QuickPID.cpp as the original PID_v1.cpp and it works again. Maybe a truncation problem using uint32_t on 16 bit boards (i'm testing in a nano and a uno). My suggestion is to get back to myPID.SetSampleTime in milli seconds instead of micro seconds.

Dlloydev commented 2 years ago

Hello and thanks for your comment.

Interesting that switching to millis resolved the issue ... I'd like to see the code or minimum code that reproduces the issue. I would have guessed the issue would have been synchronizing the PID and Output control timings.

Yeah, I'll need to update the timer examples and PID_RelayOutput.ino so they all work as expected with a relay output configuration. I've got some hardware on order to do similar testing with the sTune library and examples.

Note that if QuickPID is put in timer mode, this tells compute to run each time it is called. Now the compute and control timing can be easily synchronized the user code.

An additional thing I'd like to include is minimum control time (ON or OFF) for mechanical relays. Already, the minWindow helps at the beginning of the window, but if the Output becomes, for example, 4995, then there will be a 5ms de-energized period prior to the next window. This can harm contacts under load as it might be less than the off-time and contact bounce period of the relay.

I think a minWindow value of 50ms would be more than adequate for most relays ... 500ms seems to excessively reduce the usable output control range for the PID to 10% -100%. Using 50ms at each end of the window, the output control range becomes 1% to 99% and any value out of range is ignored. Both ON and OFF switching duration would be protected and the overall maximum switching frequency (0.2Hz) for the relay would be governed by the windowSize of 5-sec.

Here's an example (untested) that should synchronize everything in software using Ticker.h. Make sure there's no calls to delay() in your code ...

/********************************************************
   PID Basic Software TIMER controlled Relay Example
   Reading analog input 0 to control digital output 6
 ********************************************************/
#include <Ticker.h> // https://github.com/sstaub/Ticker
#include <QuickPID.h>
void runPid();

#define PIN_INPUT 0
#define RELAY_PIN 6
const uint32_t sampleTimeUs = 5000000; // 5 sec
static boolean computeNow = false;

//Define Variables we'll be connecting to
float Setpoint, Input, Output;

//Specify the links and initial tuning parameters
float Kp = 2, Ki = 5, Kd = 1;

Ticker timer1(runPid, sampleTimeUs, 0, MICROS_MICROS);
QuickPID myPID(&Input, &Output, &Setpoint);

unsigned long windowSize = 5000;
unsigned long minWindow = 500;
unsigned long windowStartTime;

void setup() {
  timer1.start();

  pinMode(RELAY_PIN, OUTPUT);
  windowStartTime = millis();

  //initialize the variables we're linked to
  //Input = analogRead(PIN_INPUT);
  Setpoint = 100;

  //tell the PID to range between 0 and the full window size
  myPID.SetOutputLimits(0, windowSize);

  //apply PID gains
  myPID.SetTunings(Kp, Ki, Kd);

  //PID set to timer mode
  myPID.SetMode(myPID.Control::timer);
}

void loop() {
  timer1.update();
  if (computeNow) { //compute and Output rate (sampleTimeUs) controlled by timer1
    windowStartTime += windowSize; //shift the Relay Window
    Input = analogRead(PIN_INPUT);
    myPID.Compute();
    computeNow = false;
  }
  if ((Output > minWindow) && (Output < windowSize - minWindow)) { //if in control range
    if (Output > (millis() - windowStartTime)) digitalWrite(RELAY_PIN, HIGH);
    else digitalWrite(RELAY_PIN, LOW);
  }
}

void runPid() {
  computeNow = true;
}
SinisterRj commented 2 years ago

Hello Dlloydev, thanks for the fast repply.

In my opinion the better way of doing it is using _myPID.SetOutputLimits(MIN_WINDOW, WINDOW_SIZE - MINWINDOW); instead of a user's defined timer.

I had to roll back to because the controll with your library don't work at all for me (maybe more problems with types?). but here is my example code:

/*************************************************************
   PID Relay Output Example
   Same as basic example, except that this time, the output
   is going to a digital pin which (we presume) is controlling
   a relay.  The pid is designed to Output an analog value,
   but the relay can only be On/Off.
   To connect them together we use "time proportioning
   control", essentially a really slow version of PWM.
   First we decide on a window size (5000mS for example).
   We then set the pid to adjust its output between 0 and that
   window size. Lastly, we add some logic that translates the
   PID output into "Relay On Time" with the remainder of the
   window being "Relay Off Time". The minWindow setting is a
   floor (minimum time) the relay would be on.
 *************************************************************/

//#include "QuickPID2.h"
#include <PID_v1.h>

#define PIN_INPUT 0
#define RELAY_PIN 6

//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
double Kp = 2, Ki = 0.5, Kd = 2;

//QuickPID2 myPID(&Input, &Output, &Setpoint);
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

#define WINDOW_SIZE 500
#define MIN_WINDOW  5

unsigned long windowStartTime;
unsigned long oldMs, newMs = 0;

//***** Simulation *****
//set to false to connect to the real world
boolean useSimulation = true;
double kpmodel=1.5, taup=100, theta[50];
double outputStart=5;
unsigned long  modelTime;

void setup()
{
  Serial.begin(38400);

  if(useSimulation)
  {
    for(byte i=0;i<50;i++)
    {
      theta[i]=outputStart;
    }
    modelTime = 0;
  }

  pinMode(RELAY_PIN, OUTPUT);
  windowStartTime = millis();

  //initialize the variables we're linked to
  Setpoint = 100;

  //tell the PID to range between MIN_WINDOW and the full window size less the MIN_WINDOW to avoid fast relay switching.
  myPID.SetOutputLimits(MIN_WINDOW, WINDOW_SIZE - MIN_WINDOW);

  //myPID.SetSampleTimeUs(WINDOW_SIZE);
  myPID.SetSampleTime(WINDOW_SIZE);

  //apply PID gains
  myPID.SetTunings(Kp, Ki, Kd);

  //turn the PID on
  //myPID.SetMode(myPID.Control::automatic);
  myPID.SetMode(AUTOMATIC);

  oldMs = millis();
}

void loop()
{
  unsigned long now = millis();
  if(!useSimulation)
  { //pull the input in from the real world
    Input = analogRead(PIN_INPUT);
  }

  /************************************************
     turn the output pin on/off based on pid output
   ************************************************/

  if (myPID.Compute()) {
  //Serial.print();
  //Serial.print(":");
    windowStartTime = millis();
    newMs = millis();  
    Serial.println();      
    Serial.print(newMs-oldMs);
    Serial.print(":");
    Serial.print(Output);
    Serial.print(":");
    oldMs=newMs;
  }

  if ((unsigned int)Output > (millis() - windowStartTime)) {
    digitalWrite(RELAY_PIN, HIGH);
    Serial.print("1");
  } else {
    digitalWrite(RELAY_PIN, LOW);
    Serial.print("0");
  }
  delay(5);  

  if(useSimulation)
  {
    theta[30]=Output;
    if(now>=modelTime)
    {
      modelTime +=100; 
      DoModel();
    }
  }
}

void DoModel()
{
  //cycle the dead time
  for(byte i=0;i<49;i++)
  {
    theta[i] = theta[i+1];
  }
  //compute the input
  Input = (kpmodel / taup) *(theta[0]-outputStart) + Input*(1-1/taup) + ((float)random(-10,10))/100;

}
Dlloydev commented 2 years ago

Thanks for the reply and example!

I like the approach of having the PID control the timing. I should get a chance later to do some testing ... what I'd like to end up with is a working and very basic relay output example.

Dlloydev commented 2 years ago

I've pushed an updated example PID_RelayOutput.ino. It tested OK with my TCLab hardware and setup (Kp, Ki, Kd ZN tunings from sTune).