pid.py
Go to the documentation of this file.
00001 #
00002 #  Description:  PID (Proportional, Integral, Derivative) control output.
00003 #
00004 #  Copyright (C) 2005 Austin Robot Technology, Jack O'Quin
00005 #  Copyright (C) 2008 Austin Robot Technology, Patrick Beeson
00006 #
00007 #  License: Modified BSD Software License Agreement
00008 #
00009 
00010 
00011 #/**  @file
00012 #   
00013 #  @brief PID (Proportional, Integral, Derivative) control output.
00014 # */
00015 
00016 PKG_NAME = 'art_common'
00017 import roslib;
00018 roslib.load_manifest(PKG_NAME)
00019 import rospy
00020 import math
00021 
00022 FLT_MAX = 1e100                         # really large floating point value
00023 
00024 #/** @brief PID (Proportional, Integral, Derivative) control output. */
00025 class Pid(object):
00026 
00027     def __init__(self, ctlname, kp=1.0, ki=0.0, kd=0.0,
00028               omax=FLT_MAX, omin=-FLT_MAX, C=0.0):
00029         """ @brief Constructor
00030             @param ctlname control output name for log messages
00031         """
00032 
00033         # PID control parameters
00034         self.name = ctlname             # control name for logging
00035         self.kp = kp                    # proportional gain
00036         self.ki = ki                    # integral gain
00037         self.kd = kd                    # derivative gain
00038         self.omax = omax                # output maximum limit
00039         self.omin = omin                # output minimum limit
00040         self.C = C                      # tracker to adapt integral
00041         self.Clear()
00042   
00043     def Configure(self):
00044         "@brief Configure PID parameters"
00045 
00046         # configure PID constants
00047 
00048         # CfgParam was changed to return the new value.
00049         # I've made a slight modification so that this function
00050         # catches the return value.
00051         self.kp = self.CfgParam("kp", self.kp)
00052         self.ki = self.CfgParam("ki", self.ki)
00053         self.kd = self.CfgParam("kd", self.kd)
00054         rospy.logdebug("%s gains (%.3f, %.3f, %.3f)",
00055                        self.name, self.kp, self.ki, self.kd)
00056         self.omax = self.CfgParam("omax", self.omax)
00057         self.omin = self.CfgParam("omin", self.omin)
00058         self.C = self.CfgParam("C", self.C);
00059         rospy.logdebug("%s output range [%.1f, %.1f]",
00060                        self.name, self.omin, self.omax)
00061   
00062     def Update(self, error, output):
00063         """ @brief Update PID control output.
00064         
00065             @param error current output error
00066             @param output current output level
00067             @returns recommended change in output
00068         """
00069 
00070         if (self.starting):
00071             self.istate=0
00072             self.dstate=output
00073             self.starting=False
00074     
00075         # Proportional term
00076         p = self.kp * error
00077     
00078         # Derivative term
00079         d = self.kd * (output - self.dstate)
00080         self.dstate = output            # previous output level
00081 
00082         # Integral term
00083         i = self.ki * self.istate       # integrator state
00084         PID_control = (p + i - d)
00085 
00086         rospy.logdebug("%s PID: %.3f = %.3f + %.3f - %.3f",
00087                        self.name, PID_control, p, i, d)
00088 
00089         PID_out=PID_control
00090 
00091         if (PID_control > self.omax):
00092             PID_out=self.omax
00093         if (PID_control < self.omin):
00094             PID_out=self.omin
00095 
00096         # Integral term -- In reading other code, I is calculated
00097         # after the output.
00098         #
00099         # The C term reduces the integral when the controller is
00100         # already pushing as hard as it can.
00101         self.istate = self.istate + error
00102         tracking = self.C*(PID_out-PID_control)
00103         if ((self.istate > 0 and -tracking > self.istate)
00104             or (self.istate < 0 and -tracking < self.istate)):
00105             self.istate = 0
00106         else:
00107             self.istate = self.istate + tracking
00108 
00109         if (math.isnan(self.istate) or math.isinf(self.istate)):
00110             self.istate=0
00111 
00112         rospy.logdebug("%s istate = %.3f, PID_out: %.3f, "
00113                        + "C*(PID_out-PID_control):%.3f",
00114                        self.name, self.istate, PID_out,
00115                        self.C*(PID_out-PID_control))
00116 
00117         return PID_out
00118 
00119     def Clear(self):
00120         """#/** @brief Clears the integral term if the setpoint
00121                        has been reached */
00122         """
00123         self.starting=True
00124 
00125     def CopyHistory(self, pid):
00126         """
00127         #/** @brief Copy the error history from another PID
00128         # *  @param pid The PID controller that has the history to copy
00129         # */
00130         """
00131 
00132         # These values do not depend on the constants of the other
00133         # PID, so they're safe to copy
00134         self.dstate = pid.dstate
00135         self.istate = pid.istate
00136         # Check if we were called on an unused PID for whatever reason
00137         self.starting = pid.starting
00138   
00139     def CfgParam(self, pname, fvalue):
00140         """
00141         /** @brief Configure one PID parameter
00142          *  @param node node handle for parameter server -- #NOTE: Removed by David
00143          *  @param pname base name for this parameter
00144          *  @returns parameter value, if defined
00145          *           (already set to default value).
00146          */
00147         """
00148         # Original program logic
00149         """
00150         optname = self.name + '_' + pname
00151         if (node.getParamCached(optname, fvalue)):
00152 
00153             param_value = dvalue         # convert double to float
00154             if (fvalue != param_value): # new value?
00155 
00156                 rospy.info("%s changed to %.3f", optname, param_value)
00157                 fvalue = param_value
00158         return fvalue
00159         """
00160         
00161         # The original program logic used something called a NodeHandle,
00162         # supported by roscpp. However, rospy appears to have no such thing. 
00163         # Also, when this function is called, the variables in the function
00164         # call are passed by value, so this function has to return the new
00165         # value and the calling function has to catch the return value.
00166         # So I've changed both functions a bit.        
00167         optname = self.name + '_' + pname
00168         if (rospy.has_param(optname)) :
00169           param_value = rospy.get_param(optname)
00170           if (fvalue != param_value) :
00171             rospy.loginfo("%s changed to %.3f", optname, param_value)
00172             fvalue = param_value
00173         return fvalue


art_common
Author(s): Austin Robot Technology
autogenerated on Fri Jan 3 2014 11:08:22