Merge f4193d845e
into 20199df217
This commit is contained in:
commit
d39a11d987
@ -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
|
||||||
|
|
@ -3,7 +3,7 @@
|
|||||||
|
|
||||||
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
|
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
|
||||||
|
|
||||||
*
|
** Modified by Jason Melvin <jwmelvin@gmail.com>
|
||||||
|
|
||||||
* This Library is licensed under a GPLv3 License
|
* This Library is licensed under a GPLv3 License
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user