PID_v1.cpp

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