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/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(float* Input, float* Output, float* Setpoint,
00060         float Kp, float Ki, float 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    if(timeChange>=SampleTime)
00091    {
00092       /*Compute all the working error variables*/
00093       float input = *myInput;
00094       float error = *mySetpoint - input;
00095       ITerm+= (ki * error);
00096       if(ITerm > outMax) ITerm= outMax;
00097       else if(ITerm < outMin) ITerm= outMin;
00098       float dInput = (input - lastInput);
00099 // pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint);
00100  
00101       /*Compute PID Output*/
00102       float output = kp * error + ITerm- kd * dInput;
00103 // pc.printf("Input = %f, Error = %f, Output = %f, SetPoint = %f\n\r",input,error,output,*mySetpoint);
00104       
00105       if(output > outMax) output = outMax;
00106       else if(output < outMin) output = outMin;
00107       *myOutput = output;
00108  
00109       /*Remember some variables for next time*/
00110       lastInput = input;
00111       lastTime = now;
00112       return true;
00113    }
00114    else return false;
00115 }
00116 
00117 
00118 /* SetTunings(...)*************************************************************
00119  * This function allows the controller's dynamic performance to be adjusted. 
00120  * it's called automatically from the constructor, but tunings can also
00121  * be adjusted on the fly during normal operation
00122  ******************************************************************************/ 
00123 void PID::SetTunings(float Kp, float Ki, float Kd)
00124 {
00125    if (Kp<0 || Ki<0 || Kd<0) return;
00126  
00127    dispKp = Kp; dispKi = Ki; dispKd = Kd;
00128    
00129    float SampleTimeInSec = ((float)SampleTime)/1000;  
00130    kp = Kp;
00131    ki = Ki * SampleTimeInSec;
00132    kd = Kd / SampleTimeInSec;
00133  
00134   if(controllerDirection ==REVERSE)
00135    {
00136       kp = (0 - kp);
00137       ki = (0 - ki);
00138       kd = (0 - kd);
00139    }
00140 }
00141   
00142 /* SetSampleTime(...) *********************************************************
00143  * sets the period, in Milliseconds, at which the calculation is performed  
00144  ******************************************************************************/
00145 void PID::SetSampleTime(int NewSampleTime)
00146 {
00147    if (NewSampleTime > 0)
00148    {
00149       float ratio  = (float)NewSampleTime
00150                       / (float)SampleTime;
00151       ki *= ratio;
00152       kd /= ratio;
00153       SampleTime = (unsigned long)NewSampleTime;
00154    }
00155 }
00156  
00157 /* SetOutputLimits(...)****************************************************
00158  *     This function will be used far more often than SetInputLimits.  while
00159  *  the input to the controller will generally be in the 0-1023 range (which is
00160  *  the default already,)  the output will be a little different.  maybe they'll
00161  *  be doing a time window and will need 0-8000 or something.  or maybe they'll
00162  *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
00163  *  here.
00164  **************************************************************************/
00165 void PID::SetOutputLimits(float Min, float Max)
00166 {
00167    if(Min >= Max) return;
00168    outMin = Min;
00169    outMax = Max;
00170  
00171    if(inAuto)
00172    {
00173        if(*myOutput > outMax) *myOutput = outMax;
00174        else if(*myOutput < outMin) *myOutput = outMin;
00175      
00176        if(ITerm > outMax) ITerm= outMax;
00177        else if(ITerm < outMin) ITerm= outMin;
00178    }
00179 }
00180 
00181 /* SetMode(...)****************************************************************
00182  * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
00183  * when the transition from manual to auto occurs, the controller is
00184  * automatically initialized
00185  ******************************************************************************/ 
00186 void PID::SetMode(int Mode)
00187 {
00188     bool newAuto = (Mode == AUTOMATIC);
00189     if(newAuto == !inAuto)
00190     {  /*we just went from manual to auto*/
00191         PID::Initialize();
00192     }
00193     inAuto = newAuto;
00194 }
00195  
00196 /* Initialize()****************************************************************
00197  *  does all the things that need to happen to ensure a bumpless transfer
00198  *  from manual to automatic mode.
00199  ******************************************************************************/ 
00200 void PID::Initialize()
00201 {
00202    ITerm = *myOutput;
00203    lastInput = *myInput;
00204    if(ITerm > outMax) ITerm = outMax;
00205    else if(ITerm < outMin) ITerm = outMin;
00206 }
00207 
00208 /* SetControllerDirection(...)*************************************************
00209  * The PID will either be connected to a DIRECT acting process (+Output leads 
00210  * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
00211  * know which one, because otherwise we may increase the output when we should
00212  * be decreasing.  This is called from the constructor.
00213  ******************************************************************************/
00214 void PID::SetControllerDirection(int Direction)
00215 {
00216    if(inAuto && Direction !=controllerDirection)
00217    {
00218       kp = (0 - kp);
00219       ki = (0 - ki);
00220       kd = (0 - kd);
00221    }   
00222    controllerDirection = Direction;
00223 }
00224 
00225 /* Status Funcions*************************************************************
00226  * Just because you set the Kp=-1 doesn't mean it actually happened.  these
00227  * functions query the internal state of the PID.  they're here for display 
00228  * purposes.  this are the functions the PID Front-end uses for example
00229  ******************************************************************************/
00230 float PID::GetKp(){ return  dispKp; }
00231 float PID::GetKi(){ return  dispKi;}
00232 float PID::GetKd(){ return  dispKd;}
00233 int PID::GetMode(){ return  inAuto ? AUTOMATIC : MANUAL;}
00234 int PID::GetDirection(){ return controllerDirection;}
00235