I am using the PID-beta6 library, which came with a nexus omni4wd robot.
I need to understand the way the PID works in this library. I already know what a PID is, because I finished my control systems lecture, but we only had a minor overview about discrete control.
So first of all here are the important sections from the code :
PID::PID(int *Input, int *Output, int *Setpoint, float Kc, float TauI, float TauD)
{
PID::ConstructorCommon(Input, Output, Setpoint, Kc, TauI, TauD);
UsingFeedForward = false;
PID::Reset();
}
void PID::ConstructorCommon(int *Input, int *Output, int *Setpoint, float Kc, float TauI, float TauD)
{
PID::SetInputLimits(0, 1023); //default the limits to the
PID::SetOutputLimits(0, 255); //full ranges of the I/O
tSample = 1000; //default Controller Sample Time is 1 second
PID::SetTunings( Kc, TauI, TauD);
nextCompTime = millis();
inAuto = false;
myOutput = Output;
myInput = Input;
mySetpoint = Setpoint;
Err = lastErr = prevErr = 0;
}
/* SetInputLimits(...)*****************************************************
* I don't see this function being called all that much (other than from the
* constructor.) it needs to be here so we can tell the controller what it's
* input limits are, and in most cases the 0-1023 default should be fine. if
* there's an application where the signal being fed to the controller is
* outside that range, well, then this function's here for you.
**************************************************************************/
void PID::SetInputLimits(int INMin, int INMax)
{
//after verifying that mins are smaller than maxes, set the values
if(INMin >= INMax) return;
inMin = INMin;
inSpan = INMax - INMin;
}
/* SetOutputLimits(...)****************************************************
* This function will be used far more often than SetInputLimits. while
* the input to the controller will generally be in the 0-1023 range (which is
* the default already,) the output will be a little different. maybe they'll
* be doing a time window and will need 0-8000 or something. or maybe they'll
* want to clamp it from 0-125. who knows. at any rate, that can all be done
* here.
**************************************************************************/
void PID::SetOutputLimits(int OUTMin, int OUTMax)
{
//after verifying that mins are smaller than maxes, set the values
if(OUTMin >= OUTMax) return;
outMin = OUTMin;
outSpan = OUTMax - OUTMin;
}
/* SetTunings(...)*************************************************************
* This function allows the controller's dynamic performance to be adjusted.
* it's called automatically from the constructor, but tunings can also
* be adjusted on the fly during normal operation
******************************************************************************/
void PID::SetTunings(float Kc, float TauI, float TauD)
{
//verify that the tunings make sense
if (Kc == 0.0 || TauI < 0.0 || TauD < 0.0) return;
//we're going to do some funky things to the input numbers so all
//our math works out, but we want to store the numbers intact
//so we can return them to the user when asked.
P_Param = Kc;
I_Param = TauI;
D_Param = TauD;
//convert Reset Time into Reset Rate, and compensate for Calculation frequency
float tSampleInSec = ((float)tSample / 1000.0);
float tempTauR;
if (TauI == 0.0)
tempTauR = 0.0;
else
tempTauR = (1.0 / TauI) * tSampleInSec;
kc = Kc;
taur = tempTauR;
taud = TauD / tSampleInSec;
cof_A = kc * (1 + taur + taud);
cof_B = kc * (1 + 2 * taud);
cof_C = kc * taud;
}
/* SetSampleTime(...)*******************************************************
* sets the frequency, in Milliseconds, with which the PID calculation is performed
******************************************************************************/
void PID::SetSampleTime(int NewSampleTime)
{
if (NewSampleTime > 0)
{
//convert the time-based tunings to reflect this change
taur *= ((float)NewSampleTime)/((float) tSample);
taud *= ((float)NewSampleTime)/((float) tSample);
tSample = (unsigned long)NewSampleTime;
cof_A = kc * (1 + taur + taud);
cof_B = kc * (1 + 2 * taud);
cof_C = kc * taud;
}
}
/* Compute() **********************************************************************
* This, as they say, is where the magic happens. this function should be called
* every time "void loop()" executes. the function will decide for itself whether a new
* pid Output needs to be computed
*
* Some notes for people familiar with the nuts and bolts of PID control:
* - I used the Ideal form of the PID equation. mainly because I like IMC
* tunings. lock in the I and D, and then just vary P to get more
* aggressive or conservative
*
* - While this controller presented to the outside world as being a Reset Time
* controller, when the user enters their tunings the I term is converted to
* Reset Rate. I did this merely to avoid the div0 error when the user wants
* to turn Integral action off.
*
* - Derivative on Measurement is being used instead of Derivative on Error. The
* performance is identical, with one notable exception. DonE causes a kick in
* the controller output whenever there's a setpoint change. DonM does not.
*
* If none of the above made sense to you, and you would like it to, go to:
* http://www.controlguru.com . Dr. Cooper was my controls professor, and is
* gifted at concisely and clearly explaining PID control
*********************************************************************************/
void PID::Compute()
{
justCalced=false;
if (!inAuto) return; //if we're in manual just leave;
unsigned long now = millis();
//millis() wraps around to 0 at some point. depending on the version of the
//Arduino Program you are using, it could be in 9 hours or 50 days.
//this is not currently addressed by this algorithm.
//...Perform PID Computations if it's time...
if (now>=nextCompTime)
{
Err = *mySetpoint - *myInput;
//if we're using an external bias (i.e. the user used the
//overloaded constructor,) then pull that in now
if(UsingFeedForward)
{
bias = *myBias - outMin;
}
// perform the PID calculation.
//float output = bias + kc * ((Err - lastErr)+ (taur * Err) + (taud * (Err - 2*lastErr + prevErr)));
noInterrupts();
int output = bias + (cof_A * Err - cof_B * lastErr + cof_C * prevErr);
interrupts();
//make sure the computed output is within output constraints
if (output < -outSpan) output = -outSpan;
else if (output > outSpan) output = outSpan;
prevErr = lastErr;
lastErr = Err;
//scale the output from percent span back out to a real world number
*myOutput = output;
nextCompTime += tSample; // determine the next time the computation
if(nextCompTime < now) nextCompTime = now + tSample; // should be performed
justCalced=true; //set the flag that will tell the outside world that the output was just computed
}
}
So I don't understand with which method you can get to the form int output = bias + (cof_A * Err - cof_B * lastErr + cof_C * prevErr);
Why do subtract the cof_B? I get cof_A = kc * (1 + taur + taud); ; that's the normal subdivision to the P I and D. But what are cof_B and cof_C for???
And how do you get to the form, mentioned above.
I hope you can help me!
Thank you,
Niklas