Adding PID library
This commit is contained in:
Fabien Poussin
parent
d4d384557d
commit
d66ce4b75b
|
|
@ -0,0 +1,191 @@
|
|||
/**********************************************************************************************
|
||||
* Arduino PID Library - Version 1.2.1
|
||||
* by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
|
||||
* Modified by Fabien Poussin <fabien.poussin@gmail.com> for ChibiOS.
|
||||
*
|
||||
* This Library is licensed under the MIT License
|
||||
**********************************************************************************************/
|
||||
|
||||
#include "pid.h"
|
||||
#include "osal.h"
|
||||
|
||||
#define TIME_MS (osalOsGetSystemTimeX() / (OSAL_ST_FREQUENCY / 1000))
|
||||
|
||||
/*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.
|
||||
***************************************************************************/
|
||||
void pid_create(pid_t* p, float* Input, float* Output, float* Setpoint,
|
||||
float Kp, float Ki, float Kd, int POn, int ControllerDirection)
|
||||
{
|
||||
p->myOutput = Output;
|
||||
p->myInput = Input;
|
||||
p->mySetpoint = Setpoint;
|
||||
p->inAuto = false;
|
||||
|
||||
pid_setOutputLimits(p, 0, 255); // default output limit corresponds to
|
||||
// the arduino pwm limits
|
||||
|
||||
p->SampleTime = 100; // default Controller Sample Time is 100ms
|
||||
|
||||
pid_setControllerDirection(p, ControllerDirection);
|
||||
pid_setTunings(p, Kp, Ki, Kd, POn);
|
||||
|
||||
p->lastTime = TIME_MS - p->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 pid_compute(pid_t* p)
|
||||
{
|
||||
if(!p->inAuto) return false;
|
||||
unsigned long now = TIME_MS;
|
||||
unsigned long timeChange = (now - p->lastTime);
|
||||
if(timeChange >= p->SampleTime)
|
||||
{
|
||||
/* Compute all the working error variables */
|
||||
float input = *p->myInput;
|
||||
float error = *p->mySetpoint - input;
|
||||
float dInput = (input - p->lastInput);
|
||||
p->outputSum += (p->ki * error);
|
||||
|
||||
/* Add Proportional on Measurement, if P_ON_M is specified */
|
||||
if(!p->pOnE) p->outputSum -= p->kp * dInput;
|
||||
|
||||
if(p->outputSum > p->outMax) p->outputSum = p->outMax;
|
||||
else if(p->outputSum < p->outMin) p->outputSum = p->outMin;
|
||||
|
||||
/* Add Proportional on Error, if P_ON_E is specified */
|
||||
float output;
|
||||
if(p->pOnE) output = p->kp * error;
|
||||
else output = 0;
|
||||
|
||||
/* Compute Rest of PID Output */
|
||||
output += p->outputSum - p->kd * dInput;
|
||||
|
||||
if(output > p->outMax) output = p->outMax;
|
||||
else if(output < p->outMin) output = p->outMin;
|
||||
*p->myOutput = output;
|
||||
|
||||
/* Remember some variables for next time */
|
||||
p->lastInput = input;
|
||||
p->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 pid_setTunings(pid_t* p, float Kp, float Ki, float Kd, int POn)
|
||||
{
|
||||
if (Kp<0 || Ki<0 || Kd<0) return;
|
||||
|
||||
p->pOn = POn;
|
||||
p->pOnE = POn == PID_P_ON_E;
|
||||
|
||||
p->dispKp = Kp; p->dispKi = Ki; p->dispKd = Kd;
|
||||
|
||||
float SampleTimeInSec = ((float)p->SampleTime)/1000;
|
||||
p->kp = Kp;
|
||||
p->ki = Ki * SampleTimeInSec;
|
||||
p->kd = Kd / SampleTimeInSec;
|
||||
|
||||
if(p->controllerDirection == PID_REVERSE)
|
||||
{
|
||||
p->kp = (0 - p->kp);
|
||||
p->ki = (0 - p->ki);
|
||||
p->kd = (0 - p->kd);
|
||||
}
|
||||
}
|
||||
|
||||
/* SetSampleTime(...) *********************************************************
|
||||
* sets the period, in Milliseconds, at which the calculation is performed
|
||||
******************************************************************************/
|
||||
void pid_setSampleTime(pid_t* p, int NewSampleTime)
|
||||
{
|
||||
if (NewSampleTime > 0)
|
||||
{
|
||||
float ratio = (float)NewSampleTime / (float)p->SampleTime;
|
||||
p->ki *= ratio;
|
||||
p->kd /= ratio;
|
||||
p->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 pid_setOutputLimits(pid_t* p, float Min, float Max)
|
||||
{
|
||||
if(Min >= Max) return;
|
||||
p->outMin = Min;
|
||||
p->outMax = Max;
|
||||
|
||||
if(p->inAuto)
|
||||
{
|
||||
if(*p->myOutput > p->outMax) *p->myOutput = p->outMax;
|
||||
else if(*p->myOutput < p->outMin) *p->myOutput = p->outMin;
|
||||
|
||||
if(p->outputSum > p->outMax) p->outputSum = p->outMax;
|
||||
else if(p->outputSum < p->outMin) p->outputSum = p->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(pid_t* p, int Mode)
|
||||
{
|
||||
bool newAuto = (Mode == PID_AUTOMATIC);
|
||||
if(newAuto && !p->inAuto)
|
||||
{ /* we just went from manual to auto */
|
||||
pid_initialize(p);
|
||||
}
|
||||
p->inAuto = newAuto;
|
||||
}
|
||||
|
||||
/* Initialize()****************************************************************
|
||||
* does all the things that need to happen to ensure a bumpless transfer
|
||||
* from manual to automatic mode.
|
||||
******************************************************************************/
|
||||
void pid_initialize(pid_t* p)
|
||||
{
|
||||
p->outputSum = *p->myOutput;
|
||||
p->lastInput = *p->myInput;
|
||||
if(p->outputSum > p->outMax) p->outputSum = p->outMax;
|
||||
else if(p->outputSum < p->outMin) p->outputSum = p->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(pid_t* p, int Direction)
|
||||
{
|
||||
if(p->inAuto && Direction != p->controllerDirection)
|
||||
{
|
||||
p->kp = (0 - p->kp);
|
||||
p->ki = (0 - p->ki);
|
||||
p->kd = (0 - p->kd);
|
||||
}
|
||||
p->controllerDirection = Direction;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
#ifndef PID_h
|
||||
#define PID_h
|
||||
|
||||
#include "chtypes.h"
|
||||
|
||||
//Constants used in some of the functions below
|
||||
#define PID_AUTOMATIC 1
|
||||
#define PID_MANUAL 0
|
||||
#define PID_DIRECT 0
|
||||
#define PID_REVERSE 1
|
||||
#define PID_P_ON_M 0
|
||||
#define PID_P_ON_E 1
|
||||
|
||||
|
||||
typedef struct {
|
||||
|
||||
float dispKp; // * we'll hold on to the tuning parameters in user-entered
|
||||
float dispKi; // format for display purposes
|
||||
float dispKd; //
|
||||
|
||||
float kp; // * (P)roportional Tuning Parameter
|
||||
float ki; // * (I)ntegral Tuning Parameter
|
||||
float kd; // * (D)erivative Tuning Parameter
|
||||
|
||||
int controllerDirection;
|
||||
int pOn;
|
||||
|
||||
float *myInput; // * Pointers to the Input, Output, and Setpoint variables
|
||||
float *myOutput; // This creates a hard link between the variables and the
|
||||
float *mySetpoint; // PID, freeing the user from having to constantly tell us
|
||||
// what these values are. with pointers we'll just know.
|
||||
unsigned long lastTime;
|
||||
float outputSum;
|
||||
float lastInput;
|
||||
|
||||
unsigned long SampleTime;
|
||||
float outMin;
|
||||
float outMax;
|
||||
|
||||
bool inAuto;
|
||||
bool pOnE;
|
||||
|
||||
} pid_t;
|
||||
|
||||
|
||||
//commonly used functions **************************************************************************
|
||||
void pid_create(pid_t* p, float* Input, float* Output, float* Setpoint, // * constructor. links the PID to the Input, Output, and
|
||||
float Kp, float Ki, float Kd, int POn, int ControllerDirection); // Setpoint. Initial tuning parameters are also set here.
|
||||
// (overload for specifying proportional mode)
|
||||
|
||||
void pid_setmode(pid_t* p, int mode); // * sets PID to either Manual (0) or Auto (non-0)
|
||||
|
||||
bool pid_compute(pid_t* p); // * 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 pid_setOutputLimits(pid_t* p, float Min, float Max); // * 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 pid_setTunings(pid_t* p, float Kp, float Ki, float Kd, int POn); // * While most users will set the tunings once in the
|
||||
// constructor, this function gives the user the option
|
||||
// of changing tunings during runtime for Adaptive control
|
||||
|
||||
void pid_setControllerDirection(pid_t* p, int Direction); // * 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 pid_setSampleTime(pid_t* p, int NewSampleTime); // * sets the frequency, in Milliseconds, with which
|
||||
// the PID calculation is performed. default is 100
|
||||
|
||||
void pid_initialize(pid_t* p);
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue