From 0446659d3d6eb95b41c609a8adf1adeef1e3756b Mon Sep 17 00:00:00 2001
From: "Eugeniy E. Mikhailov" <evgmik@gmail.com>
Date: Mon, 13 Dec 2021 16:42:32 -0500
Subject: added pid draft

---
 qolab/feedback/__init__.py | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)
 create mode 100644 qolab/feedback/__init__.py

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
+
-- 
cgit v1.2.3