-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathPID.py
133 lines (104 loc) · 3.98 KB
/
PID.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
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Aug 5 17:51:22 2021
@author: Rayan Bahrami
Safe Autonomous Systems Lab (SAS Lab)
Stevens Institute of Technology
"""
# fmt: off
import time
class PID:
"""
PID controller with a lowpass filter on D term.
Backward Euler method are used in approximations.
"""
def __init__(self, Kp: float, Kd: float, Ki: float,
derivativeFilterFreq: int,
# previousTime: None,
minOutput: float, maxOutput: float,
current_time = None):
"""
TODO:
Parameters
----------
Kp : float
DESCRIPTION.
Kd : float
DESCRIPTION.
Ki : float
DESCRIPTION.
derivativeFilterFreq : int
DESCRIPTION.
previousTime : float
DESCRIPTION.
minOutput : TYPE, optional
DESCRIPTION. The default is None.
maxOutput : TYPE, optional
DESCRIPTION. The default is None.
current_time : TYPE, optional
DESCRIPTION. The default is None.
Returns
-------
None.
"""
self.Kp = Kp
self.Kd = Kd
self.Ki = Ki
self.F = derivativeFilterFreq
# self.sample_time = sampleTime
self.Ki = Ki
self.minOutput = minOutput
self.maxOutput = maxOutput
self.previousError = 0.0
self.previousDterm = 0.0
self.previousIterm = 0.0
# self.currentTime = current_time if current_time is not None else time.time()
# self.previousTime = ros::Time::now()
# self.previousTime = self.currentTime
self.previousTime = current_time if current_time is not None else time.time()
# self.reset(self)
def reset(self, current_time = None):
self.previousError = 0.0
self.previousDterm = 0.0
self.previousIterm = 0.0
# self.previousTime = ros::Time::now()
self.previousTime = current_time if current_time is not None else time.time()
def update(self, feedback_value=None, target_value=None, tracking_error=None, current_time=None):
"""
Parameters
----------
feedback_value : TYPE
DESCRIPTION.
target_value : TYPE
DESCRIPTION.
tracking_error : TYPE, optional
DESCRIPTION. The default is None.
current_time : TYPE, optional
DESCRIPTION. The default is None.
Returns
-------
TYPE
DESCRIPTION.
"""
if tracking_error is None and feedback_value is None:
raise Exception("ERROR! PID requires either tracking_error OR both feedback_value and target_value as input.")
if tracking_error is None and target_value is None:
raise Exception("ERROR! PID requires either tracking_error OR both feedback_value and target_value as input.")
# TOO: add if {dt>0 OR Ts} for updating the D term
# what if feedback_value, target_value are not given
currentTime = current_time if current_time is not None else time.time()
error = (target_value - feedback_value) if tracking_error is None else tracking_error
dt = currentTime - self.previousTime
Pterm = self.Kp * error
Dterm = self.previousDterm/(1+dt*self.F) + (
self.Kd*self.F*(error - self.previousError))/(1+self.F*dt)
Iterm = self.previousIterm + self.Ki * dt * error
Iterm = max(self.minOutput*.4, min(self.maxOutput*.4, Iterm))
# self.currentTime = current_time if current_time is not None else time.time()
self.previousError = error
self.previousDterm = Dterm
self.previousIterm = Iterm
self.previousTime = currentTime
output = Pterm + Dterm + Iterm
return max(self.minOutput, min(self.maxOutput, output))