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
+