imax9000 / arduino-pid-library

Some AuDHD nerd on them internets.

I've no idea what I'm working right now.

If I'm not responding to your issues/PRs for too long - feel free to ping me on some social media (you may be able to find some links here).

Oh, and while you're here, you may want to help my friends and family avoid being killed by russians: https://savelife.in.ua/en/donate-en/

arduino-pid-library's Issues

Copy Brett's blog posts to this repo, so they won't get lost

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

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

Not getting default SampleTime after multiple compilations

I'm using PID_v2 in an Arduino application, and I get the following output:

MPU6050 connection successful
Initializing DMP...
Enabling DMP...
DMP ready! Waiting for MPU6050 drift rate to settle...
MPU6050 Ready at 0.52 Sec
TurnRatePID started with Kp/Ki/Kd = 5.0,0.8,3.0, kp/ki/kd =  = 5.00,0.080,30.00, SampleTime = 30
End of test - Stopping!

From this code:

//05/31/21 latest & greatest PID values
double TurnRate_Kp = 5.0;
double TurnRate_Ki = 0.8;
double TurnRate_Kd = 3;
double TurnRatePIDSetPointDPS, TurnRatePIDOutput;
double TurnRatePIDInput = 15.f;
PID TurnRatePID(&TurnRatePIDInput, &TurnRatePIDOutput, &TurnRatePIDSetPointDPS, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, DIRECT);

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

#pragma region MPU6050
	mySerial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);
	mpu.initialize();

	// verify connection
	Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

	float StartSec = 0; //used to time MPU6050 init
	Serial.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	// make sure it worked (returns 0 if successful)
	if (devStatus == 0)
	{
		// turn on the DMP, now that it's ready
		Serial.println(F("Enabling DMP..."));
		mpu.setDMPEnabled(true);

		// set our DMP Ready flag so the main loop() function knows it's okay to use it
		Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
		dmpReady = true;

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
		bMPU6050Ready = true;
		StartSec = millis() / 1000.f;
		mySerial.printf("MPU6050 Ready at %2.2f Sec\n", StartSec);
	}
	else
	{
		// ERROR!
		// 1 = initial memory load failed
		// 2 = DMP configuration updates failed
		// (if it's going to break, usually the code will be 1)
		Serial.print(F("DMP Initialization failed (code "));
		Serial.print(devStatus);
		Serial.println(F(")"));

		bMPU6050Ready = false;
	}
#pragma endregion MPU6050

	mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%,%2.1f, kp/ki/kd =  = %2.2f,%2.3f,%,%2.2f, SampleTime = %d\n",
		TurnRatePID.GetKp(), TurnRatePID.GetKi(), TurnRatePID.GetKd(), TurnRatePID.Getkp(), TurnRatePID.Getki(), TurnRatePID.Getkd(), TURN_RATE_UPDATE_INTERVAL_MSEC);

#pragma region TIMER_INTERRUPT
	//09/18/20 can't use Timer1 cuz doing so conflicts with PWM on pins 10-12
	cli();//stop interrupts
	TCCR5A = 0;// set entire TCCR5A register to 0
	TCCR5B = 0;// same for TCCR5B
	TCNT5 = 0;//initialize counter value to 0

	// set compare match register for 5hz increments
	//10/11/20 chg timer interval to 10Hz vs 5
	//OCR5A = 3124;// = (16*10^6) / (5*1024) - 1 (must be <65536)
	OCR5A = 1562;// = (16*10^6) / (10*1024) - 1 (must be <65536)
	TCCR5B |= (1 << WGM52);// turn on CTC mode
	TCCR5B |= (1 << CS52) | (1 << CS50);// Set CS10 and CS12 bits for 1024 prescaler
	TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt
	sei();//allow interrupts
	pinMode(TIMER_INT_OUTPUT_PIN, OUTPUT);
#pragma endregion TIMER_INTERRUPT


	mySerial.printf("End of test - Stopping!\n");
	while (true)
	{
		CheckForUserInput();
	}
}

I expected the SampleTime to be 100 after the default PID constructor, but it isn't - it's still the same value I set later in the code. I have tried a full 'Clean' build, but get the same (unexpected) result.

What gives?

Frank

integral reset

Hi,

What is the correct method of resetting the controller, specifically integral term?

setmode doesn't seem to have any effect, other than "pausing it".

I.e I tell my robot to drive in a straight line, and then abruptly stop it, and put the controller into manual mode. When i then direct it to say reverse, and put it back into automatic, that robot still travels forward for a bit, before decelerating and accelerating rearwards.

Thanks!

Add a method to PID_v2 to adjust the output while in Manual mode

Dear All,

First of all, thank you very much for this PID V2 Library!
I have an issue with the PID SetMode function. Whatever I set, the output is still in control, the process overwrites my manual inputs.
This is perfectly working for me in PID V1, but once I replaced the library with V2 it stops to work.

I used the following methods to activate / deactivate the process, neither was working for me:
SERVOPID.SetMode(MANUAL);
SERVOPID.SetMode(PID::Manual);

I checked and it should be fine.

I called the
SERVOValue = SERVOPID.Run(PITSensorValue);
after I manually adjust the SERVOValue, the PID V1 did not do anything with the value, but V2 is adjusting.

Many thanks and Kindest Regards,
Gergely

New features and usage documentation

Hi, thanks for the work!
Is there any description about the improvements over the PID_v1 and also some doc about the usage of the v2?
Thanks