pid_controller.cpp

00001 //*********************************************************************************
00002 // Arduino PID Library Version 1.0.1 Modified Version for C++
00003 // Platform Independent
00004 // 
00005 // Revision: 1.1
00006 // 
00007 // Description: The PID Controller module originally meant for Arduino made
00008 // platform independent. Some small bugs present in the original Arduino source
00009 // have been rectified as well.
00010 // 
00011 // For a detailed explanation of the theory behind this library, go to:
00012 // http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
00013 // 
00014 // Revisions can be found here:
00015 // https://github.com/tcleg
00016 // 
00017 // Modified by: Trent Cleghorn , <trentoncleghorn@gmail.com>
00018 // 
00019 // Copyright (C) Brett Beauregard , <br3ttb@gmail.com>
00020 // 
00021 //                                 GPLv3 License
00022 // 
00023 // This program is free software: you can redistribute it and/or modify it under 
00024 // the terms of the GNU General Public License as published by the Free Software 
00025 // Foundation, either version 3 of the License, or (at your option) any later 
00026 // version.
00027 // 
00028 // This program is distributed in the hope that it will be useful, but WITHOUT ANY 
00029 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 
00030 // PARTICULAR PURPOSE.  See the GNU General Public License for more details.
00031 // 
00032 // You should have received a copy of the GNU General Public License along with 
00033 // this program.  If not, see <http://www.gnu.org/licenses/>.
00034 //*********************************************************************************
00035 
00036 //*********************************************************************************
00037 // Headers
00038 //*********************************************************************************
00039 #include "pid_controller.h"
00040 
00041 //*********************************************************************************
00042 // Macros and Globals
00043 //*********************************************************************************
00044 #define CONSTRAIN(x,lower,upper)    ((x)<(lower)?(lower):((x)>(upper)?(upper):(x)))
00045 
00046 //*********************************************************************************
00047 // Public Class Functions
00048 //*********************************************************************************
00049 
00050 PIDControl::
00051 PIDControl (float kp, float ki, float kd, float sampleTimeSeconds, float minOutput, 
00052             float maxOutput, PIDMode mode, PIDDirection controllerDirection)        
00053 {
00054     controllerDirection = controllerDirection;
00055     mode = mode;
00056     iTerm = 0.0f;
00057     input = 0.0f;
00058     lastInput = 0.0f;
00059     output = 0.0f;
00060     setpoint = 0.0f;
00061     
00062     if(sampleTimeSeconds > 0.0f)
00063     {
00064         sampleTime = sampleTimeSeconds;
00065     }
00066     else
00067     {
00068         // If the passed parameter was incorrect, set to 1 second
00069         sampleTime = 1.0f;
00070     }
00071     
00072     PIDOutputLimitsSet(minOutput, maxOutput);
00073     PIDTuningsSet(kp, ki, kd);
00074 }
00075         
00076 bool PIDControl::
00077 PIDCompute() 
00078 {
00079     float error, dInput;
00080 
00081     if(mode == MANUAL)
00082     {
00083         return false;
00084     }
00085     
00086     // The classic PID error term
00087     error = setpoint - input;
00088     
00089     // Compute the integral term separately ahead of time
00090     iTerm += alteredKi * error;
00091     
00092     // Constrain the integrator to make sure it does not exceed output bounds
00093     iTerm = CONSTRAIN(iTerm, outMin, outMax);
00094     
00095     // Take the "derivative on measurement" instead of "derivative on error"
00096     dInput = input - lastInput;
00097     
00098     // Run all the terms together to get the overall output
00099     output = alteredKp * error + iTerm - alteredKd * dInput;
00100     
00101     // Bound the output
00102     output = CONSTRAIN(output, outMin, outMax);
00103     
00104     // Make the current input the former input
00105     lastInput = input;
00106     
00107     return true;
00108 }
00109      
00110 void PIDControl::
00111 PIDModeSet(PIDMode mode)                                                                                                                                       
00112 {
00113     // If the mode changed from MANUAL to AUTOMATIC
00114     if(mode != mode && mode == AUTOMATIC)
00115     {
00116         // Initialize a few PID parameters to new values
00117         iTerm = output;
00118         lastInput = input;
00119         
00120         // Constrain the integrator to make sure it does not exceed output bounds
00121         iTerm = CONSTRAIN(iTerm, outMin, outMax);
00122     }
00123     
00124     mode = mode;
00125 }
00126 
00127 void PIDControl::
00128 PIDOutputLimitsSet(float min, float max)                                                          
00129 {
00130     // Check if the parameters are valid
00131     if(min >= max)
00132     {
00133         return;
00134     }
00135     
00136     // Save the parameters
00137     outMin = min;
00138     outMax = max;
00139     
00140     // If in automatic, apply the new constraints
00141     if(mode == AUTOMATIC)
00142     {
00143         output = CONSTRAIN(output, min, max);
00144         iTerm  = CONSTRAIN(iTerm,  min, max);
00145     }
00146 }
00147 
00148 void PIDControl::
00149 PIDTuningsSet(float kp, float ki, float kd)                                                      
00150 {
00151     // Check if the parameters are valid
00152     if(kp < 0.0f || ki < 0.0f || kd < 0.0f)
00153     {
00154         return;
00155     }
00156     
00157     // Save the parameters for displaying purposes
00158     dispKp = kp;
00159     dispKi = ki;
00160     dispKd = kd;
00161     
00162     // Alter the parameters for PID
00163     alteredKp = kp;
00164     alteredKi = ki * sampleTime;
00165     alteredKd = kd / sampleTime;
00166     
00167     // Apply reverse direction to the altered values if necessary
00168     if(controllerDirection == REVERSE)
00169     {
00170         alteredKp = -(alteredKp);
00171         alteredKi = -(alteredKi);
00172         alteredKd = -(alteredKd);
00173     }
00174 }
00175 
00176 void PIDControl::
00177 PIDTuningKpSet(float kp)
00178 {
00179     PIDTuningsSet(kp, dispKi, dispKd);
00180 }
00181 
00182 void PIDControl::
00183 PIDTuningKiSet(float ki)
00184 {
00185     PIDTuningsSet(dispKp, ki, dispKd);
00186 }
00187 
00188 void PIDControl::
00189 PIDTuningKdSet(float kd)
00190 {
00191     PIDTuningsSet(dispKp, dispKi, kd);
00192 }
00193 
00194 void PIDControl::
00195 PIDControllerDirectionSet(PIDDirection controllerDirection)                                                                                                               
00196 {
00197     // If in automatic mode and the controller's sense of direction is reversed
00198     if(mode == AUTOMATIC && controllerDirection == REVERSE)
00199     {
00200         // Reverse sense of direction of PID gain constants
00201         alteredKp = -(alteredKp);
00202         alteredKi = -(alteredKi);
00203         alteredKd = -(alteredKd);
00204     }
00205     
00206     controllerDirection = controllerDirection;
00207 }
00208 
00209 void PIDControl::
00210 PIDSampleTimeSet(float sampleTimeSeconds)                                                                                                                                                                      
00211 {
00212     float ratio;
00213 
00214     if(sampleTimeSeconds > 0.0f)
00215     {
00216         // Find the ratio of change and apply to the altered values
00217         ratio = sampleTimeSeconds / sampleTime;
00218         alteredKi *= ratio;
00219         alteredKd /= ratio;
00220         
00221         // Save the new sampling time
00222         sampleTime = sampleTimeSeconds;
00223     }
00224 }
00225