From 7dd1ce4fd279b1381c2d795d584f40a7c3fe0ee9 Mon Sep 17 00:00:00 2001
From: Nikolay Khabarov <2xl@mail.ru>
Date: Sat, 17 Jun 2017 00:48:02 +0300
Subject: [PATCH] pid with test

---
 cnc/pid.py        | 84 +++++++++++++++++++++++++++++++++++++++++++++++
 tests/test_pid.py | 66 +++++++++++++++++++++++++++++++++++++
 2 files changed, 150 insertions(+)
 create mode 100644 cnc/pid.py
 create mode 100644 tests/test_pid.py

diff --git a/cnc/pid.py b/cnc/pid.py
new file mode 100644
index 0000000..4de3998
--- /dev/null
+++ b/cnc/pid.py
@@ -0,0 +1,84 @@
+import time
+import math
+
+
+class Pid(object):
+    # PID coefficients
+    P = 0.422
+    I = 0.208
+    D = 0.014
+    WINDUP_LIMIT = 3.0
+    FIX_ACCURACY = 0.01
+    FIX_TIME_S = 2.5
+
+    def __init__(self, target_value, start_time=time.time()):
+        """
+        Proportional-integral-derivative controller implementation.
+        :param target_value: value which PID should achieve.
+        :param start_time: start time, current system time by default.
+        """
+        self._last_time = start_time
+        self._target_value = target_value
+        self._integral = 0
+        self._last_error = 0
+        self._is_target_fixed = False
+        self._target_fix_timer = None
+
+    def update(self, current_value, current_time=time.time()):
+        """
+        Update PID with new current value.
+        :param current_value: current value.
+        :param current_time: time when current value measured, current system
+                             time if not specified.
+        :return: value in range 0..1.0 which represents PID output.
+        """
+        delta_time = current_time - self._last_time
+        self._last_time = current_time
+        error = self._target_value - current_value
+        self._integral += error * delta_time
+        # integral windup protection
+        if abs(self._integral) > self.WINDUP_LIMIT:
+            self._integral = math.copysign(self.WINDUP_LIMIT, self._integral)
+        delta_error = error - self._last_error
+        self._last_error = error
+
+        res = self.P * error + self.I * self._integral + self.D * delta_error
+        if res > 1.0:
+            res = 1.0
+        if res < 0.0:
+            res = 0.0
+
+        if not self._is_target_fixed:
+            if abs(error) < self._target_value * self.FIX_ACCURACY \
+                    and res < 1.0:
+                if self._target_fix_timer is None:
+                    self._target_fix_timer = current_time
+                elif current_time - self._target_fix_timer > self.FIX_TIME_S:
+                    self._is_target_fixed = True
+            else:
+                self._target_fix_timer = None
+        return res
+
+    def is_fixed(self):
+        """
+        Check if target value is reached and PID maintains this value.
+        :return: boolean value
+        """
+        return self._is_target_fixed
+
+
+# for test purpose, see details in corresponding test file
+if __name__ == "__main__":
+    p = Pid(230, 0)
+    c = 0.0039
+    h = 3.09
+    t0 = 25
+    t = t0
+    r = 0
+    for i in range(1, 601):
+        # natural cooling
+        t -= (t - t0) * c
+        # heating
+        t += h * r
+        r = p.update(t, i)
+        print(i, t, r, p.is_fixed())
diff --git a/tests/test_pid.py b/tests/test_pid.py
new file mode 100644
index 0000000..4fb8d8d
--- /dev/null
+++ b/tests/test_pid.py
@@ -0,0 +1,66 @@
+import unittest
+
+from cnc.pid import *
+
+
+class TestPid(unittest.TestCase):
+    def setUp(self):
+        self._environment_temp = 25
+        # Coefficients below were chosen by an experimental way with a real
+        # hardware: reprap heating bed and extruder.
+        self._bed_c = 0.00231  # bed cooling coefficient
+        self._bed_h = 0.25  # bed heating coefficient
+        self._extruder_c = 0.0039  # extruder cooling coefficient
+        self._extruder_h = 3.09  # extruder heating coefficient
+
+    def tearDown(self):
+        pass
+
+    def __simulate(self, target_temp, environment_temp, cool, heat):
+        # Simulate heating some hypothetical thing with heater(h is a heat
+        # transfer coefficient, which becomes just a delta temperature each
+        # second) from environment temperature to target_temp. Consider that
+        # there is natural air cooling process with some heat transfer
+        # coefficient c. Heating power is controlled by PID.
+        pid = Pid(target_temp, 0)
+        temperature = environment_temp
+        heater_power = 0
+        fixed_at = None
+        zeros_counter = 0
+        for j in range(1, 15 * 60 + 1):  # simulate for 15 minutes
+            # natural cooling
+            temperature -= (temperature - environment_temp) * cool
+            # heating
+            temperature += heat * heater_power
+            heater_power = pid.update(temperature, j)
+            if fixed_at is None:
+                if pid.is_fixed():
+                    fixed_at = j
+            else:
+                self.assertLess(abs(temperature - target_temp),
+                                pid.FIX_ACCURACY * target_temp,
+                                msg="PID failed to control temperature "
+                                "{}/{} {}".format(temperature, target_temp, j))
+            if heater_power == 0.0:
+                zeros_counter += 1
+        self.assertLess(zeros_counter, 20, msg="PID turns on/off, instead of "
+                                               "fine control")
+        self.assertLess(fixed_at, 600,
+                        msg="failed to heat in 10 minutes, final temperature "
+                            "{}/{}".format(temperature, target_temp))
+
+    def test_bed(self):
+        # check if bed typical temperatures can be reached in simulation
+        for target in range(50, 101, 10):
+            self.__simulate(target, self._environment_temp,
+                            self._bed_c, self._bed_h)
+
+    def test_extruder(self):
+        # check if extruder typical temperatures can be reached in simulation
+        for target in range(150, 251, 10):
+            self.__simulate(target, self._environment_temp,
+                            self._extruder_c, self._extruder_h)
+
+
+if __name__ == '__main__':
+    unittest.main()