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.
This commit is contained in:
parent
20199df217
commit
f8ae2f9b73
@ -1,16 +1,17 @@
|
|||||||
/**********************************************************************************************
|
/**********************************************************************************************
|
||||||
* Arduino PID Library - Version 1.0.1
|
* Arduino PID Library - Version 1.0.2
|
||||||
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
|
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
|
||||||
*
|
*
|
||||||
* This Library is licensed under a GPLv3 License
|
* 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
|
#if ARDUINO >= 100
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
#else
|
#else
|
||||||
#include "WProgram.h"
|
#include <WProgram.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#include <PID_v1.h>
|
#include <PID_v1.h>
|
||||||
|
|
||||||
/*Constructor (...)*********************************************************
|
/*Constructor (...)*********************************************************
|
||||||
@ -18,22 +19,24 @@
|
|||||||
* reliable defaults, so we need to have the user set them.
|
* reliable defaults, so we need to have the user set them.
|
||||||
***************************************************************************/
|
***************************************************************************/
|
||||||
PID::PID(double* Input, double* Output, double* Setpoint,
|
PID::PID(double* Input, double* Output, double* Setpoint,
|
||||||
double Kp, double Ki, double Kd, int ControllerDirection)
|
double Kp, double Ki, double Kd, double Kf, int ControllerDirection)
|
||||||
{
|
{
|
||||||
PID::SetOutputLimits(0, 255); //default output limit corresponds to
|
PID::SetOutputLimits(0, 255); //default output limit corresponds to
|
||||||
//the arduino pwm limits
|
//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);
|
||||||
|
|
||||||
SampleTime = 100; //default Controller Sample Time is 0.1 seconds
|
lastTime = millis()-sampleTime;
|
||||||
|
inAuto = false;
|
||||||
PID::SetControllerDirection(ControllerDirection);
|
myOutput = Output;
|
||||||
PID::SetTunings(Kp, Ki, Kd);
|
myInput = Input;
|
||||||
|
mySetpoint = Setpoint;
|
||||||
lastTime = millis()-SampleTime;
|
|
||||||
inAuto = false;
|
|
||||||
myOutput = Output;
|
|
||||||
myInput = Input;
|
|
||||||
mySetpoint = Setpoint;
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -47,26 +50,24 @@ void PID::Compute()
|
|||||||
if(!inAuto) return;
|
if(!inAuto) return;
|
||||||
unsigned long now = millis();
|
unsigned long now = millis();
|
||||||
unsigned long timeChange = (now - lastTime);
|
unsigned long timeChange = (now - lastTime);
|
||||||
if(timeChange>=SampleTime)
|
if(timeChange>=sampleTime)
|
||||||
{
|
{
|
||||||
/*Compute all the working error variables*/
|
/*Compute all the working error variables*/
|
||||||
double input = *myInput;
|
double input = *myInput; // read current input condition
|
||||||
double error = *mySetpoint - input;
|
double error = *mySetpoint - input; // error is difference between setpoint and input
|
||||||
ITerm+= (ki * error);
|
iTerm+= (ki * error); // add additional error to the integral term
|
||||||
if(ITerm > outMax) ITerm= outMax;
|
iTerm = constrain( iTerm , -windupI , windupI ); // limit the integral term to +/- windup parameter
|
||||||
else if(ITerm < outMin) ITerm= outMin;
|
double dInput = (input - lastInput); // derivative is based on change in the input
|
||||||
double dInput = (input - lastInput);
|
|
||||||
|
|
||||||
/*Compute PID Output*/
|
/*Compute PID Output*/
|
||||||
double output = kp * error + ITerm- kd * dInput;
|
double output = kp * error + iTerm - kd * dInput + kf * (*mySetpoint - ff_zero);
|
||||||
|
|
||||||
if(output > outMax) output = outMax;
|
output = constrain( output , outMin , outMax ); // limit output to within min/max
|
||||||
else if(output < outMin) output = outMin;
|
*myOutput = output; // write the current output
|
||||||
*myOutput = output;
|
|
||||||
|
|
||||||
/*Remember some variables for next time*/
|
/*Remember some variables for next time*/
|
||||||
lastInput = input;
|
lastInput = input; // last input is for computing the derivative term
|
||||||
lastTime = now;
|
lastTime = now; // last time is for determining when to recompute
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -76,40 +77,60 @@ void PID::Compute()
|
|||||||
* it's called automatically from the constructor, but tunings can also
|
* it's called automatically from the constructor, but tunings can also
|
||||||
* be adjusted on the fly during normal operation
|
* be adjusted on the fly during normal operation
|
||||||
******************************************************************************/
|
******************************************************************************/
|
||||||
void PID::SetTunings(double Kp, double Ki, double Kd)
|
void PID::SetTunings(double Kp, double Ki, double Kd, double Kf)
|
||||||
{
|
{
|
||||||
if (Kp<0 || Ki<0 || Kd<0) return;
|
if (Kp<0 || Ki<0 || Kd<0 || Kf<0) return;
|
||||||
|
|
||||||
dispKp = Kp; dispKi = Ki; dispKd = Kd;
|
dispKp = Kp; dispKi = Ki; dispKd = Kd; dispKf = Kf;
|
||||||
|
|
||||||
double SampleTimeInSec = ((double)SampleTime)/1000;
|
double sampleTimeInSec = ((double)sampleTime)/1000;
|
||||||
kp = Kp;
|
kp = Kp;
|
||||||
ki = Ki * SampleTimeInSec;
|
ki = Ki * sampleTimeInSec;
|
||||||
kd = Kd / SampleTimeInSec;
|
kd = Kd / sampleTimeInSec;
|
||||||
|
kf = Kf;
|
||||||
|
|
||||||
if(controllerDirection ==REVERSE)
|
if(controllerDirection ==REVERSE)
|
||||||
{
|
{
|
||||||
kp = (0 - kp);
|
kp = (0 - kp);
|
||||||
ki = (0 - ki);
|
ki = (0 - ki);
|
||||||
kd = (0 - kd);
|
kd = (0 - kd);
|
||||||
|
kf = (0 - kf);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* SetSampleTime(...) *********************************************************
|
/* SetsampleTime(...) *********************************************************
|
||||||
* sets the period, in Milliseconds, at which the calculation is performed
|
* sets the period, in Milliseconds, at which the calculation is performed
|
||||||
******************************************************************************/
|
******************************************************************************/
|
||||||
void PID::SetSampleTime(int NewSampleTime)
|
void PID::SetSampleTime(int NewsampleTime)
|
||||||
{
|
{
|
||||||
if (NewSampleTime > 0)
|
if (NewsampleTime > 0)
|
||||||
{
|
{
|
||||||
double ratio = (double)NewSampleTime
|
double ratio = (double)NewsampleTime
|
||||||
/ (double)SampleTime;
|
/ (double)sampleTime;
|
||||||
ki *= ratio;
|
ki *= ratio;
|
||||||
kd /= ratio;
|
kd /= ratio;
|
||||||
SampleTime = (unsigned long)NewSampleTime;
|
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(...)****************************************************
|
/* SetOutputLimits(...)****************************************************
|
||||||
* This function will be used far more often than SetInputLimits. while
|
* 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 input to the controller will generally be in the 0-1023 range (which is
|
||||||
@ -121,16 +142,16 @@ void PID::SetSampleTime(int NewSampleTime)
|
|||||||
void PID::SetOutputLimits(double Min, double Max)
|
void PID::SetOutputLimits(double Min, double Max)
|
||||||
{
|
{
|
||||||
if(Min >= Max) return;
|
if(Min >= Max) return;
|
||||||
|
if (outMax) windupI *= Max / outMax;
|
||||||
outMin = Min;
|
outMin = Min;
|
||||||
outMax = Max;
|
outMax = Max;
|
||||||
|
|
||||||
if(inAuto)
|
if(inAuto)
|
||||||
{
|
{
|
||||||
if(*myOutput > outMax) *myOutput = outMax;
|
if(*myOutput > outMax) *myOutput = outMax;
|
||||||
else if(*myOutput < outMin) *myOutput = outMin;
|
else if(*myOutput < outMin) *myOutput = outMin;
|
||||||
|
|
||||||
if(ITerm > outMax) ITerm= outMax;
|
if(iTerm > outMax) iTerm= outMax;
|
||||||
else if(ITerm < outMin) ITerm= outMin;
|
else if(iTerm < outMin) iTerm= outMin;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -155,10 +176,10 @@ void PID::SetMode(int Mode)
|
|||||||
******************************************************************************/
|
******************************************************************************/
|
||||||
void PID::Initialize()
|
void PID::Initialize()
|
||||||
{
|
{
|
||||||
ITerm = *myOutput;
|
iTerm = *myOutput;
|
||||||
lastInput = *myInput;
|
lastInput = *myInput;
|
||||||
if(ITerm > outMax) ITerm = outMax;
|
if(iTerm > windupI) iTerm = windupI;
|
||||||
else if(ITerm < outMin) ITerm = outMin;
|
else if(iTerm < outMin) iTerm = outMin;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* SetControllerDirection(...)*************************************************
|
/* SetControllerDirection(...)*************************************************
|
||||||
@ -171,9 +192,10 @@ void PID::SetControllerDirection(int Direction)
|
|||||||
{
|
{
|
||||||
if(inAuto && Direction !=controllerDirection)
|
if(inAuto && Direction !=controllerDirection)
|
||||||
{
|
{
|
||||||
kp = (0 - kp);
|
kp = (0 - kp);
|
||||||
ki = (0 - ki);
|
ki = (0 - ki);
|
||||||
kd = (0 - kd);
|
kd = (0 - kd);
|
||||||
|
kf = (0 - kf);
|
||||||
}
|
}
|
||||||
controllerDirection = Direction;
|
controllerDirection = Direction;
|
||||||
}
|
}
|
||||||
@ -186,6 +208,8 @@ void PID::SetControllerDirection(int Direction)
|
|||||||
double PID::GetKp(){ return dispKp; }
|
double PID::GetKp(){ return dispKp; }
|
||||||
double PID::GetKi(){ return dispKi;}
|
double PID::GetKi(){ return dispKi;}
|
||||||
double PID::GetKd(){ return dispKd;}
|
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::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
|
||||||
int PID::GetDirection(){ return controllerDirection;}
|
int PID::GetDirection(){ return controllerDirection;}
|
||||||
|
|
||||||
|
@ -1,11 +1,10 @@
|
|||||||
#ifndef PID_v1_h
|
#ifndef PID_v1_h
|
||||||
#define PID_v1_h
|
#define PID_v1_h
|
||||||
#define LIBRARY_VERSION 1.0.0
|
#define LIBRARY_VERSION 1.0.2
|
||||||
|
|
||||||
class PID
|
class PID
|
||||||
{
|
{
|
||||||
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
//Constants used in some of the functions below
|
//Constants used in some of the functions below
|
||||||
@ -15,66 +14,74 @@ class PID
|
|||||||
#define REVERSE 1
|
#define REVERSE 1
|
||||||
|
|
||||||
//commonly used functions **************************************************************************
|
//commonly used functions **************************************************************************
|
||||||
PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and
|
PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and
|
||||||
double, double, double, int); // Setpoint. Initial tuning parameters are also set here
|
double, double, double, double, int); // Setpoint. Initial tuning parameters are also set here
|
||||||
|
// kp, ki, kd, kf, direction
|
||||||
|
|
||||||
void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0)
|
void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0)
|
||||||
|
|
||||||
void Compute(); // * performs the PID calculation. it should be
|
void Compute(); // * performs the PID calculation. it should be
|
||||||
// called every time loop() cycles. ON/OFF and
|
// called every time loop() cycles. ON/OFF and
|
||||||
// calculation frequency can be set using SetMode
|
// calculation frequency can be set using SetMode
|
||||||
// SetSampleTime respectively
|
// SetSampleTime respectively
|
||||||
|
|
||||||
void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but
|
|
||||||
//it's likely the user will want to change this depending on
|
|
||||||
//the application
|
|
||||||
|
|
||||||
|
|
||||||
|
void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but
|
||||||
|
//it's likely the user will want to change this depending on
|
||||||
|
//the application
|
||||||
|
|
||||||
//available but not commonly used functions ********************************************************
|
//available but not commonly used functions ********************************************************
|
||||||
void SetTunings(double, double, // * While most users will set the tunings once in the
|
void SetTunings(double, double, // * While most users will set the tunings once in the
|
||||||
double); // constructor, this function gives the user the option
|
double, double); // constructor, this function gives the user the option
|
||||||
// of changing tunings during runtime for Adaptive control
|
// of changing tunings during runtime for Adaptive control
|
||||||
void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT
|
|
||||||
// means the output will increase when error is positive. REVERSE
|
void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT
|
||||||
// means the opposite. it's very unlikely that this will be needed
|
// means the output will increase when error is positive. REVERSE
|
||||||
// once it is set in the constructor.
|
// means the opposite. it's very unlikely that this will be needed
|
||||||
void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which
|
// once it is set in the constructor.
|
||||||
// the PID calculation is performed. default is 100
|
|
||||||
|
void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which
|
||||||
|
// the PID calculation is performed. default is 100
|
||||||
|
|
||||||
|
void SetWindupI(double); // set the integral windup limit (default is outMax)
|
||||||
|
|
||||||
|
void SetFFzero(double); // set the zero point for the feedforward term (default is 0; for temp control, set to ambient)
|
||||||
|
|
||||||
//Display functions ****************************************************************
|
//Display functions ****************************************************************
|
||||||
double GetKp(); // These functions query the pid for interal values.
|
double GetKp(); // These functions query the pid for interal values.
|
||||||
double GetKi(); // they were created mainly for the pid front-end,
|
double GetKi(); // they were created mainly for the pid front-end,
|
||||||
double GetKd(); // where it's important to know what is actually
|
double GetKd(); // where it's important to know what is actually
|
||||||
int GetMode(); // inside the PID.
|
double GetKf(); // inside the PID.
|
||||||
int GetDirection(); //
|
double GetWi();
|
||||||
|
double GetFFzero();
|
||||||
|
int GetMode();
|
||||||
|
int GetDirection();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void Initialize();
|
void Initialize();
|
||||||
|
|
||||||
double dispKp; // * we'll hold on to the tuning parameters in user-entered
|
double dispKp; // * we'll hold on to the tuning parameters in user-entered
|
||||||
double dispKi; // format for display purposes
|
double dispKi; // format for display purposes
|
||||||
double dispKd; //
|
double dispKd;
|
||||||
|
double dispKf;
|
||||||
|
|
||||||
double kp; // * (P)roportional Tuning Parameter
|
double kp; // * (P)roportional Tuning Parameter
|
||||||
double ki; // * (I)ntegral Tuning Parameter
|
double ki; // * (I)ntegral Tuning Parameter
|
||||||
double kd; // * (D)erivative Tuning Parameter
|
double kd; // * (D)erivative Tuning Parameter
|
||||||
|
double kf; // * (F)eedforward turning parameter
|
||||||
|
double ff_zero; // * feedforward zero point
|
||||||
|
|
||||||
int controllerDirection;
|
int controllerDirection;
|
||||||
|
|
||||||
double *myInput; // * Pointers to the Input, Output, and Setpoint variables
|
double *myInput; // * Pointers to the Input, Output, and Setpoint variables
|
||||||
double *myOutput; // This creates a hard link between the variables and the
|
double *myOutput; // This creates a hard link between the variables and the
|
||||||
double *mySetpoint; // PID, freeing the user from having to constantly tell us
|
double *mySetpoint; // PID, freeing the user from having to constantly tell us
|
||||||
// what these values are. with pointers we'll just know.
|
// what these values are. with pointers we'll just know.
|
||||||
|
|
||||||
unsigned long lastTime;
|
unsigned long lastTime;
|
||||||
double ITerm, lastInput;
|
double iTerm, lastInput;
|
||||||
|
double windupI; // windup limit for integral term
|
||||||
int SampleTime;
|
int sampleTime;
|
||||||
double outMin, outMax;
|
double outMin, outMax;
|
||||||
bool inAuto;
|
bool inAuto;
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue
Block a user