motor valve using trinamic motor

This valve needs 8 turns to open. As the encoder forgets
the number if turns on power cycle, a home switch is
mounte, which engages during the last turn when closing.
The final close position is determined by closing the valve
with a defined motor current/torque.

+ fix an issue in StateMachine.start: the first cycle
  must be called after the new state is assigned

Change-Id: I34cd05d10d97b043f9e3126310943b74ee727382
Reviewed-on: https://forge.frm2.tum.de/review/c/sine2020/secop/playground/+/28030
Tested-by: Jenkins Automated Tests <pedersen+jenkins@frm2.tum.de>
Reviewed-by: Enrico Faulhaber <enrico.faulhaber@frm2.tum.de>
Reviewed-by: Markus Zolliker <markus.zolliker@psi.ch>

secop/lib/statemachine.py

@@ -177,9 +177,9 @@ class StateMachine:
 
         :return: a delay or None when idle
         """
-        if self.state is None:
-            return None
         with self._lock:
+            if self.state is None:
+                return None
             for _ in range(999):
                 self.now = time.time()
                 try:
@@ -236,7 +236,7 @@ class StateMachine:
                 pass
             delay = self.cycle()
 
-    def _start(self, state, first_delay, **kwds):
+    def _start(self, state, **kwds):
         self._restart = None
         self._idle_event.clear()
         self.last_error = None
@@ -245,10 +245,12 @@ class StateMachine:
         self._new_state(state)
         self.start_time = self.now
         self._last_time = self.now
+        first_delay = self.cycle()  # important: call once (e.g. set status to busy)
         if self._threaded:
             if self._thread is None or not self._thread.is_alive():
                 # restart thread if dead (may happen when cleanup failed)
-                self._thread = mkthread(self._run, first_delay)
+                if first_delay is not None:
+                    self._thread = mkthread(self._run, first_delay)
             else:
                 self.trigger(first_delay)
 
@@ -269,11 +271,9 @@ class StateMachine:
                     self.last_error = self.stopped
                     self.cleanup(self)  # ignore return state on restart
                     self.stopped = False
-                delay = self.cycle()
+                    self._start(state, **kwds)
-                self._start(state, delay, **kwds)
         else:
-            delay = self.cycle()  # important: call once (e.g. set status to busy)
+            self._start(state, **kwds)
-            self._start(state, delay, **kwds)
 
     def stop(self):
         """stop machine, go to idle state

secop/modules.py

@@ -736,15 +736,19 @@ class Module(HasAccessibles):
             value = self.writeDict.pop(pname, Done)
             # in the mean time, a poller or handler might already have done it
             if value is not Done:
-                try:
+                wfunc = getattr(self, 'write_' + pname, None)
-                    self.log.debug('initialize parameter %s', pname)
+                if wfunc is None:
-                    getattr(self, 'write_' + pname)(value)
+                    setattr(self, pname, value)
-                except SilentError:
+                else:
-                    pass
+                    try:
-                except SECoPError as e:
+                        self.log.debug('initialize parameter %s', pname)
-                    self.log.error(str(e))
+                        wfunc(value)
-                except Exception:
+                    except SilentError:
-                    self.log.error(formatException())
+                        pass
+                    except SECoPError as e:
+                        self.log.error(str(e))
+                    except Exception:
+                        self.log.error(formatException())
         if started_callback:
             started_callback()
 

secop/persistent.py

@@ -72,15 +72,16 @@ class PersistentMixin(HasAccessibles):
         persistentdir = os.path.join(generalConfig.logdir, 'persistent')
         os.makedirs(persistentdir, exist_ok=True)
         self.persistentFile = os.path.join(persistentdir, '%s.%s.json' % (self.DISPATCHER.equipment_id, self.name))
-        self.initData = {}
+        self.initData = {}  # "factory" settings
         for pname in self.parameters:
             pobj = self.parameters[pname]
-            if hasattr(self, 'write_' + pname) and getattr(pobj, 'persistent', 0):
+            flag = getattr(pobj, 'persistent', 0)
-                self.initData[pname] = pobj.value
+            if flag:
-                if pobj.persistent == 'auto':
+                if flag == 'auto':
                     def cb(value, m=self):
                         m.saveParameters()
                     self.valueCallbacks[pname].append(cb)
+                self.initData[pname] = pobj.value
         self.writeDict.update(self.loadParameters(write=False))
 
     def loadParameters(self, write=True):

secop_psi/motorvalve.py

@@ -0,0 +1,267 @@
+#  -*- coding: utf-8 -*-
+# *****************************************************************************
+#
+# This program is free software; you can redistribute it and/or modify it under
+# the terms of the GNU General Public License as published by the Free Software
+# Foundation; either version 2 of the License, or (at your option) any later
+# version.
+#
+# This program is distributed in the hope that it will be useful, but WITHOUT
+# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+# FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+# details.
+#
+# You should have received a copy of the GNU General Public License along with
+# this program; if not, write to the Free Software Foundation, Inc.,
+# 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+#
+# Module authors:
+#   Markus Zolliker <markus.zolliker@psi.ch>
+#
+# *****************************************************************************
+
+"""motor valve using a trinamic PD-1161 motor
+
+the valve has an end switch connected to the 'home' digital input
+of the motor controller. Motor settings for the currently used valve:
+
+[valve_motor]
+description = valve motor
+class = secop_psi.trinamic.Motor
+maxcurrent=0.3   # a value corresponding to the torque needed to firmly close the valve.
+move_limit=9999  # no move limit needed
+acceleration=150
+encoder_tolerance=3.6  # typical value
+auto_reset=True   # motor stalls on closing
+
+[valve]
+description = trinamic angular motor valve
+class = secop_psi.motorvalve.MotorValve
+motor = valve_motor
+turns = 9      # number of turns needed to open fully
+speed = 400    # close to max. speed
+lowspeed = 50  # speed for final closing / reference run
+"""
+
+from secop.core import Drivable, Parameter, EnumType, Attached, FloatRange, \
+    Command, IDLE, BUSY, WARN, ERROR, Done, PersistentParam, PersistentMixin
+from secop.errors import HardwareError
+from secop_psi.trinamic import Motor
+from secop.lib.statemachine import StateMachine, Retry, Stop
+
+
+class MotorValve(PersistentMixin, Drivable):
+    motor = Attached(Motor)
+    value = Parameter('current state', EnumType(
+        closed=0, opened=1, undefined=-1), default=-1)
+    target = Parameter('target state', EnumType(close=0, open=1))
+    turns = Parameter('number of turns to open', FloatRange(), readonly=False, group='settings')
+    speed = Parameter('speed for far moves', FloatRange(), readonly=False, group='settings')
+    lowspeed = Parameter('speed for finding closed position', FloatRange(), readonly=False, group='settings')
+    closed_pos = PersistentParam('fully closed position', FloatRange(),
+                                 persistent='auto', export=True, default=-999)  # TODO: export=False
+    pollinterval = Parameter(group='settings')
+
+    _state = None
+
+    # remark: the home button must be touched when the motor is at zero
+
+    def earlyInit(self):
+        super().earlyInit()
+        self._state = StateMachine(logger=self.log, count=3, cleanup=self.handle_error)
+
+    def write_target(self, target):
+        if self.status[0] == ERROR:
+            raise HardwareError('%s: need refrun' % self.status[1])
+        self.target = target
+        self._state.start(self.goto_target, count=3)
+        return Done
+
+    def goto_target(self, state):
+        self.value = 'undefined'
+        if self.motor.isBusy():
+            mot_target = 0 if self.target == self.target.close else self.turns * 360
+            if abs(mot_target - self.motor.target) > self.motor.tolerance:
+                self.motor.stop()
+        return self.open_valve if self.target == self.target.open else self.close_valve
+
+    def read_value(self):
+        """determine value and status"""
+        if self.status[0] == ERROR:
+            return 'undefined'
+        if self.motor.isBusy():
+            return Done
+        motpos = self.motor.read_value()
+        if self.motor.read_home():
+            if motpos > 360:
+                self.status = ERROR, 'home button must be released at this position'
+            elif motpos > 5:
+                if self.status[0] != ERROR:
+                    self.status = WARN, 'position undefined'
+            elif motpos < -360:
+                self.status = ERROR, 'motor must not reach -1 turn!'
+            elif abs(motpos - self.closed_pos) < self.motor.tolerance:
+                self.status = IDLE, 'closed'
+                return 'closed'
+            self.status = WARN, 'nearly closed'
+            return 'undefined'
+        if abs(motpos - self.turns * 360) < 5:
+            self.status = IDLE, 'opened'
+            return 'opened'
+        if motpos < 5:
+            self.status = ERROR, 'home button must be engaged at this position'
+        elif self.status[0] != ERROR:
+            self.status = WARN, 'position undefined'
+        return 'undefined'
+
+    def open_valve(self, state):
+        if state.init:
+            self.closed_pos = -999
+            self.value = 'undefined'
+            self.status = BUSY, 'opening'
+            self.motor.write_speed(self.speed)
+            self.motor.write_target(self.turns * 360)
+        if self.motor.isBusy():
+            if self.motor.home and self.motor.value > 360:
+                self.motor.stop()
+                self.status = ERROR, 'opening valve failed (home switch not released)'
+                return None
+            return Retry()
+        motvalue = self.motor.read_value()
+        if abs(motvalue - self.turns * 360) < 5:
+            self.read_value()  # value = opened, status = IDLE
+        else:
+            if state.count > 0:
+                state.count -= 1
+                self.log.info('target %g not reached, try again', motvalue)
+                return self.goto_target
+            self.status = ERROR, 'opening valve failed (motor target not reached)'
+        return None
+
+    def close_valve(self, state):
+        if state.init:
+            self.closed_pos = -999
+            self.status = BUSY, 'closing'
+            self.motor.write_speed(self.speed)
+            self.motor.write_target(0)
+        if self.motor.isBusy():
+            if self.motor.home:
+                return self.find_closed
+            return Retry()
+        motvalue = self.motor.read_value()
+        if abs(motvalue) > 5:
+            if state.count > 0:
+                state.count -= 1
+                self.log.info('target %g not reached, try again', motvalue)
+                return self.goto_target
+            self.status = ERROR, 'closing valve failed (zero not reached)'
+            return None
+        if self.read_value() == self.value.undefined:
+            if self.status[0] != ERROR:
+                return self.find_closed
+        return None
+
+    def find_closed(self, state):
+        """drive with low speed until motor stalls"""
+        if state.init:
+            self.motor.write_speed(self.lowspeed)
+            state.prev = self.motor.value
+            self.motor.write_target(-360)
+        if self.motor.isBusy():
+            if not self.motor.home:
+                self.motor.stop()
+                return None
+            return Retry()
+        motvalue = self.motor.read_value()
+        if motvalue < -360:
+            self.read_value()  # status -> error
+            return None
+        if motvalue < state.prev - 5:
+            # moved by more than 5 deg
+            state.prev = self.motor.value
+            self.motor.write_target(-360)
+            return Retry()
+        if motvalue > 5:
+            self.status = ERROR, 'closing valve failed (zero not reached)'
+            return None
+        if motvalue < -355:
+            self.status = ERROR, 'closing valve failed (does not stop)'
+            return None
+        self.closed_pos = motvalue
+        self.read_value()  # value = closed, status = IDLE
+        return None
+
+    @Command
+    def ref_run(self):
+        """start reference run"""
+        self.target = 'close'
+        self._state.start(self.ref_home, count=3)
+
+    @Command
+    def stop(self):
+        self._state.stop()
+        self.motor.stop()
+
+    def ref_home(self, state):
+        if state.init:
+            self.closed_pos = -999
+            self.motor.write_speed(self.lowspeed)
+            if self.motor.read_home():
+                self.status = BUSY, 'refrun: release home'
+                self.motor.write_target(self.motor.read_value() + 360)
+                return self.ref_released
+            self.status = BUSY, 'refrun: find home'
+            self.motor.write_target(self.motor.read_value() - (self.turns + 1) * 360)
+        if not self.motor.isBusy():
+            self.status = ERROR, 'ref run failed, can not find home switch'
+            return None
+        if not self.motor.home:
+            return Retry()
+        self.motor.write_speed(self.lowspeed)
+        state.prev = self.motor.read_value()
+        self.motor.write_target(state.prev - 360)
+        self.status = BUSY, 'refrun: find closed'
+        return self.ref_closed
+
+    def ref_released(self, state):
+        if self.motor.isBusy():
+            if self.motor.home:
+                return Retry()
+        elif self.motor.read_home():
+            if state.count > 0:
+                state.count -= 1
+                self.log.info('home switch not released, try again')
+                self.motor.write_target(self.motor.target)
+                return Retry()
+            self.status = ERROR, 'ref run failed, can not release home switch'
+            return None
+        return self.ref_home
+
+    def ref_closed(self, state):
+        if self.motor.isBusy():
+            if not self.motor.home:
+                self.motor.stop()
+                return None
+            return Retry()
+        self.motor.set_zero(max(-50, (self.motor.read_value() - state.prev) * 0.5))
+        self.read_value()  # check home button is valid
+        if abs(self.motor.target - self.motor.value) < 5:
+            self.status = ERROR, 'ref run failed, does not stop'
+        if self.status[0] == ERROR:
+            return None
+        self.log.info('refrun successful')
+        return self.close_valve
+
+    def handle_error(self, state):
+        if state.stopped:  # stop or restart case
+            if state.stopped is Stop:
+                self.status = WARN, 'stopped'
+            return None
+        if state.count > 0:
+            state.count -= 1
+            self.log.info('error %r, try again', state.last_error)
+            state.default_cleanup(state)  # log error cause
+            state.last_error = None
+            return self.goto_target  # try again
+        self.status = ERROR, str(state.last_error)
+        return state.default_cleanup(state)

test/test_statemachine.py

@@ -78,7 +78,6 @@ def test_fun():
     s.cycle()  # do nothing
     assert s.step == 0
     s.start(rise, level=0, direction=0)
-    s.cycle()
     for i in range(1, 4):
         assert s.status == 'rise'
         assert s.step == i
@@ -100,7 +99,6 @@ def test_fun():
 def test_max_chain():
     s = StateMachine(step=0, status='', threaded=False, logger=LoggerStub())
     s.start(fall, level=999+1, direction=0)
-    s.cycle()
     assert isinstance(s.last_error, RuntimeError)
     assert s.state is None