diff options
Diffstat (limited to 'os')
| -rw-r--r-- | os/various/pid.c | 191 | ||||
| -rw-r--r-- | os/various/pid.h | 78 |
2 files changed, 269 insertions, 0 deletions
diff --git a/os/various/pid.c b/os/various/pid.c new file mode 100644 index 0000000..fa9b92f --- /dev/null +++ b/os/various/pid.c @@ -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; +} + diff --git a/os/various/pid.h b/os/various/pid.h new file mode 100644 index 0000000..d98df3e --- /dev/null +++ b/os/various/pid.h @@ -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 |