PID.cpp

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