Arduino-PID-Library/PID_v1/PID_v1.cpp
Jason Melvin f8ae2f9b73 Added feedforward ability and windup control.
In addition to feedforward with gain and offset, also added
control over windup.
Also cleaned up some variable names for consistent capitalization.
2012-01-24 09:52:26 -05:00

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;}