diff --git a/frappy_psi/attocube.py b/frappy_psi/attocube.py index d5c7895..5ffcaf4 100644 --- a/frappy_psi/attocube.py +++ b/frappy_psi/attocube.py @@ -17,34 +17,19 @@ # Markus Zolliker # ***************************************************************************** -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 , , is True whn moving is True when in error 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 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 diff --git a/frappy_psi/pyanc350.py b/frappy_psi/pyanc350.py new file mode 100644 index 0000000..398ad95 --- /dev/null +++ b/frappy_psi/pyanc350.py @@ -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)