This commit is contained in:
GitHub Merge Button 2012-01-24 07:06:22 -08:00
commit d39a11d987
3 changed files with 131 additions and 100 deletions

View File

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

View File

@ -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

View File

@ -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