Adding PID library

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