In addition to feedforward with gain and offset, also added control over windup. Also cleaned up some variable names for consistent capitalization.
216 lines
7.9 KiB
C++
216 lines
7.9 KiB
C++
/**********************************************************************************************
|
|
* Arduino PID Library - Version 1.0.2
|
|
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
|
|
*
|
|
* Modified by Jason Melvin to include feedforward and adjustable windup
|
|
*
|
|
* This Code is licensed under a Creative Commons Attribution-ShareAlike 3.0 Unported License.
|
|
**********************************************************************************************/
|
|
|
|
#if ARDUINO >= 100
|
|
#include "Arduino.h"
|
|
#else
|
|
#include <WProgram.h>
|
|
#endif
|
|
#include <PID_v1.h>
|
|
|
|
/*Constructor (...)*********************************************************
|
|
* The parameters specified here are those for for which we can't set up
|
|
* reliable defaults, so we need to have the user set them.
|
|
***************************************************************************/
|
|
PID::PID(double* Input, double* Output, double* Setpoint,
|
|
double Kp, double Ki, double Kd, double Kf, int ControllerDirection)
|
|
{
|
|
PID::SetOutputLimits(0, 255); //default output limit corresponds to
|
|
//the arduino pwm limits
|
|
|
|
windupI = outMax; // default windup limit is outMax
|
|
sampleTime = 100; // default Controller Sample Time is 0.1 seconds
|
|
ff_zero = 0; // default feedforward zero point is 0.0 (for temp control, set to ambient)
|
|
|
|
PID::SetControllerDirection(ControllerDirection);
|
|
PID::SetTunings(Kp, Ki, Kd, Kf);
|
|
|
|
lastTime = millis()-sampleTime;
|
|
inAuto = false;
|
|
myOutput = Output;
|
|
myInput = Input;
|
|
mySetpoint = Setpoint;
|
|
|
|
}
|
|
|
|
|
|
/* 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
|
|
**********************************************************************************/
|
|
void PID::Compute()
|
|
{
|
|
if(!inAuto) return;
|
|
unsigned long now = millis();
|
|
unsigned long timeChange = (now - lastTime);
|
|
if(timeChange>=sampleTime)
|
|
{
|
|
/*Compute all the working error variables*/
|
|
double input = *myInput; // read current input condition
|
|
double error = *mySetpoint - input; // error is difference between setpoint and input
|
|
iTerm+= (ki * error); // add additional error to the integral term
|
|
iTerm = constrain( iTerm , -windupI , windupI ); // limit the integral term to +/- windup parameter
|
|
double dInput = (input - lastInput); // derivative is based on change in the input
|
|
|
|
/*Compute PID Output*/
|
|
double output = kp * error + iTerm - kd * dInput + kf * (*mySetpoint - ff_zero);
|
|
|
|
output = constrain( output , outMin , outMax ); // limit output to within min/max
|
|
*myOutput = output; // write the current output
|
|
|
|
/*Remember some variables for next time*/
|
|
lastInput = input; // last input is for computing the derivative term
|
|
lastTime = now; // last time is for determining when to recompute
|
|
}
|
|
}
|
|
|
|
|
|
/* 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(double Kp, double Ki, double Kd, double Kf)
|
|
{
|
|
if (Kp<0 || Ki<0 || Kd<0 || Kf<0) return;
|
|
|
|
dispKp = Kp; dispKi = Ki; dispKd = Kd; dispKf = Kf;
|
|
|
|
double sampleTimeInSec = ((double)sampleTime)/1000;
|
|
kp = Kp;
|
|
ki = Ki * sampleTimeInSec;
|
|
kd = Kd / sampleTimeInSec;
|
|
kf = Kf;
|
|
|
|
if(controllerDirection ==REVERSE)
|
|
{
|
|
kp = (0 - kp);
|
|
ki = (0 - ki);
|
|
kd = (0 - kd);
|
|
kf = (0 - kf);
|
|
}
|
|
}
|
|
|
|
/* SetsampleTime(...) *********************************************************
|
|
* sets the period, in Milliseconds, at which the calculation is performed
|
|
******************************************************************************/
|
|
void PID::SetSampleTime(int NewsampleTime)
|
|
{
|
|
if (NewsampleTime > 0)
|
|
{
|
|
double ratio = (double)NewsampleTime
|
|
/ (double)sampleTime;
|
|
ki *= ratio;
|
|
kd /= ratio;
|
|
sampleTime = (unsigned long)NewsampleTime;
|
|
}
|
|
}
|
|
|
|
/* SetWindupI(...)********************
|
|
* Sets the windup limit for the integral term,
|
|
* which is otherwise limited to outMax
|
|
***************************************/
|
|
void PID::SetWindupI(double limit)
|
|
{
|
|
if (limit > 0) windupI = limit;
|
|
}
|
|
|
|
/* SetFF_zero(...)********************
|
|
* Sets the zero point for the feedforward term,
|
|
* which is otherwise 0.0
|
|
***************************************/
|
|
void PID::SetFFzero(double zeropoint)
|
|
{
|
|
ff_zero = zeropoint;
|
|
}
|
|
|
|
/* 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(double Min, double Max)
|
|
{
|
|
if(Min >= Max) return;
|
|
if (outMax) windupI *= Max / outMax;
|
|
outMin = Min;
|
|
outMax = Max;
|
|
if(inAuto)
|
|
{
|
|
if(*myOutput > outMax) *myOutput = outMax;
|
|
else if(*myOutput < outMin) *myOutput = outMin;
|
|
|
|
if(iTerm > outMax) iTerm= outMax;
|
|
else if(iTerm < outMin) iTerm= outMin;
|
|
}
|
|
}
|
|
|
|
/* SetMode(...)****************************************************************
|
|
* Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
|
|
* when the transition from manual to auto occurs, the controller is
|
|
* automatically initialized
|
|
******************************************************************************/
|
|
void PID::SetMode(int Mode)
|
|
{
|
|
bool newAuto = (Mode == AUTOMATIC);
|
|
if(newAuto == !inAuto)
|
|
{ /*we just went from manual to auto*/
|
|
PID::Initialize();
|
|
}
|
|
inAuto = newAuto;
|
|
}
|
|
|
|
/* Initialize()****************************************************************
|
|
* does all the things that need to happen to ensure a bumpless transfer
|
|
* from manual to automatic mode.
|
|
******************************************************************************/
|
|
void PID::Initialize()
|
|
{
|
|
iTerm = *myOutput;
|
|
lastInput = *myInput;
|
|
if(iTerm > windupI) iTerm = windupI;
|
|
else if(iTerm < outMin) iTerm = outMin;
|
|
}
|
|
|
|
/* SetControllerDirection(...)*************************************************
|
|
* The PID will either be connected to a DIRECT acting process (+Output leads
|
|
* to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
|
|
* know which one, because otherwise we may increase the output when we should
|
|
* be decreasing. This is called from the constructor.
|
|
******************************************************************************/
|
|
void PID::SetControllerDirection(int Direction)
|
|
{
|
|
if(inAuto && Direction !=controllerDirection)
|
|
{
|
|
kp = (0 - kp);
|
|
ki = (0 - ki);
|
|
kd = (0 - kd);
|
|
kf = (0 - kf);
|
|
}
|
|
controllerDirection = Direction;
|
|
}
|
|
|
|
/* Status Funcions*************************************************************
|
|
* Just because you set the Kp=-1 doesn't mean it actually happened. these
|
|
* functions query the internal state of the PID. they're here for display
|
|
* purposes. this are the functions the PID Front-end uses for example
|
|
******************************************************************************/
|
|
double PID::GetKp(){ return dispKp; }
|
|
double PID::GetKi(){ return dispKi;}
|
|
double PID::GetKd(){ return dispKd;}
|
|
double PID::GetKf(){ return dispKf;}
|
|
double PID::GetWi(){ return windupI;}
|
|
double PID::GetFFzero(){ return ff_zero;}
|
|
int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
|
|
int PID::GetDirection(){ return controllerDirection;}
|