- added Pid

- added Temp Controller
- added Kalman

components/pid/pid.py

@@ -0,0 +1,48 @@
+
+
+class Pid:
+    def __init__(self):
+        # Integrator
+        self.yi = 0
+
+        # Differentiator
+        self.xd = 0
+
+        # Auto windup
+        self.y_min = -1.0
+        self.y_max = 1.0
+
+        # Output
+        self.y = 0
+
+    @staticmethod
+    def params(kp, ki, kd, rho):
+        p = dict(kp=kp, ki=ki, kd=kd, rho=rho)
+        return p
+
+    def reset(self):
+        self.yi = 0
+        self.xd = 0
+
+    def process(self, dt, params, err):
+        kp = params['kp']
+        ki = params['ki']
+        kd = params['kd']
+        rho = params['rho']
+
+        yi = rho*self.yi + ki*dt * err
+        yd = err - self.xd
+
+        _yp = kp * err
+        _yi = yi
+        _yd = kd/dt * yd
+
+        y = _yp + _yi + _yd
+
+        self.y = max(self.y_min, min(self.y_max, y))
+
+        self.yi = yi
+        self.xd = err
+
+    def get_y(self):
+        return self.y