PID_v2.cpp

00001 /**********************************************************************************************
00002  * Arduino PID Library - Version 1.1.1
00003  * by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
00004  *
00005  * This Library is licensed under a GPLv3 License
00006  **********************************************************************************************/
00007 #if 0
00008 #if ARDUINO >= 100
00009 #include "Arduino.h"
00010 #else
00011 #include "WProgram.h"
00012 #endif
00013 #endif
00014 #include "mbed.h"
00015 #include <PID_v2.h>
00016 
00017 extern Timer g_Timer;
00018 #define constrain(x,a,b) ((x) < (a) ? (a) : ((x) > (b) ? (b) : (x)))
00019 
00020 /*Constructor (...)*********************************************************
00021  *    The parameters specified here are those for for which we can't set up
00022  *    reliable defaults, so we need to have the user set them.
00023  ***************************************************************************/
00024 PID::PID(double Kp, double Ki, double Kd, int ControllerDirection)
00025 {
00026 
00027 //   myOutput = Output;
00028 //   myInput = Input;
00029 //   mySetpoint = Setpoint;
00030     inAuto = false;
00031 
00032     PID::SetOutputLimits(0, 255);               //default output limit corresponds to
00033     PID::SetITermLimits(0, 255);
00034     //the arduino pwm limits
00035 
00036     SampleTime = 1;                         //default Controller Sample Time is 0.1 seconds
00037 
00038     PID::SetControllerDirection(ControllerDirection);
00039     PID::SetTunings(Kp, Ki, Kd);
00040 
00041     //lastTime = micros()/1000 - SampleTime;
00042     lastTime = g_Timer.read_us() / 1000 - SampleTime;
00043 }
00044 
00045 
00046 /* Compute() **********************************************************************
00047  *     This, as they say, is where the magic happens.  this function should be called
00048  *   every time "void loop()" executes.  the function will decide for itself whether a new
00049  *   pid Output needs to be computed.  returns true when the output is computed,
00050  *   false when nothing has been done.
00051  **********************************************************************************/
00052 float PID::Compute(float Input, float Setpoint)
00053 {
00054     float output = 0;
00055     if(!inAuto) return output;
00056     //unsigned long now = micros();
00057     unsigned long now = g_Timer.read_us();
00058     float dt = (now - lastTime)/1000.0;
00059 
00060     //if(dt>=SampleTime) {
00061     if (1) {
00062         /*Compute all the working error variables*/
00063         float input = Input;
00064         float error = Setpoint - input;
00065         ITerm += error * dt;
00066         ITerm = constrain(ITerm, errorMin, errorMax);
00067         float dInput = (input - lastInput)/dt;
00068 
00069         /*Compute PID Output*/
00070         output = kp * error + ki * ITerm - kd * dInput;
00071 
00072         output = constrain(output, outMin, outMax);
00073 //    *myOutput = output;
00074 
00075         /*Remember some variables for next time*/
00076         lastInput = input;
00077         lastTime = now;
00078         //  return true;
00079     }
00080     return output;
00081 }
00082 
00083 
00084 /* SetTunings(...)*************************************************************
00085  * This function allows the controller's dynamic performance to be adjusted.
00086  * it's called automatically from the constructor, but tunings can also
00087  * be adjusted on the fly during normal operation
00088  ******************************************************************************/
00089 void PID::SetTunings(double Kp, double Ki, double Kd)
00090 {
00091     if (Kp<0 || Ki<0 || Kd<0) return;
00092 
00093     //double SampleTimeInSec = ((double)SampleTime)/1000;
00094     kp = Kp;
00095     ki = Ki;
00096     kd = Kd;
00097 
00098     dispKp = Kp;
00099     dispKi = Ki;
00100     dispKd = Kd;
00101 
00102     if(controllerDirection ==REVERSE) {
00103         kp = (0 - kp);
00104         ki = (0 - ki);
00105         kd = (0 - kd);
00106     }
00107 }
00108 
00109 /* SetSampleTime(...) *********************************************************
00110  * sets the period, in Milliseconds, at which the calculation is performed
00111  ******************************************************************************/
00112 void PID::SetSampleTime(int NewSampleTime)
00113 {
00114     if (NewSampleTime > 0) {
00115         double ratio  = (double)NewSampleTime
00116                         / (double)SampleTime;
00117         ki *= ratio;
00118         kd /= ratio;
00119         SampleTime = (unsigned long)NewSampleTime;
00120     }
00121 }
00122 
00123 /* SetOutputLimits(...)****************************************************
00124  *     This function will be used far more often than SetInputLimits.  while
00125  *  the input to the controller will generally be in the 0-1023 range (which is
00126  *  the default already,)  the output will be a little different.  maybe they'll
00127  *  be doing a time window and will need 0-8000 or something.  or maybe they'll
00128  *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
00129  *  here.
00130  **************************************************************************/
00131 void PID::SetOutputLimits(double Min, double Max)
00132 {
00133     if(Min >= Max) return;
00134     outMin = Min;
00135     outMax = Max;
00136 
00137     if(inAuto)
00138         *myOutput = constrain(*myOutput, outMin, outMax);
00139 }
00140 
00141 void PID::SetITermLimits(double Min, double Max)
00142 {
00143     if(Min >= Max) return;
00144     errorMin = Min;
00145     errorMax = Max;
00146 
00147     if(inAuto)
00148         ITerm = constrain(ITerm, errorMin, errorMax);
00149 }
00150 
00151 /* SetMode(...)****************************************************************
00152  * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
00153  * when the transition from manual to auto occurs, the controller is
00154  * automatically initialized
00155  ******************************************************************************/
00156 void PID::SetMode(int Mode)
00157 {
00158     bool newAuto = (Mode == AUTOMATIC);
00159     if(newAuto == !inAuto) {
00160         /*we just went from manual to auto*/
00161         PID::Initialize();
00162     }
00163     inAuto = newAuto;
00164 }
00165 
00166 /* Initialize()****************************************************************
00167  *  does all the things that need to happen to ensure a bumpless transfer
00168  *  from manual to automatic mode.
00169  ******************************************************************************/
00170 void PID::Initialize()
00171 {
00172     ITerm = *myOutput;
00173     lastInput = *myInput;
00174     ITerm = constrain(ITerm, errorMin, errorMax);
00175 }
00176 
00177 /* SetControllerDirection(...)*************************************************
00178  * The PID will either be connected to a DIRECT acting process (+Output leads
00179  * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
00180  * know which one, because otherwise we may increase the output when we should
00181  * be decreasing.  This is called from the constructor.
00182  ******************************************************************************/
00183 void PID::SetControllerDirection(int Direction)
00184 {
00185     if(inAuto && Direction !=controllerDirection) {
00186         kp = (0 - kp);
00187         ki = (0 - ki);
00188         kd = (0 - kd);
00189     }
00190     controllerDirection = Direction;
00191 }
00192 
00193 /* Status Funcions*************************************************************
00194  * Just because you set the Kp=-1 doesn't mean it actually happened.  these
00195  * functions query the internal state of the PID.  they're here for display
00196  * purposes.  this are the functions the PID Front-end uses for example
00197  ******************************************************************************/
00198 double PID::GetKp()
00199 {
00200     return  dispKp;
00201 }
00202 double PID::GetKi()
00203 {
00204     return  dispKi;
00205 }
00206 double PID::GetKd()
00207 {
00208     return  dispKd;
00209 }
00210 int PID::GetMode()
00211 {
00212     return  inAuto ? AUTOMATIC : MANUAL;
00213 }
00214 int PID::GetDirection()
00215 {
00216     return controllerDirection;
00217 }
00218 
00219