Can't synchronize PID::Compute() with external master timing source

[paynterf]

I have an autonomous robot that uses a master timing source generated by a TIMER5 interrupt on an Arduino Mega 2560. All my environmental variables (distance, heading, etc) are updated at every timer interrupt in the ISR and are available throughout the program. This works very well, except in the PID engine, as there does not appear to be any way to use the master timing interval to compute a new PID output.

It's easy enough to more-or-less synchronize by setting the PID engine sampling interval to the same interval as the master timer, but there is no guarantee that the two intervals won't slide by each other over time, potentially leading to unexpected behavior.

In one of my applications, I am controlling the turn rate of the robot using a PID engine. In the typical code configuration:

if(myPID.Compute())
{
    //compute the new turn rate using rate = (new hdg - old hdg) / delta_T
   //use the PID output term to control something
   ...
   ...
}

The control behavior is poor, because the new output value is generated in PID::Compute using a turn rate value that is at least one entire sampling interval in the past; in other words, the new output is generated before the new input variable is available.

Some of the PID documentation I researched said (or at least implied) that by setting the PID's sample time to zero using PID::SetSampleTime(0), that Compute() would actually produce a new output value every time it was called. This meant that I could do something like the following:

if (bTimeForNavUpdate) //set true in ISR
{
	bTimeForNavUpdate = false;

	//4/28/21 now time interval is constant at ~100mSec
	//11/14/20 need to handle -179 to +179 transition
	float deltaDeg = IMUHdgValDeg - prev_hdg;
	deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;
	deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;
	TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

	TurnRatePID.Compute();//04/10/21 SampleTime == 0 so now this updates every time

	SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);
	SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);
	prev_hdg = IMUHdgValDeg;
}

But this doesn't work because a SetSampleTime() argument of zero is ignored. The ostensible reason for this is that a sample time of zero results in the 'D' term being divided by zero - oops. Here's the relevant code:

void PID::SetSampleTime(int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      SampleTime = (unsigned long)NewSampleTime;
   }
}

However, if the above code is modified to move the SampleTime assignment out of the 'if' statement, it can be set to zero without causing a 'divide-by-zero' problem, as follows:

void PID::SetSampleTime(int NewSampleTime)
{
  Serial.println("In PID::SetSampleTime with NewSampleTime = "); Serial.println(NewSampleTime);
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      //SampleTime = (unsigned long)NewSampleTime;
   }

    SampleTime = (unsigned long)NewSampleTime;
}

I modified my copy of the library as above, and now I can do the following:

if (bTimeForNavUpdate) //set true in ISR
{
	bTimeForNavUpdate = false;

	//4/28/21 now time interval is constant at ~100mSec
	//11/14/20 need to handle -179 to +179 transition
	float deltaDeg = IMUHdgValDeg - prev_hdg;
	deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;
	deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;
	TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

	TurnRatePID.Compute();//04/10/21 SampleTime == 0 so now this updates every time

	SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);
	SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);
	prev_hdg = IMUHdgValDeg;
}

This seems to work very well - much better than the default method.

Any reason this can't be done with the library version of SetSampleTime()? After all, a SetSampleTime() argument of zero is currently ignored, so allowing a knowledgeable user to set the sample time interval to zero, thereby forcing Compute() to generate a new value every time it is called should not interfere at all with 'normal' use cases.

Thoughts?

Frank

[imax9000]

Any reason this can't be done with the library version of SetSampleTime()? After all, a SetSampleTime() argument of zero is currently ignored, so allowing a knowledgeable user to set the sample time interval to zero, thereby forcing Compute() to generate a new value every time it is called should not interfere at all with 'normal' use cases.

Ki and Kd are also scaled to the sampling interval, which affects the computation as well.

Looking more closely at the code, it seems that it doesn't behave correctly when the actual interval between calls to Compute() is substantially different from SampleTime.

I'll see if I can find some time to refactor it to work based on the actual time passed.