- reworked

git-svn-id: http://moon:8086/svn/projects/HendiControl@113 fda53097-d464-4ada-af97-ba876c37ca34

Control/brewpi/pid.py

@@ -0,0 +1,58 @@
+import numpy as np;
+
+
+class Pid():
+    def __init__(self):
+        # Integrator
+        self.yi = 0
+
+        # Differentiator
+        self.xd = 0
+
+        # Auto windup
+        self.y_min = 0.0
+        self.y_max = 80.0
+        self.diff_aw = 0.0
+
+        # Output
+        self.y = 0
+        self.yc = 0
+
+    @staticmethod
+    def params(kp, ki, kd, rho):
+        p = dict(kp=kp, ki=ki, kd=kd, rho=rho)
+        return p
+
+    def process(self, params, err):
+        kp = params['kp']
+        ki = params['ki']
+        kd = params['kd']
+        rho = params['rho']
+
+        err_aw = err + self.diff_aw
+        yi = rho*self.yi + err_aw
+        yd = err - self.xd
+
+        _yp = kp * err
+        _yi = ki * yi
+        _yd = kd * yd
+
+        self.y = _yp + _yi + _yd
+
+        yaw = yi
+        if yaw < self.y_min:
+            self.diff_aw = abs(yaw - self.y_min)
+
+        if yaw > self.y_max:
+            self.diff_aw = -abs(yaw - self.y_max)
+
+        self.yc = max(self.y_min, min(self.y_max, self.y))
+
+        self.yi = yi
+        self.xd = err
+
+    def get_y(self):
+        return self.y
+
+    def get_yc(self):
+        return self.yc