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>
This commit is contained in:
@ -177,9 +177,9 @@ class StateMachine:
|
|||||||
|
|
||||||
:return: a delay or None when idle
|
:return: a delay or None when idle
|
||||||
"""
|
"""
|
||||||
if self.state is None:
|
|
||||||
return None
|
|
||||||
with self._lock:
|
with self._lock:
|
||||||
|
if self.state is None:
|
||||||
|
return None
|
||||||
for _ in range(999):
|
for _ in range(999):
|
||||||
self.now = time.time()
|
self.now = time.time()
|
||||||
try:
|
try:
|
||||||
@ -236,7 +236,7 @@ class StateMachine:
|
|||||||
pass
|
pass
|
||||||
delay = self.cycle()
|
delay = self.cycle()
|
||||||
|
|
||||||
def _start(self, state, first_delay, **kwds):
|
def _start(self, state, **kwds):
|
||||||
self._restart = None
|
self._restart = None
|
||||||
self._idle_event.clear()
|
self._idle_event.clear()
|
||||||
self.last_error = None
|
self.last_error = None
|
||||||
@ -245,10 +245,12 @@ class StateMachine:
|
|||||||
self._new_state(state)
|
self._new_state(state)
|
||||||
self.start_time = self.now
|
self.start_time = self.now
|
||||||
self._last_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._threaded:
|
||||||
if self._thread is None or not self._thread.is_alive():
|
if self._thread is None or not self._thread.is_alive():
|
||||||
# restart thread if dead (may happen when cleanup failed)
|
# 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:
|
else:
|
||||||
self.trigger(first_delay)
|
self.trigger(first_delay)
|
||||||
|
|
||||||
@ -269,11 +271,9 @@ class StateMachine:
|
|||||||
self.last_error = self.stopped
|
self.last_error = self.stopped
|
||||||
self.cleanup(self) # ignore return state on restart
|
self.cleanup(self) # ignore return state on restart
|
||||||
self.stopped = False
|
self.stopped = False
|
||||||
delay = self.cycle()
|
self._start(state, **kwds)
|
||||||
self._start(state, delay, **kwds)
|
|
||||||
else:
|
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):
|
def stop(self):
|
||||||
"""stop machine, go to idle state
|
"""stop machine, go to idle state
|
||||||
|
@ -736,15 +736,19 @@ class Module(HasAccessibles):
|
|||||||
value = self.writeDict.pop(pname, Done)
|
value = self.writeDict.pop(pname, Done)
|
||||||
# in the mean time, a poller or handler might already have done it
|
# in the mean time, a poller or handler might already have done it
|
||||||
if value is not Done:
|
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:
|
if started_callback:
|
||||||
started_callback()
|
started_callback()
|
||||||
|
|
||||||
|
@ -72,15 +72,16 @@ class PersistentMixin(HasAccessibles):
|
|||||||
persistentdir = os.path.join(generalConfig.logdir, 'persistent')
|
persistentdir = os.path.join(generalConfig.logdir, 'persistent')
|
||||||
os.makedirs(persistentdir, exist_ok=True)
|
os.makedirs(persistentdir, exist_ok=True)
|
||||||
self.persistentFile = os.path.join(persistentdir, '%s.%s.json' % (self.DISPATCHER.equipment_id, self.name))
|
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:
|
for pname in self.parameters:
|
||||||
pobj = self.parameters[pname]
|
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):
|
def cb(value, m=self):
|
||||||
m.saveParameters()
|
m.saveParameters()
|
||||||
self.valueCallbacks[pname].append(cb)
|
self.valueCallbacks[pname].append(cb)
|
||||||
|
self.initData[pname] = pobj.value
|
||||||
self.writeDict.update(self.loadParameters(write=False))
|
self.writeDict.update(self.loadParameters(write=False))
|
||||||
|
|
||||||
def loadParameters(self, write=True):
|
def loadParameters(self, write=True):
|
||||||
|
267
secop_psi/motorvalve.py
Normal file
267
secop_psi/motorvalve.py
Normal file
@ -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)
|
@ -78,7 +78,6 @@ def test_fun():
|
|||||||
s.cycle() # do nothing
|
s.cycle() # do nothing
|
||||||
assert s.step == 0
|
assert s.step == 0
|
||||||
s.start(rise, level=0, direction=0)
|
s.start(rise, level=0, direction=0)
|
||||||
s.cycle()
|
|
||||||
for i in range(1, 4):
|
for i in range(1, 4):
|
||||||
assert s.status == 'rise'
|
assert s.status == 'rise'
|
||||||
assert s.step == i
|
assert s.step == i
|
||||||
@ -100,7 +99,6 @@ def test_fun():
|
|||||||
def test_max_chain():
|
def test_max_chain():
|
||||||
s = StateMachine(step=0, status='', threaded=False, logger=LoggerStub())
|
s = StateMachine(step=0, status='', threaded=False, logger=LoggerStub())
|
||||||
s.start(fall, level=999+1, direction=0)
|
s.start(fall, level=999+1, direction=0)
|
||||||
s.cycle()
|
|
||||||
assert isinstance(s.last_error, RuntimeError)
|
assert isinstance(s.last_error, RuntimeError)
|
||||||
assert s.state is None
|
assert s.state is None
|
||||||
|
|
||||||
|
Reference in New Issue
Block a user