diff options
| -rw-r--r-- | qolab/feedback/__init__.py | 31 |
1 files changed, 31 insertions, 0 deletions
diff --git a/qolab/feedback/__init__.py b/qolab/feedback/__init__.py new file mode 100644 index 0000000..c778b2a --- /dev/null +++ b/qolab/feedback/__init__.py @@ -0,0 +1,31 @@ +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.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 + |