From bfbb8172e07dccd26b25f753caf1815210b617e8 Mon Sep 17 00:00:00 2001
From: Markus Zolliker <markus.zolliker@psi.ch>
Date: Fri, 9 Aug 2024 10:56:31 +0200
Subject: [PATCH] improve picontrol

after suggestions from Marek (WIP)

- keep Mareks version created during Markus absence for now
- add a class with input_module (which is not a mixin)
- not 100 % tested: HasConvergence now inherits from Drivable

Change-Id: I6b3896e7c0fdaa4379d1cbc5603d43bd7a0b3a48
---
 frappy_psi/convergence.py |   4 +-
 frappy_psi/picontrol.py   | 111 ++++++++++++++++++++++++++++++++++++--
 2 files changed, 110 insertions(+), 5 deletions(-)

diff --git a/frappy_psi/convergence.py b/frappy_psi/convergence.py
index 0ef4918..560626f 100644
--- a/frappy_psi/convergence.py
+++ b/frappy_psi/convergence.py
@@ -19,12 +19,12 @@
 #
 # *****************************************************************************
 
-from frappy.core import Parameter, FloatRange, BUSY, IDLE, WARN
+from frappy.core import Drivable, Parameter, FloatRange, BUSY, IDLE, WARN
 from frappy.lib.statemachine import StateMachine, Retry, Stop
 from frappy.lib import merge_status
 
 
-class HasConvergence:
+class HasConvergence(Drivable):
     """mixin for convergence checks
 
     Implementation based on tolerance, settling time and timeout.
diff --git a/frappy_psi/picontrol.py b/frappy_psi/picontrol.py
index eb95907..be201d5 100644
--- a/frappy_psi/picontrol.py
+++ b/frappy_psi/picontrol.py
@@ -16,22 +16,61 @@
 # Module authors:
 #   Markus Zolliker <markus.zolliker@psi.ch>
 #   Jael Celia Lorenzana <jael-celia.lorenzana@psi.ch>
+#   Marek Bartkowiak <marek.bartkowiak@psi.ch>
 #
 # *****************************************************************************
 
-"""soft PI control"""
+"""soft PI control
+
+recipe using the PImixin:
+
+assume you have class Sensor inheriting from Readable, you create a new class:
+
+   class SensorWithLoop(HasConvergence, PImixin, Sensor):
+       pass
+
+and this is an example cfg
+
+    Mod('T_sample',
+        'frappy_psi.<your driver module>.SensorWithLoop',
+        'controlled T',
+        meaning=['temperature', 20],
+        output_module='htr_sample',
+        p=1,
+        i=0.01,
+    )
+
+
+recipe using PI:
+
+example cfg:
+
+    Mod('T_softloop',
+        'frappy_psi.picontrol.PI',
+        'softloop controlled Temperature mixing chamber',
+        input = 'ts',
+        output = 'htr_mix',
+        control_active = 1,
+        output_max = 80000,
+        p = 2E6,
+        i = 10000,
+        tlim = 1.0,
+       )
+"""
 
 import time
 import math
-from frappy.core import Writable, Parameter, FloatRange, IDLE
+from frappy.core import Readable, Writable, Parameter, Attached, IDLE
 from frappy.lib import clamp
-from frappy.datatypes import LimitsType, EnumType
+from frappy.datatypes import LimitsType, EnumType, BoolType, FloatRange
 from frappy.mixins import HasOutputModule
+from frappy_psi.convergence import HasConvergence
 
 
 class PImixin(HasOutputModule, Writable):
     p = Parameter('proportional term', FloatRange(0), readonly=False)
     i = Parameter('integral term', FloatRange(0), readonly=False)
+    # output_module is inherited
     output_range = Parameter('min output',
                              LimitsType(FloatRange()), default=(0, 0), readonly=False)
     output_func = Parameter('output function',
@@ -84,3 +123,69 @@ class PImixin(HasOutputModule, Writable):
     def write_target(self, _):
         if not self.control_active:
             self.activate_control()
+
+
+class PI(Writable):
+    """temporary, but working version from Marek"""
+    input = Attached(Readable, 'the input module')
+    output = Attached(Writable, 'the output module')
+    output_max = Parameter('max output value', FloatRange(0), readonly=False)
+    p = Parameter('proportional term', FloatRange(0), readonly=False)
+    i = Parameter('integral term', FloatRange(0), readonly=False)
+    control_active = Parameter('control flag', BoolType(), readonly=False, default=False)
+    value = Parameter(unit='K')
+    tlim = Parameter('max Temperature', FloatRange(0), readonly=False)
+    _lastdiff = None
+    _lasttime = 0
+    _lastvalue = 0
+
+    def doPoll(self):
+        super().doPoll()
+        if not self.control_active:
+            return
+        self.value = self.input.value
+        self.status = IDLE, 'controlling'
+        now = time.time()
+        deltat = min(10.0, now-self._lasttime)
+        self._lasttime = now
+        if self.value != self._lastvalue:
+            diff = self.target - self.value   # calculate the difference to target
+            self._lastvalue = self.value
+        # else ? (diff is undefined!)
+        if self.value > self.tlim:
+            self.write_control_active(False)
+            return
+        if self._lastdiff is None:
+            self._lastdiff = diff
+        deltadiff = diff - self._lastdiff   # calculate the change in deltaT
+        self._lastdiff = diff
+        output = self.output.target
+        output += self.p * deltadiff + self.i * deltat * diff
+        if output > self.output_max:
+            output = self.output_max
+        elif output < 0:
+            output = 0
+        self.output.write_target(output)
+
+    def write_control_active(self, value):
+        if not value:
+            self.output.write_target(0)
+
+    def write_target(self, value):
+        self.control_active = True
+
+
+# proposal for replacing above PI class, inheriting from PImixin
+# additional features:
+# - is a Drivable, using the convergence criteria from HasConvergence
+# - tries to determine the output limits automatically
+# unchecked!
+
+class PIproposed(HasConvergence, PImixin):
+    input_module = Attached(Readable, 'the input module')
+
+    def read_value(self):
+        return self.input_module.value
+
+    def read_status(self):
+        return self.input_module.status