Source Code for Module baxter_control.pid

 1  # Modified example source code from the book 
 2  # "Real-World Instrumentation with Python" 
 3  # by J. M. Hughes, published by O'Reilly Media, December 2010, 
 4  # ISBN 978-0-596-80956-0. 
 5   
 6  import rospy 
 7   
 8   
 9 -class PID(object): 
10      """ 
11      PID control class 
12   
13      This class implements a simplistic PID control algorithm. When first 
14      instantiated all the gain variables are set to zero, so calling 
15      the method compute_output will just return zero. 
16      """ 
17   
18 -    def __init__(self, kp=0.0, ki=0.0, kd=0.0): 
19          # initialize gains 
20          self._kp = kp 
21          self._ki = ki 
22          self._kd = kd 
23   
24          # initialize error, results, and time descriptors 
25          self._prev_err = 0.0 
26          self._cp = 0.0 
27          self._ci = 0.0 
28          self._cd = 0.0 
29          self._cur_time = 0.0 
30          self._prev_time = 0.0 
31   
32          self.initialize() 
33   
34 -    def initialize(self): 
35          """ 
36          Initialize pid controller. 
37          """ 
38          # reset delta t variables 
39          self._cur_time = rospy.get_time() 
40          self._prev_time = self._cur_time 
41   
42          self._prev_err = 0.0 
43   
44          # reset result variables 
45          self._cp = 0.0 
46          self._ci = 0.0 
47          self._cd = 0.0 
48   
49 -    def set_kp(self, invar): 
50          """ 
51          Set proportional gain. 
52          """ 
53          self._kp = invar 
54   
55 -    def set_ki(self, invar): 
56          """ 
57          Set integral gain. 
58          """ 
59          self._ki = invar 
60   
61 -    def set_kd(self, invar): 
62          """ 
63          Set derivative gain. 
64          """ 
65          self._kd = invar 
66   
67 -    def compute_output(self, error): 
68          """ 
69          Performs a PID computation and returns a control value based on 
70          the elapsed time (dt) and the error signal from a summing junction 
71          (the error parameter). 
72          """ 
73          self._cur_time = rospy.get_time()  # get t 
74          dt = self._cur_time - self._prev_time  # get delta t 
75          de = error - self._prev_err  # get delta error 
76   
77          self._cp = error  # proportional term 
78          self._ci += error * dt  # integral term 
79   
80          self._cd = 0 
81          if dt > 0:  # no div by zero 
82              self._cd = de / dt  # derivative term 
83   
84          self._prev_time = self._cur_time  # save t for next pass 
85          self._prev_err = error  # save t-1 error 
86   
87          # sum the terms and return the result 
88          return ((self._kp * self._cp) + (self._ki * self._ci) + 
89                  (self._kd * self._cd)) 
90