blob: af165e7115f481ca14199e8bf5f3696c12a799d1 (
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
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
|
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 getGp(self):
return self.Gp
def setGp(self,val):
self.Gp=val
def getGi(self):
return self.Gi
def setGi(self,val):
self.Gi=val
def getGd(self):
return self.Gd
def setGd(self,val):
self.Gd=val
def getSign(self):
return self.sign
def setSign(self,val):
self.sign=val
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
|
