integerPID alternative class
This commit is contained in:
parent
fb095d8cfc
commit
61a2c22ee2
191
PID_v1.cpp
191
PID_v1.cpp
|
@ -11,7 +11,8 @@
|
||||||
#include "WProgram.h"
|
#include "WProgram.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#include <PID_v1.h>
|
#include "PID_v1.h"
|
||||||
|
|
||||||
|
|
||||||
/*Constructor (...)*********************************************************
|
/*Constructor (...)*********************************************************
|
||||||
* The parameters specified here are those for for which we can't set up
|
* The parameters specified here are those for for which we can't set up
|
||||||
|
@ -193,3 +194,191 @@ double PID::GetKd(){ return dispKd;}
|
||||||
int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
|
int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
|
||||||
int PID::GetDirection(){ return controllerDirection;}
|
int PID::GetDirection(){ return controllerDirection;}
|
||||||
|
|
||||||
|
/*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.
|
||||||
|
***************************************************************************/
|
||||||
|
integerPID::integerPID(long* Input, long* Output, long* Setpoint,
|
||||||
|
byte Kp, byte Ki, byte Kd, byte ControllerDirection)
|
||||||
|
{
|
||||||
|
|
||||||
|
myOutput = Output;
|
||||||
|
myInput = Input;
|
||||||
|
mySetpoint = Setpoint;
|
||||||
|
inAuto = false;
|
||||||
|
|
||||||
|
integerPID::SetOutputLimits(0, 255); //default output limit corresponds to
|
||||||
|
//the arduino pwm limits
|
||||||
|
|
||||||
|
//SampleTime = 100; //default Controller Sample Time is 0.1 seconds
|
||||||
|
|
||||||
|
integerPID::SetControllerDirection(ControllerDirection);
|
||||||
|
integerPID::SetTunings(Kp, Ki, Kd);
|
||||||
|
|
||||||
|
//lastTime = millis()-SampleTime;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/* 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. returns true when the output is computed,
|
||||||
|
* false when nothing has been done.
|
||||||
|
**********************************************************************************/
|
||||||
|
bool integerPID::Compute()
|
||||||
|
{
|
||||||
|
if(!inAuto) return false;
|
||||||
|
unsigned long now = millis();
|
||||||
|
SampleTime = (now - lastTime);
|
||||||
|
//if(timeChange>=SampleTime)
|
||||||
|
{
|
||||||
|
/*Compute all the working error variables*/
|
||||||
|
long input = *myInput;
|
||||||
|
long error = *mySetpoint - input;
|
||||||
|
ITerm+= (ki * error)/100;
|
||||||
|
//ITerm+= divs100(ki * error);
|
||||||
|
if(ITerm > outMax) ITerm= outMax;
|
||||||
|
else if(ITerm < outMin) ITerm= outMin;
|
||||||
|
long dInput = (input - lastInput);
|
||||||
|
|
||||||
|
/*Compute PID Output*/
|
||||||
|
long output = (kp * error)/100 + ITerm- (kd * dInput)/100;
|
||||||
|
//long output = divs100(kp * error) + ITerm- divs100(kd * dInput);
|
||||||
|
|
||||||
|
if(output > outMax) output = outMax;
|
||||||
|
else if(output < outMin) output = outMin;
|
||||||
|
*myOutput = output;
|
||||||
|
|
||||||
|
/*Remember some variables for next time*/
|
||||||
|
lastInput = input;
|
||||||
|
//lastTime = now;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
//else return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/* 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 integerPID::SetTunings(byte Kp, byte Ki, byte Kd)
|
||||||
|
{
|
||||||
|
if (Kp<0 || Ki<0 || Kd<0) return;
|
||||||
|
|
||||||
|
dispKp = Kp; dispKi = Ki; dispKd = Kd;
|
||||||
|
|
||||||
|
/*
|
||||||
|
double SampleTimeInSec = ((double)SampleTime)/1000;
|
||||||
|
kp = Kp;
|
||||||
|
ki = Ki * SampleTimeInSec;
|
||||||
|
kd = Kd / SampleTimeInSec;
|
||||||
|
*/
|
||||||
|
long InverseSampleTimeInSec = 100000 / SampleTime;
|
||||||
|
kp = Kp;
|
||||||
|
ki = (Ki * 100) / InverseSampleTimeInSec;
|
||||||
|
kd = (Kd * InverseSampleTimeInSec) / 100;
|
||||||
|
|
||||||
|
if(controllerDirection ==REVERSE)
|
||||||
|
{
|
||||||
|
kp = (0 - kp);
|
||||||
|
ki = (0 - ki);
|
||||||
|
kd = (0 - kd);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* SetSampleTime(...) *********************************************************
|
||||||
|
* sets the period, in Milliseconds, at which the calculation is performed
|
||||||
|
******************************************************************************/
|
||||||
|
void integerPID::SetSampleTime(int NewSampleTime)
|
||||||
|
{
|
||||||
|
if (NewSampleTime > 0)
|
||||||
|
{
|
||||||
|
unsigned long ratioX1000 = (unsigned long)(NewSampleTime * 1000) / (unsigned long)SampleTime;
|
||||||
|
ki = (ki * ratioX1000) / 1000;
|
||||||
|
//kd /= ratio;
|
||||||
|
kd = (kd * 1000) / ratioX1000;
|
||||||
|
SampleTime = (unsigned long)NewSampleTime;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 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 integerPID::SetOutputLimits(long Min, long Max)
|
||||||
|
{
|
||||||
|
if(Min >= Max) return;
|
||||||
|
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 integerPID::SetMode(int Mode)
|
||||||
|
{
|
||||||
|
bool newAuto = (Mode == AUTOMATIC);
|
||||||
|
if(newAuto == !inAuto)
|
||||||
|
{ /*we just went from manual to auto*/
|
||||||
|
integerPID::Initialize();
|
||||||
|
}
|
||||||
|
inAuto = newAuto;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Initialize()****************************************************************
|
||||||
|
* does all the things that need to happen to ensure a bumpless transfer
|
||||||
|
* from manual to automatic mode.
|
||||||
|
******************************************************************************/
|
||||||
|
void integerPID::Initialize()
|
||||||
|
{
|
||||||
|
ITerm = *myOutput;
|
||||||
|
lastInput = *myInput;
|
||||||
|
if(ITerm > outMax) ITerm = outMax;
|
||||||
|
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 integerPID::SetControllerDirection(byte Direction)
|
||||||
|
{
|
||||||
|
if(inAuto && Direction !=controllerDirection)
|
||||||
|
{
|
||||||
|
kp = (0 - kp);
|
||||||
|
ki = (0 - ki);
|
||||||
|
kd = (0 - kd);
|
||||||
|
}
|
||||||
|
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
|
||||||
|
******************************************************************************/
|
||||||
|
byte integerPID::GetKp(){ return dispKp; }
|
||||||
|
byte integerPID::GetKi(){ return dispKi;}
|
||||||
|
byte integerPID::GetKd(){ return dispKd;}
|
||||||
|
int integerPID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
|
||||||
|
int integerPID::GetDirection(){ return controllerDirection;}
|
||||||
|
|
||||||
|
|
77
PID_v1.h
77
PID_v1.h
|
@ -76,5 +76,82 @@ class PID
|
||||||
double outMin, outMax;
|
double outMin, outMax;
|
||||||
bool inAuto;
|
bool inAuto;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class integerPID
|
||||||
|
{
|
||||||
|
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
//Constants used in some of the functions below
|
||||||
|
#define AUTOMATIC 1
|
||||||
|
#define MANUAL 0
|
||||||
|
#define DIRECT 0
|
||||||
|
#define REVERSE 1
|
||||||
|
|
||||||
|
//commonly used functions **************************************************************************
|
||||||
|
integerPID(long*, long*, long*, // * constructor. links the PID to the Input, Output, and
|
||||||
|
byte, byte, byte, byte); // Setpoint. Initial tuning parameters are also set here
|
||||||
|
|
||||||
|
void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0)
|
||||||
|
|
||||||
|
bool Compute(); // * performs the PID calculation. it should be
|
||||||
|
// called every time loop() cycles. ON/OFF and
|
||||||
|
// calculation frequency can be set using SetMode
|
||||||
|
// SetSampleTime respectively
|
||||||
|
|
||||||
|
void SetOutputLimits(long, long); //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 ********************************************************
|
||||||
|
void SetTunings(byte, byte, // * While most users will set the tunings once in the
|
||||||
|
byte); // constructor, this function gives the user the option
|
||||||
|
// of changing tunings during runtime for Adaptive control
|
||||||
|
void SetControllerDirection(byte); // * Sets the Direction, or "Action" of the controller. DIRECT
|
||||||
|
// means the output will increase when error is positive. REVERSE
|
||||||
|
// means the opposite. it's very unlikely that this will be needed
|
||||||
|
// once it is set in the constructor.
|
||||||
|
void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which
|
||||||
|
// the PID calculation is performed. default is 100
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//Display functions ****************************************************************
|
||||||
|
byte GetKp(); // These functions query the pid for interal values.
|
||||||
|
byte GetKi(); // they were created mainly for the pid front-end,
|
||||||
|
byte GetKd(); // where it's important to know what is actually
|
||||||
|
int GetMode(); // inside the PID.
|
||||||
|
int GetDirection(); //
|
||||||
|
|
||||||
|
private:
|
||||||
|
void Initialize();
|
||||||
|
|
||||||
|
byte dispKp; // * we'll hold on to the tuning parameters in user-entered
|
||||||
|
byte dispKi; // format for display purposes
|
||||||
|
byte dispKd; //
|
||||||
|
|
||||||
|
byte kp; // * (P)roportional Tuning Parameter
|
||||||
|
byte ki; // * (I)ntegral Tuning Parameter
|
||||||
|
byte kd; // * (D)erivative Tuning Parameter
|
||||||
|
|
||||||
|
int controllerDirection;
|
||||||
|
|
||||||
|
long *myInput; // * Pointers to the Input, Output, and Setpoint variables
|
||||||
|
long *myOutput; // This creates a hard link between the variables and the
|
||||||
|
long *mySetpoint; // PID, freeing the user from having to constantly tell us
|
||||||
|
// what these values are. with pointers we'll just know.
|
||||||
|
|
||||||
|
unsigned long lastTime;
|
||||||
|
long ITerm, lastInput;
|
||||||
|
|
||||||
|
unsigned long SampleTime;
|
||||||
|
long outMin, outMax;
|
||||||
|
bool inAuto;
|
||||||
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue