blob: de58d13d08d1e323102367019090e001b3cf9191 (
plain)
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
|
import time
class PID:
def __init__(self, Gp=0, Gi=0, Gd=0, sign=1):
self.Gp = Gp
self.Gi = Gi
self.Gd = Gd
self.sign = sign
self.reset()
def reset(self):
self.err_1dt_back = 0
self.err_2dt_back = 0
self.err_now = 0
self.last_update = time.time()
def feedback(self, err):
# PID feedback
# see https://en.wikipedia.org/wiki/PID_controller#Pseudocode
self.err_2dt_back = self.err_1dt_back;
self.err_1dt_back = self.err_now;
self.err_now = err
tnow = time.time()
dt = tnow - self.last_update
self.last_update = tnow
A0= self.Gp+self.Gi*dt + self.Gd/dt;
A1= -self.Gp-2*self.Gd/dt;
A2= self.Gd/dt;
u = A0*self.err_now + A1*self.err_1dt_back + A2*self.err_2dt_back;
return u*self.sign
|
