frappy_psi.attocube: add lock protection to hw access
in order to avoid sporadic timeout problems Change-Id: I36f67ae72b65e9c1f3179cae942b0a7d94584e55
This commit is contained in:
parent
db29776dd5
commit
1715f95dd4
@ -17,34 +17,19 @@
|
||||
# Markus Zolliker <markus.zolliker@psi.ch>
|
||||
# *****************************************************************************
|
||||
|
||||
import sys
|
||||
import time
|
||||
import threading
|
||||
from frappy.core import Drivable, Parameter, Command, Property, \
|
||||
ERROR, WARN, BUSY, IDLE, nopoll, Limit, TupleOf
|
||||
# from frappy.features import HasSimpleOffset
|
||||
ERROR, WARN, BUSY, IDLE, nopoll, Limit
|
||||
from frappy.datatypes import IntRange, FloatRange, StringType, BoolType
|
||||
from frappy.errors import BadValueError
|
||||
sys.path.append('/home/l_samenv/Documents/anc350/Linux64/userlib/lib')
|
||||
from PyANC350v4 import Positioner
|
||||
from frappy.errors import BadValueError, HardwareError
|
||||
from frappy_psi.pyanc350 import HwAxis
|
||||
|
||||
|
||||
class Stopped(RuntimeError):
|
||||
"""thread was stopped"""
|
||||
|
||||
|
||||
class DriveInfo:
|
||||
def __init__(self, value, target, status=(BUSY, 'changed target')):
|
||||
self.pos = value
|
||||
self.direction = -1 if target < value else 1
|
||||
self.target = target
|
||||
self.status = status
|
||||
self.thread = None
|
||||
self.statusbits = ''
|
||||
self.output = False
|
||||
self.sensor_connected = False
|
||||
|
||||
|
||||
class Axis(Drivable):
|
||||
axis = Property('axis number', IntRange(0, 2), 0)
|
||||
value = Parameter('axis position', FloatRange(unit='deg'))
|
||||
@ -53,19 +38,21 @@ class Axis(Drivable):
|
||||
gear = Parameter('gear factor', FloatRange(), readonly=False, value=1)
|
||||
tolerance = Parameter('positioning tolerance', FloatRange(0, unit='$'),
|
||||
readonly=False, default=0.01)
|
||||
output = Parameter('enable output', BoolType(), readonly=False)
|
||||
sensor_connected = Parameter('a sensor is connected', BoolType())
|
||||
info = Parameter('axis info', StringType())
|
||||
statusbits = Parameter('status bits', StringType())
|
||||
step_mode = Parameter('step mode (soft closed loop)', BoolType(),
|
||||
default=False, readonly=False, group='step_mode')
|
||||
maxtry = Parameter('max. number of move tries', IntRange(),
|
||||
default=5, readonly=False, group='step_mode')
|
||||
steps_fwd = Parameter('forward steps / main unit', FloatRange(0),
|
||||
timeout = Parameter('timeout after no progress detected', FloatRange(0),
|
||||
default=1, readonly=False, group='step_mode')
|
||||
steps_fwd = Parameter('forward steps / main unit', FloatRange(0), unit='$/s',
|
||||
default=0, readonly=False, group='step_mode')
|
||||
steps_bwd = Parameter('backward steps / main unit', FloatRange(0),
|
||||
steps_bwd = Parameter('backward steps / main unit', FloatRange(0, unit='$/s'),
|
||||
default=0, readonly=False, group='step_mode')
|
||||
delay = Parameter('delay between tries within loop', FloatRange(0),
|
||||
delay = Parameter('delay between tries within loop', FloatRange(0, unit='s'),
|
||||
readonly=False, default=0.05, group='step_mode')
|
||||
maxstep = Parameter('max. step duration', FloatRange(0, unit='s'),
|
||||
default=0.25, readonly=False, group='step_mode')
|
||||
prop = Parameter('factor for control loop', FloatRange(0, 1),
|
||||
readonly=False, default=0.8, group='step_mode')
|
||||
target_min = Limit()
|
||||
@ -78,71 +65,98 @@ class Axis(Drivable):
|
||||
SCALES = {'deg': 1, 'm': 1, 'mm': 1000, 'um': 1000000, 'µm': 1000000}
|
||||
_thread = None
|
||||
_moving_since = 0
|
||||
_output = False
|
||||
_sensor_connected = False
|
||||
_status = IDLE, ''
|
||||
_statusbits = None
|
||||
_calib_range = None
|
||||
_try_cnt = 0
|
||||
_at_target = False
|
||||
|
||||
def initModule(self):
|
||||
super().initModule()
|
||||
self._stopped = threading.Event()
|
||||
# TODO: catch timeout
|
||||
self._hw = Positioner()
|
||||
|
||||
def write_gear(self, value):
|
||||
self._scale = self.SCALES[self.parameters['value'].datatype.unit] * self.gear
|
||||
return value
|
||||
self._hw = HwAxis(self.axis)
|
||||
|
||||
def initialReads(self):
|
||||
self.read_info()
|
||||
super().initialReads()
|
||||
|
||||
def shutdownModule(self):
|
||||
self._hw.close()
|
||||
|
||||
def read_value(self):
|
||||
if self._thread:
|
||||
return self.value
|
||||
try:
|
||||
return self._read_pos()
|
||||
except Stopped:
|
||||
return self.value
|
||||
|
||||
def write_gear(self, value):
|
||||
self._scale = self.SCALES[self.parameters['value'].datatype.unit] * self.gear
|
||||
return value
|
||||
|
||||
def read_frequency(self):
|
||||
return self._hw.getFrequency()
|
||||
|
||||
def write_frequency(self, value):
|
||||
self._hw.setFrequency(value)
|
||||
return self._hw.getFrequency()
|
||||
|
||||
def read_amplitude(self):
|
||||
return self._hw.getAmplitude()
|
||||
|
||||
def write_amplitude(self, value):
|
||||
self._hw.setAmplitude(value)
|
||||
return self._hw.getAmplitude()
|
||||
|
||||
def read_statusbits(self):
|
||||
self._get_status()
|
||||
return self.statusbits
|
||||
|
||||
def _get_status(self):
|
||||
"""get axis status
|
||||
|
||||
- update _output and _sensor_connected
|
||||
- update self.sensor_connected and self.statusbits
|
||||
- return <moving flag>, <error flag>, <reason>
|
||||
|
||||
<moving flag> is True whn moving
|
||||
<in_error> is True when in error
|
||||
<reason> is an error text, when in error, 'at target' or '' otherwise
|
||||
"""
|
||||
statusbits = self._hw.getAxisStatus(self.axis)
|
||||
self._sensor_connected, self._output, moving, at_target, fwd_stuck, bwd_stuck, error = statusbits
|
||||
self._statusbits = statusbits
|
||||
statusbits = self._hw.getAxisStatus()
|
||||
self.sensor_connected, self._output, moving, at_target, fwd_stuck, bwd_stuck, error = statusbits
|
||||
self.statusbits = ''.join(k for k, v in zip('OTFBE', (self._output,) + statusbits[3:]) if v)
|
||||
if error:
|
||||
return ERROR, 'other error'
|
||||
if bwd_stuck:
|
||||
return ERROR, 'end of travel backward'
|
||||
if bwd_stuck:
|
||||
if fwd_stuck:
|
||||
return ERROR, 'end of travel forward'
|
||||
target_reached = at_target > self._at_target
|
||||
self._at_target = at_target
|
||||
if self._moving_since:
|
||||
if time.time() < self._moving_since + 1:
|
||||
if target_reached:
|
||||
return IDLE, 'at target'
|
||||
if time.time() < self._moving_since + 0.25:
|
||||
return BUSY, 'started'
|
||||
if at_target:
|
||||
self.setFastPoll(False)
|
||||
self._moving_since = 0
|
||||
return IDLE, 'at target'
|
||||
if moving and self._output:
|
||||
return BUSY, 'moving'
|
||||
if not self._output:
|
||||
return WARN, 'switched output off by unknown reason'
|
||||
return WARN, 'switched moving off by unknown reason'
|
||||
return WARN, 'stopped by unknown reason'
|
||||
if self._moving_since is False:
|
||||
return IDLE, 'stopped'
|
||||
if not self.step_mode and at_target:
|
||||
return IDLE, 'at target'
|
||||
return IDLE, ''
|
||||
|
||||
def check_value(self, value, direction):
|
||||
"""check if value allows moving in current direction"""
|
||||
if direction > 0:
|
||||
if value > self.target_max:
|
||||
raise BadValueError('above upper limit')
|
||||
elif value < self.target_min:
|
||||
raise BadValueError('below lower limit')
|
||||
|
||||
def read_status(self):
|
||||
status = self._get_status()
|
||||
self.statusbits = ''.join(k for k, v in zip('SOMTFBE', self._statusbits) if v)
|
||||
if self.step_mode:
|
||||
return self._status
|
||||
if self._moving_since:
|
||||
if status[0] != BUSY:
|
||||
self._moving_since = 0
|
||||
self.setFastPoll(False)
|
||||
return status
|
||||
|
||||
def _wait(self, delay):
|
||||
@ -150,73 +164,54 @@ class Axis(Drivable):
|
||||
raise Stopped()
|
||||
|
||||
def _read_pos(self):
|
||||
if not self.sensor_connected:
|
||||
return 0
|
||||
poslist = []
|
||||
for i in range(9):
|
||||
if i:
|
||||
self._wait(0.001)
|
||||
poslist.append(self._hw.getPosition(self.axis) * self._scale)
|
||||
poslist.append(self._hw.getPosition() * self._scale)
|
||||
self._poslist = sorted(poslist)
|
||||
return self._poslist[len(poslist) // 2] # median
|
||||
|
||||
def read_value(self):
|
||||
if self._thread:
|
||||
return self.value
|
||||
return self._read_pos()
|
||||
|
||||
def read_frequency(self):
|
||||
return self._hw.getFrequency(self.axis)
|
||||
|
||||
def write_frequency(self, value):
|
||||
self._hw.setFrequency(self.axis, value)
|
||||
return self._hw.getFrequency(self.axis)
|
||||
|
||||
def read_amplitude(self):
|
||||
return self._hw.getAmplitude(self.axis)
|
||||
|
||||
def write_amplitude(self, value):
|
||||
self._hw.setAmplitude(self.axis, value)
|
||||
return self._hw.getAmplitude(self.axis)
|
||||
|
||||
def write_output(self, value):
|
||||
if self._thread:
|
||||
if not value:
|
||||
self.stop()
|
||||
else:
|
||||
self._hw.setAxisOutput(self.axis, enable=value, autoDisable=0)
|
||||
self._output = value
|
||||
return value
|
||||
|
||||
def read_output(self):
|
||||
return self._output
|
||||
|
||||
def _stop_thread(self):
|
||||
if self._thread:
|
||||
self._thread.join()
|
||||
|
||||
def _run_drive(self, target):
|
||||
self.value = self._read_pos()
|
||||
self.status = self._status = BUSY, 'drive by steps'
|
||||
cnt = self.maxtry
|
||||
prev = 0
|
||||
tol = self.tolerance
|
||||
deadline = time.time() + self.timeout
|
||||
max_steps = self.maxstep * self.frequency
|
||||
while True:
|
||||
dif = target - self.value
|
||||
if target > self._poslist[2] + tol: # 78th percentile
|
||||
steps = max(1, min(dif, (dif + tol) * self.prop) * self.steps_fwd)
|
||||
elif target < self._poslist[-3] - tol: # 22th percentile
|
||||
steps = min(-1, max(dif, (dif - tol) * self.prop) * self.steps_bwd)
|
||||
for _ in range(2):
|
||||
dif = target - self.value
|
||||
steps_per_unit = self.steps_bwd if dif < 0 else self.steps_fwd
|
||||
tol = max(self.tolerance, 0.6 / steps_per_unit) # avoid a tolerance less than 60% of a step
|
||||
if abs(dif) > tol * 3:
|
||||
break
|
||||
# extra wait time when already close
|
||||
self._wait(2 * self.delay)
|
||||
self.read_value()
|
||||
status = None
|
||||
if abs(dif) < tol:
|
||||
status = IDLE, 'in tolerance'
|
||||
elif self._poslist[2] <= target <= self._poslist[-3]: # target within noise
|
||||
status = IDLE, 'within noise'
|
||||
elif dif > 0:
|
||||
steps = min(max_steps, min(dif, (dif + tol) * self.prop) * steps_per_unit)
|
||||
else:
|
||||
self._status = IDLE, 'in tolerance'
|
||||
self.read_status()
|
||||
return
|
||||
if cnt <= 0:
|
||||
self._status = ERROR, 'too many tries'
|
||||
self.read_status()
|
||||
return
|
||||
if abs(steps) > prev * 0.7:
|
||||
cnt -= 1
|
||||
prev = abs(steps)
|
||||
steps = max(-max_steps, max(dif, (dif - tol) * self.prop) * steps_per_unit)
|
||||
if status or steps == 0:
|
||||
self._status = status
|
||||
break
|
||||
if round(steps) == 0: # this should not happen
|
||||
self._status = WARN, 'steps=0'
|
||||
break
|
||||
self._move_steps(steps)
|
||||
if self._step_size > self.prop * 0.25 / steps_per_unit:
|
||||
# some progress happened
|
||||
deadline = time.time() + self.timeout
|
||||
elif time.time() > deadline:
|
||||
self._status = WARN, 'timeout - no progress'
|
||||
break
|
||||
self.read_status()
|
||||
|
||||
def _thread_wrapper(self, func, *args):
|
||||
try:
|
||||
@ -226,49 +221,73 @@ class Axis(Drivable):
|
||||
except Exception as e:
|
||||
self._status = ERROR, f'{type(e).__name__} - {e}'
|
||||
finally:
|
||||
self._hw.setAxisOutput(self.axis, enable=0, autoDisable=0)
|
||||
self._hw.setAxisOutput(enable=0, autoDisable=0)
|
||||
self.setFastPoll(False)
|
||||
self._stopped.clear()
|
||||
self._thread = None
|
||||
|
||||
def _stop_thread(self):
|
||||
if self._thread:
|
||||
self._stopped.set()
|
||||
self._thread.join()
|
||||
|
||||
def _start_thread(self, *args):
|
||||
self._stop_thread()
|
||||
thread = threading.Thread(target=self._thread_wrapper, args=args)
|
||||
self._thread = thread
|
||||
thread.start()
|
||||
|
||||
def write_target(self, target):
|
||||
if not self.sensor_connected:
|
||||
raise HardwareError('no sensor connected')
|
||||
self._stop_thread()
|
||||
self._hw.setTargetRange(self.axis, self.tolerance / self._scale)
|
||||
self._hw.setTargetRange(self.tolerance / self._scale)
|
||||
if self.step_mode:
|
||||
self.status = BUSY, 'changed target'
|
||||
self._start_thread(self._run_drive, target)
|
||||
else:
|
||||
self.setFastPoll(True, 0.25)
|
||||
self._hw.setTargetPosition(self.axis, target / self._scale)
|
||||
self._hw.setAxisOutput(self.axis, enable=0, autoDisable=0)
|
||||
self._hw.startAutoMove(self.axis, enable=1, relative=0)
|
||||
self._try_cnt = 0
|
||||
self.setFastPoll(True, self.fast_interval)
|
||||
self._hw.setTargetPosition(target / self._scale)
|
||||
self._hw.setAxisOutput(enable=1, autoDisable=0)
|
||||
self._hw.startAutoMove(enable=1, relative=0)
|
||||
self._moving_since = time.time()
|
||||
self.status = self._get_status()
|
||||
return target
|
||||
|
||||
@Command()
|
||||
def stop(self):
|
||||
self._stopped.set()
|
||||
self._stop_thread()
|
||||
self._status = IDLE, 'stopped'
|
||||
if self.step_mode:
|
||||
self._stop_thread()
|
||||
self._status = IDLE, 'stopped'
|
||||
elif self._moving_since:
|
||||
self._moving_since = False # indicate stop
|
||||
self.read_status()
|
||||
|
||||
@Command(IntRange())
|
||||
def move(self, value):
|
||||
def move(self, steps):
|
||||
"""relative move by number of steps"""
|
||||
self.check_value(self.value, value)
|
||||
self._start_thread(self._run_move, value)
|
||||
self._stop_thread()
|
||||
self.read_value()
|
||||
if steps > 0:
|
||||
if self.value > self.target_max:
|
||||
raise BadValueError('above upper limit')
|
||||
elif self.value < self.target_min:
|
||||
raise BadValueError('below lower limit')
|
||||
self.status = self._status = BUSY, 'moving relative'
|
||||
self._start_thread(self._run_move, steps)
|
||||
|
||||
def _run_move(self, steps):
|
||||
self.setFastPoll(True, self.fast_interval)
|
||||
self._move_steps(steps)
|
||||
self.status = self._status = IDLE, ''
|
||||
|
||||
def _move_steps(self, steps):
|
||||
steps = round(steps)
|
||||
if not steps:
|
||||
return
|
||||
previous = self._read_pos()
|
||||
self._hw.setAxisOutput(self.axis, enable=1, autoDisable=0)
|
||||
self._hw.setAxisOutput(enable=1, autoDisable=0)
|
||||
# wait for output is really on
|
||||
for i in range(100):
|
||||
self._wait(0.001)
|
||||
@ -277,66 +296,101 @@ class Axis(Drivable):
|
||||
break
|
||||
else:
|
||||
raise ValueError('can not switch on output')
|
||||
for i in range(abs(steps)):
|
||||
for cnt in range(abs(steps)):
|
||||
self._hw.setAxisOutput(enable=1, autoDisable=0)
|
||||
if not self._thread:
|
||||
raise Stopped('stopped')
|
||||
self._hw.startSingleStep(self.axis, steps < 0)
|
||||
self._hw.startSingleStep(steps < 0)
|
||||
self._wait(1 / self.frequency)
|
||||
self._get_status()
|
||||
if not self._output and i:
|
||||
steps = i
|
||||
self._step_size = 0
|
||||
if cnt and not self._output:
|
||||
steps = cnt
|
||||
break
|
||||
self._wait(self.delay)
|
||||
self.value = pos = self._read_pos()
|
||||
if self._output:
|
||||
self._step_size = (pos - previous) / steps
|
||||
|
||||
def _run_move(self, steps):
|
||||
self.setFastPoll(True, self.fast_interval)
|
||||
self._move_steps(steps)
|
||||
self.value = self._read_pos()
|
||||
self._step_size = (self.value - previous) / steps
|
||||
|
||||
@Command(IntRange(0))
|
||||
def calib_steps(self, delta):
|
||||
"""relative move by number of steps"""
|
||||
"""calibrate steps_fwd and steps_bwd using <delta> steps forwards and backwards"""
|
||||
if not self.sensor_connected:
|
||||
raise HardwareError('no sensor connected')
|
||||
self._stop_thread()
|
||||
self._status = BUSY, 'calibrate step size'
|
||||
self.read_status()
|
||||
self._start_thread(self._run_calib, delta)
|
||||
|
||||
def _run_calib(self, steps):
|
||||
self.target = self.value = self._read_pos()
|
||||
self.value = self._read_pos()
|
||||
if self._calib_range is None or abs(self.target - self.value) > self._calib_range:
|
||||
self.target = self.value
|
||||
maxfwd = 0
|
||||
maxbwd = 0
|
||||
cntfwd = 0
|
||||
cntbwd = 0
|
||||
self._calib_range = 0
|
||||
for i in range(10):
|
||||
if self.value < self.target:
|
||||
if self.value <= self.target:
|
||||
self._status = BUSY, 'move forwards'
|
||||
self.read_status()
|
||||
self._move_steps(steps)
|
||||
self._move_steps(steps)
|
||||
while self.value < self.target:
|
||||
while True:
|
||||
self._move_steps(steps)
|
||||
if self._step_size:
|
||||
maxfwd = max(maxfwd, self._step_size)
|
||||
cntfwd += 1
|
||||
if self._step_size and self._output:
|
||||
maxfwd = max(maxfwd, self._step_size)
|
||||
cntfwd += 1
|
||||
if self.value > self.target:
|
||||
break
|
||||
else:
|
||||
self._status = BUSY, 'move backwards'
|
||||
self.read_status()
|
||||
self._move_steps(-steps)
|
||||
self._move_steps(-steps)
|
||||
while self.value > self.target:
|
||||
while True:
|
||||
self._move_steps(-steps)
|
||||
if self._step_size:
|
||||
maxbwd = max(maxbwd, self._step_size)
|
||||
cntbwd += 1
|
||||
if self._step_size:
|
||||
maxbwd = max(maxbwd, self._step_size)
|
||||
cntbwd += 1
|
||||
if self.value < self.target:
|
||||
break
|
||||
# keep track how far we had to go for calibration
|
||||
self._calib_range = max(self._calib_range, abs(self.value - self.target))
|
||||
if cntfwd >= 3 and cntbwd >= 3:
|
||||
self.steps_fwd = 1 / maxfwd
|
||||
self.steps_bwd = 1 / maxbwd
|
||||
self._run_drive()
|
||||
self._status = IDLE, 'calib step size done'
|
||||
break
|
||||
else:
|
||||
self._status = WARN, 'calib step size failed'
|
||||
self.read_status()
|
||||
|
||||
# def _measure_cap(self, event):
|
||||
# """do cap measurement in a separate thread, as it may time out"""
|
||||
# axistype = ['linear', 'gonio', 'rotator'][self._hw.getActuatorType()]
|
||||
# name = self._hw.getActuatorName()
|
||||
# self._info = f'{name} {axistype} timeout measuring capacitance'
|
||||
# for _ in range(5):
|
||||
# try:
|
||||
# cap = self._hw.measureCapacitance() * 1e9
|
||||
# break
|
||||
# except Exception:
|
||||
# pass
|
||||
# self.info = f'{name} {axistype} {cap:.3g}nF'
|
||||
# event.set()
|
||||
|
||||
@nopoll
|
||||
def read_info(self):
|
||||
"""read info from controller"""
|
||||
cap = self._hw.measureCapacitance(self.axis) * 1e9
|
||||
axistype = ['linear', 'gonio', 'rotator'][self._hw.getActuatorType(self.axis)]
|
||||
return '%s %s %.3gnF' % (self._hw.getActuatorName(self.axis), axistype, cap)
|
||||
axistype = ['linear', 'gonio', 'rotator'][self._hw.getActuatorType()]
|
||||
name = self._hw.getActuatorName()
|
||||
cap = self._hw.measureCapacitance() * 1e9
|
||||
return f'{name} {axistype} {cap:.3g}nF'
|
||||
|
||||
# self._hw = Positioner()
|
||||
# event = threading.Event()
|
||||
# self._start_thread(self._measure_cap, event)
|
||||
# t = time.time()
|
||||
# if not event.wait(0.25):
|
||||
# print('CAP TIMEOUT')
|
||||
# return self._info
|
||||
# print('CAP', time.time() - t)
|
||||
# return self.info
|
||||
|
||||
45
frappy_psi/pyanc350.py
Normal file
45
frappy_psi/pyanc350.py
Normal file
@ -0,0 +1,45 @@
|
||||
"""wrapper around PyANC350v4
|
||||
|
||||
main purpose: make Positioner thread safe
|
||||
without this, when accessing the device from different threads, timeouts may appear
|
||||
allows also to omit axis in method arguments
|
||||
"""
|
||||
import inspect
|
||||
import threading
|
||||
# make sure PYTHONPATH points to the directory of PyANC350v4.py
|
||||
# e.g. in /home/l_samenv/Documents/anc350/Linux64/userlib/lib
|
||||
from PyANC350v4 import Positioner
|
||||
|
||||
|
||||
class HwAxis:
|
||||
hw = None
|
||||
lock = None
|
||||
axes = set()
|
||||
|
||||
def __init__(self, axisNo):
|
||||
self.axisNo = axisNo
|
||||
if not self.axes:
|
||||
self.axes.add(axisNo)
|
||||
HwAxis.hw = Positioner()
|
||||
HwAxis.lock = threading.Lock()
|
||||
super().__init__()
|
||||
|
||||
def close(self):
|
||||
self.axes.discard(self.axisNo)
|
||||
if not self.axes:
|
||||
self.hw.disconnect()
|
||||
|
||||
|
||||
# wrap methods from Positioner and insert to HwAxis
|
||||
for name, method in Positioner.__dict__.items():
|
||||
try:
|
||||
arguments = inspect.getfullargspec(method).args
|
||||
except TypeError as e:
|
||||
continue
|
||||
if arguments[0:2] == ['self', 'axisNo']:
|
||||
|
||||
def wrapper(self, *args, _method=method, **kwds):
|
||||
with self.lock:
|
||||
return _method(self.hw, self.axisNo, *args, **kwds)
|
||||
|
||||
setattr(HwAxis, name, wrapper)
|
||||
Loading…
x
Reference in New Issue
Block a user