/
pidcontroller.py
42 lines (33 loc) · 1.02 KB
/
pidcontroller.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
import time
class PID:
"""PID controller."""
def __init__(self, Kp, Ki, Kd, origin_time=None):
if origin_time is None:
origin_time = time.time()
# Gains for each term
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
# Corrections (outputs)
self.Cp = 0.0
self.Ci = 0.0
self.Cd = 0.0
self.previous_time = origin_time
self.previous_error = 0.0
def Update(self, error, current_time=None):
if current_time is None:
current_time = time.time()
dt = current_time - self.previous_time
if dt <= 0.0:
return 0
de = error - self.previous_error
self.Cp = error
self.Ci += error * dt
self.Cd = de / dt
self.previous_time = current_time
self.previous_error = error
return (
(self.Kp * self.Cp) # proportional term
+ (self.Ki * self.Ci) # integral term
+ (self.Kd * self.Cd) # derivative term
)