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


lizi_arduino
Author(s): RoboTiCan
autogenerated on Wed Aug 26 2015 12:24:22