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
|
import time
from qolab.hardware.basic import BasicInstrument
class PID(BasicInstrument):
def __init__(self, Gp=0, Gi=0, Gd=0, sign=1):
super().__init__()
self.config['Device model'] = 'Generic Software PID loop'
self.config['Device type']='PID loop'
self.config['FnamePrefix'] = 'pid'
self.deviceProperties = ['Gp', 'Gi', 'Gd', 'Sign', 'Enable' ];
self.Gp = Gp
self.Gi = Gi
self.Gd = Gd
self.sign = sign
self.enable = True
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 getEnable(self):
return self.enable
def setEnable(self,val):
self.enable=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;
u *= self.sign
if not self.enable:
return 0
return u
|
