-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathpid.py
223 lines (173 loc) · 6.72 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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
"""
This is a Python PID module heavily inspired by the Arduino PID Library
written by Brett Beauregard.
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
"""
__author__ = "Stian Sandve"
__license__ = "GPLv3"
__version__ = "1.1.1"
import time
from enum import Enum
class Direction(Enum):
direct = 1
reverse = 2
class Mode(Enum):
automatic = 1
manual = 2
class PID(object):
def __init__(self, kp, ki, kd, set_point, controller_direction):
"""
The parameters specified here are those for for which we can't set up
reliable defaults, so we need to have the user set them.
:param kp: Proportional Tuning Parameter
:param ki: Integral Tuning Parameter
:param kd: Derivative Tuning Parameter
:param set_point: The value that we want the process to be.
:param controller_direction:
"""
self.kp = kp
self.ki = ki
self.kd = kd
self.direction = controller_direction
self.i_term = 0
self.out_max = 0
self.out_min = 0
self.last_input = 0
self.output = 0
self.input = 0
self.set_point = set_point
self.mode = Mode.automatic
self.set_output_limits(0, 255)
self.sample_time = 100
self.controller_direction = Direction.direct
self.set_controller_direction(controller_direction)
self.set_tunings(self.kp, self.ki, self.kd)
self.last_time = self.now()
@staticmethod
def now():
"""
Static method to make it easy to obtain the current time
in milliseconds.
:return: Current time in milliseconds.
"""
return int(round(time.time() * 1000))
def compute(self, input):
"""
This, as they say, is where the magic happens. This function should
be called every time "void loop()" executes. The function will decide
for itself whether a new PID Output needs to be computed.
:param input: Input value for the PID controller.
:return: Returns true when the output is computed,
false when nothing has been done.
"""
if self.mode is Mode.manual:
return 0, False
delta_time = self.now() - self.last_time
if delta_time >= self.sample_time:
error = self.set_point - input
self.i_term += (self.ki * error)
if self.i_term > self.out_max:
self.i_term = self.out_max
elif self.i_term < self.out_min:
self.i_term = self.out_min
delta_input = input - self.last_input
self.output = self.kp * error + self.i_term - self.kd * delta_input
if self.output > self.out_max:
self.output = self.out_max
elif self.output < self.out_min:
self.output = self.out_min
self.last_input = input
self.last_time = self.now()
return self.output, True
else:
return 0, False
def set_tunings(self, kp, ki, kd):
"""
This function allows the controller's dynamic performance to be
adjusted. It's called automatically from the constructor,
but tunings can also be adjusted on the fly during normal operation.
:param kp: Proportional Tuning Parameter
:param ki: Integral Tuning Parameter
:param kd: Derivative Tuning Parameter
"""
if kp < 0 or ki < 0 or ki < 0:
return
sample_time_in_sec = self.sample_time / 1000
self.kp = kp
self.ki = ki * sample_time_in_sec
self.kd = kd / sample_time_in_sec
if self.controller_direction is Direction.Reverse:
self.kp = 0 - kp
self.ki = 0 - ki
self.kd = 0 - kd
def set_sample_time(self, sample_time):
"""
Sets the period, in milliseconds, at which the calculation is
performed.
:param sample_time: The period, in milliseconds,
at which the calculation is performed.
"""
if sample_time > 0:
ratio = sample_time / self.sample_time
self.ki *= ratio
self.kd /= ratio
self.sample_time = sample_time
def set_output_limits(self, min, max):
"""
This function will be used far more often than set_input_limits. While
the input to the controller will generally be in the 0-1023 range
(which is the default already), the output will be a little different.
Maybe they'll be doing a time window and will need 0-8000 or something.
Or maybe they'll want to clamp it from 0-125.
:param min: Minimum output value from the PID controller
:param max: Maximum output value from the PID controller
"""
if min >= max:
return
self.out_min = min
self.out_max = max
if self.in_auto:
if self.output > self.out_max:
self.output = self.out_max
elif self.output < self.out_min:
self.output = self.out_min
if self.i_term > self.out_max:
self.i_term = self.out_max
elif self.i_term < self.out_min:
self.i_term = self.out_min
def set_mode(self, mode):
"""
Allows the controller Mode to be set to manual (0) or Automatic
(non-zero) when the transition from manual to auto occurs,
the controller is automatically initialized.
:param mode: The mode of the PID controller.
Can be either manual or automatic.
"""
if self.mode is Mode.manual and mode is Mode.automatic:
self.initialize()
self.mode = mode
def initialize(self):
"""
Does all the things that need to happen to ensure a smooth transfer
from manual to automatic mode.
"""
self.i_term = self.output
self.last_input = self.input
if self.i_term > self.out_max:
self.i_term = self.out_max
elif self.i_term < self.out_min:
self.i_term = self.out_min
def set_controller_direction(self, direction):
"""
The PID will either be connected to a DIRECT acting process
(+Output leads to +Input) or a REVERSE acting process
(+Output leads to -Input.). We need to know which one,
because otherwise we may increase the output when we should be
decreasing. This is called from the constructor.
:param direction: The direction of the PID controller.
"""
if self.mode is Mode.automatic and direction is not self.direction:
self.kp = 0 - self.kp
self.ki = 0 - self.ki
self.kd = 0 - self.kd
self.direction = direction