PID.cpp

00001 /**
00002  * Arduino PID Library - Version 1.1.1
00003  * @author Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
00004  *
00005  * @section LICENSE
00006  *
00007  * Permission is hereby granted, free of charge, to any person obtaining a copy
00008  * of this software and associated documentation files (the "Software"), to deal
00009  * in the Software without restriction, including without limitation the rights
00010  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00011  * copies of the Software, and to permit persons to whom the Software is
00012  * furnished to do so, subject to the following conditions:
00013  *
00014  * The above copyright notice and this permission notice shall be included in
00015  * all copies or substantial portions of the Software.
00016  *
00017  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00018  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00019  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00020  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00021  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00022  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00023  * THE SOFTWARE.
00024  *
00025  * @section DESCRIPTION
00026  * 
00027  * A PID controller is a widely used feedback controller commonly found in
00028  * industry.
00029  *
00030  * This library is a port of Brett Beauregard's Arduino PID library:
00031  *
00032  *  https://github.com/br3ttb/Arduino-PID-Library
00033  *
00034  * The wikipedia article on PID controllers is a good place to start on
00035  * understanding how they work:
00036  *
00037  *  http://en.wikipedia.org/wiki/PID_controller
00038  *
00039  * For a clear and elegant explanation of how to implement and tune a
00040  * controller, the controlguru website by Douglas J. Cooper (who also happened
00041  * to be Brett's controls professor) is an excellent reference:
00042  *
00043  *  http://www.controlguru.com/
00044  */
00045  
00046 /**
00047  * Includes
00048  */
00049 #include "PID.h"
00050 #include "millis.h"
00051  
00052 //extern Serial pc;
00053  
00054 
00055 /*Constructor (...)*********************************************************
00056  *    The parameters specified here are those for for which we can't set up 
00057  *    reliable defaults, so we need to have the user set them.
00058  ***************************************************************************/
00059 PID::PID(double* Input, double* Output, double* Setpoint,
00060         double Kp, double Ki, double Kd, int ControllerDirection)
00061 {
00062     
00063     myOutput = Output;
00064     myInput = Input;
00065     mySetpoint = Setpoint;
00066     inAuto = false;
00067     
00068     PID::SetOutputLimits(0, 1000);             // default output limit corresponds to 
00069                                                 // the arduino pwm limits                                              
00070     SampleTime = 100;                           // default Controller Sample Time is 0.1 seconds
00071 
00072     PID::SetControllerDirection(ControllerDirection);
00073     PID::SetTunings(Kp, Ki, Kd);
00074 
00075     lastTime = millis()-SampleTime;          
00076 }
00077  
00078  
00079 /* Compute() **********************************************************************
00080  *     This, as they say, is where the magic happens.  this function should be called
00081  *   every time "void loop()" executes.  the function will decide for itself whether a new
00082  *   pid Output needs to be computed.  returns true when the output is computed,
00083  *   false when nothing has been done.
00084  **********************************************************************************/ 
00085 bool PID::Compute()
00086 {
00087    if(!inAuto) return false;
00088    unsigned long now = millis();
00089    unsigned long timeChange = (now - lastTime);
00090    
00091    if(timeChange>=SampleTime)
00092    {
00093       /*Compute all the working error variables*/
00094       double input = *myInput;
00095       double error = *mySetpoint - input;
00096       ITerm+= (ki * error);
00097       if(ITerm > outMax) ITerm= outMax;
00098       else if(ITerm < outMin) ITerm= outMin;
00099       double dInput = (input - lastInput);
00100 // pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint);
00101  
00102       /*Compute PID Output*/
00103       double output = kp * error + ITerm- kd * dInput;
00104  //pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint);
00105       
00106       if(output > outMax) output = outMax;
00107       else if(output < outMin) output = outMin;
00108       *myOutput = output;
00109  
00110       /*Remember some variables for next time*/
00111       lastInput = input;
00112       lastTime = now;
00113       return true;
00114    }
00115    else return false;
00116 }
00117 
00118 
00119 /* SetTunings(...)*************************************************************
00120  * This function allows the controller's dynamic performance to be adjusted. 
00121  * it's called automatically from the constructor, but tunings can also
00122  * be adjusted on the fly during normal operation
00123  ******************************************************************************/ 
00124 void PID::SetTunings(double Kp, double Ki, double Kd)
00125 {
00126    if (Kp<0 || Ki<0 || Kd<0) return;
00127  
00128    dispKp = Kp; dispKi = Ki; dispKd = Kd;
00129    
00130    double SampleTimeInSec = ((double)SampleTime)/1000;  
00131    kp = Kp;
00132    ki = Ki * SampleTimeInSec;
00133    kd = Kd / SampleTimeInSec;
00134  
00135   if(controllerDirection ==REVERSE)
00136    {
00137       kp = (0 - kp);
00138       ki = (0 - ki);
00139       kd = (0 - kd);
00140    }
00141 }
00142   
00143 /* SetSampleTime(...) *********************************************************
00144  * sets the period, in Milliseconds, at which the calculation is performed  
00145  ******************************************************************************/
00146 void PID::SetSampleTime(int NewSampleTime)
00147 {
00148    if (NewSampleTime > 0)
00149    {
00150       double ratio  = (double)NewSampleTime
00151                       / (double)SampleTime;
00152       ki *= ratio;
00153       kd /= ratio;
00154       SampleTime = (unsigned long)NewSampleTime;
00155    }
00156 }
00157  
00158 /* SetOutputLimits(...)****************************************************
00159  *     This function will be used far more often than SetInputLimits.  while
00160  *  the input to the controller will generally be in the 0-1023 range (which is
00161  *  the default already,)  the output will be a little different.  maybe they'll
00162  *  be doing a time window and will need 0-8000 or something.  or maybe they'll
00163  *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
00164  *  here.
00165  **************************************************************************/
00166 void PID::SetOutputLimits(double Min, double Max)
00167 {
00168    if(Min >= Max) return;
00169    outMin = Min;
00170    outMax = Max;
00171  
00172    if(inAuto)
00173    {
00174        if(*myOutput > outMax) *myOutput = outMax;
00175        else if(*myOutput < outMin) *myOutput = outMin;
00176      
00177        if(ITerm > outMax) ITerm= outMax;
00178        else if(ITerm < outMin) ITerm= outMin;
00179    }
00180 }
00181 
00182 /* SetMode(...)****************************************************************
00183  * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
00184  * when the transition from manual to auto occurs, the controller is
00185  * automatically initialized
00186  ******************************************************************************/ 
00187 void PID::SetMode(int Mode)
00188 {
00189     bool newAuto = (Mode == AUTOMATIC);
00190     if(newAuto == !inAuto)
00191     {  /*we just went from manual to auto*/
00192         PID::Initialize();
00193     }
00194     inAuto = newAuto;
00195 }
00196  
00197 /* Initialize()****************************************************************
00198  *  does all the things that need to happen to ensure a bumpless transfer
00199  *  from manual to automatic mode.
00200  ******************************************************************************/ 
00201 void PID::Initialize()
00202 {
00203    ITerm = *myOutput;
00204    lastInput = *myInput;
00205    if(ITerm > outMax) ITerm = outMax;
00206    else if(ITerm < outMin) ITerm = outMin;
00207 }
00208 
00209 /* SetControllerDirection(...)*************************************************
00210  * The PID will either be connected to a DIRECT acting process (+Output leads 
00211  * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
00212  * know which one, because otherwise we may increase the output when we should
00213  * be decreasing.  This is called from the constructor.
00214  ******************************************************************************/
00215 void PID::SetControllerDirection(int Direction)
00216 {
00217    if(inAuto && Direction !=controllerDirection)
00218    {
00219       kp = (0 - kp);
00220       ki = (0 - ki);
00221       kd = (0 - kd);
00222    }   
00223    controllerDirection = Direction;
00224 }
00225 
00226 /* Status Funcions*************************************************************
00227  * Just because you set the Kp=-1 doesn't mean it actually happened.  these
00228  * functions query the internal state of the PID.  they're here for display 
00229  * purposes.  this are the functions the PID Front-end uses for example
00230  ******************************************************************************/
00231 double PID::GetKp(){ return  dispKp; }
00232 double PID::GetKi(){ return  dispKi;}
00233 double PID::GetKd(){ return  dispKd;}
00234 int PID::GetMode(){ return  inAuto ? AUTOMATIC : MANUAL;}
00235 int PID::GetDirection(){ return controllerDirection;}
00236