diff --git a/cfg/attocube_cfg.py b/cfg/attocube_cfg.py index e703cb9..75bcac1 100644 --- a/cfg/attocube_cfg.py +++ b/cfg/attocube_cfg.py @@ -10,7 +10,9 @@ Mod('r', value = Param(unit='deg'), tolerance = 0.01, target_min = 0, - target_max = 360 + target_max = 360, + steps_fwd = 50, + steps_bwd = 100, # gear = 1.2, ) diff --git a/frappy_psi/attocube.py b/frappy_psi/attocube.py index 6e862ef..d5c7895 100644 --- a/frappy_psi/attocube.py +++ b/frappy_psi/attocube.py @@ -21,15 +21,16 @@ import sys import time import threading from frappy.core import Drivable, Parameter, Command, Property, \ - ERROR, WARN, BUSY, IDLE, Done, nopoll, Limit + ERROR, WARN, BUSY, IDLE, nopoll, Limit, TupleOf # from frappy.features import HasSimpleOffset from frappy.datatypes import IntRange, FloatRange, StringType, BoolType -from frappy.errors import ConfigError, BadValueError +from frappy.errors import BadValueError sys.path.append('/home/l_samenv/Documents/anc350/Linux64/userlib/lib') from PyANC350v4 import Positioner -DIRECTION_NAME = {1: 'forward', -1: 'backward', 0: 'idle'} +class Stopped(RuntimeError): + """thread was stopped""" class DriveInfo: @@ -50,32 +51,41 @@ class Axis(Drivable): frequency = Parameter('frequency', FloatRange(1, unit='Hz'), readonly=False) amplitude = Parameter('amplitude', FloatRange(0, unit='V'), readonly=False) gear = Parameter('gear factor', FloatRange(), readonly=False, value=1) - tolerance = Parameter('positioning tolerance', FloatRange(0, unit='$'), readonly=False, default=0.01) + tolerance = Parameter('positioning tolerance', FloatRange(0, unit='$'), + readonly=False, default=0.01) output = Parameter('enable output', BoolType(), readonly=False) info = Parameter('axis info', StringType()) statusbits = Parameter('status bits', StringType()) - maxtry = Parameter('max. number of move tries', IntRange(), default=3, readonly=False) - settling_time = Property('time to be spent wihtin tolerance', FloatRange(0), default=0.25) - fast_interval = Property('waiting time with in internal drive polls', FloatRange(0), default=0.001) + 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), + default=0, readonly=False, group='step_mode') + steps_bwd = Parameter('backward steps / main unit', FloatRange(0), + default=0, readonly=False, group='step_mode') + delay = Parameter('delay between tries within loop', FloatRange(0), + readonly=False, default=0.05, group='step_mode') + prop = Parameter('factor for control loop', FloatRange(0, 1), + readonly=False, default=0.8, group='step_mode') target_min = Limit() target_max = Limit() - _bad_status_max = 0.05 - _settling_time = 0.1 + fast_interval = 0.25 + _hw = None _scale = 1 # scale for custom units - _move_steps = 0 # number of steps to move (used by move command) SCALES = {'deg': 1, 'm': 1, 'mm': 1000, 'um': 1000000, 'µm': 1000000} - _moving = None # None when not moving, else status text - _idle_status = IDLE, '' - _error_state = '' # empty string: no error - # _history = None - _check_sensor = False - _drive_info = DriveInfo(0, 0, (IDLE, '')) - STATUSBIT_NAMES = ['sensor', 'output', 'moving', 'at_target', 'fwd_stuck', 'bwd_stuck', 'error'] + _thread = None + _moving_since = 0 + _output = False + _sensor_connected = False + _status = IDLE, '' + _statusbits = None def initModule(self): super().initModule() + self._stopped = threading.Event() # TODO: catch timeout self._hw = Positioner() @@ -83,15 +93,14 @@ class Axis(Drivable): self._scale = self.SCALES[self.parameters['value'].datatype.unit] * self.gear return value - def startModule(self, start_events): - # self._history = [0] - super().startModule(start_events) - start_events.queue(self.read_info) + def initialReads(self): + self.read_info() + super().initialReads() - def _get_state(self, info): + def _get_status(self): """get axis status - - update _drive_info.output ans _drive_info.sensor_connected + - update _output and _sensor_connected - return , , is True whn moving @@ -99,85 +108,60 @@ class Axis(Drivable): is an error text, when in error, 'at target' or '' otherwise """ statusbits = self._hw.getAxisStatus(self.axis) - info.sensor_connected, info.output, moving, at_target, fwd_stuck, bwd_stuck, error = statusbits - info.statusbits = statusbits + self._sensor_connected, self._output, moving, at_target, fwd_stuck, bwd_stuck, error = statusbits + self._statusbits = statusbits if error: - return False, True, 'other error' + return ERROR, 'other error' if bwd_stuck: - return False, True, 'end of travel backward' + return ERROR, 'end of travel backward' if bwd_stuck: - return False, True, 'end of travel forward' - if at_target: - return False, False, 'at target' - if moving: - #if not info.output: - # return True, True, 'output off' - return True, False, '' - return False, False, '' + return ERROR, 'end of travel forward' + if self._moving_since: + if time.time() < self._moving_since + 1: + 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 IDLE, '' - def _drive_thread(self, info): - try: - for ntry in range(self.maxtry, 0, -1): - self._hw.setAxisOutput(self.axis, enable=True, autoDisable=0) - self._hw.setTargetRange(self.axis, 0 / self._scale) - self._hw.setTargetPosition(self.axis, info.target / self._scale) - self._hw.startAutoMove(self.axis, enable=1, relative=0) - inside_since = 0 - saved_inside_time = 0 - last_inside = 0 - start_time = time.time() - status = None - while info.thread: - time.sleep(self.fast_interval) - moving, in_error, reason = self._get_state(info) - if time.time() > start_time + 0.05: - if reason: - if in_error: - status = ERROR, reason - else: - status = IDLE, reason - break - if not moving: - status = WARN, 'stopped by unknown reason' - break - info.pos = self._hw.getPosition(self.axis) * self._scale - self.check_value(info.pos) - if abs(info.pos - info.target) < self.tolerance: - last_inside = time.time() - if not inside_since: - inside_since = last_inside - saved_inside_time - if last_inside > inside_since + self._settling_time: - info.status = IDLE, 'in tolerance' - self.doPoll() - return - else: - info.status = BUSY, 'driving' - if inside_since: - saved_inside_time = last_inside - inside_since - inside_since = 0 - if ntry == 1: - info.status = status - else: - info.status = BUSY, f'retry after {status[1]} ({ntry})' - self.doPoll() - except Exception as e: - info.status = ERROR, repr(e) - finally: - info.thread = None - - def check_value(self, value): + def check_value(self, value, direction): """check if value allows moving in current direction""" - if self._drive_info.direction > 0: + 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 + return status + + def _wait(self, delay): + if self._stopped.wait(delay): + raise Stopped() + + def _read_pos(self): + poslist = [] + for i in range(9): + if i: + self._wait(0.001) + poslist.append(self._hw.getPosition(self.axis) * self._scale) + self._poslist = sorted(poslist) + return self._poslist[len(poslist) // 2] # median + def read_value(self): - drv = self._drive_info - if drv.thread: - return drv.pos - return self._hw.getPosition(self.axis) * self._scale + if self._thread: + return self.value + return self._read_pos() def read_frequency(self): return self._hw.getFrequency(self.axis) @@ -193,77 +177,166 @@ class Axis(Drivable): self._hw.setAmplitude(self.axis, value) return self._hw.getAmplitude(self.axis) - #def write_tolerance(self, value): - # self._hw.setTargetRange(self.axis, value / self._scale) - # return value - def write_output(self, value): - self._hw.setAxisOutput(self.axis, enable=value, autoDisable=0) + 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_status(self): - drv = self._drive_info - if not drv.thread: - self._get_state(drv) - self.setFastPoll(False) - self.statusbits = ''.join(k for k, v in zip('SOMTFBE', drv.statusbits) if v) - return drv.status - - def _start_motor(self, target): - self._stop_thread() - self._drive_info = DriveInfo(self.value, target) - self._drive_info.thread = threading.Thread(target=self._drive_thread, args=(self._drive_info,)) - self._drive_info.thread.start() + def read_output(self): + return self._output def _stop_thread(self): - try: - drv = self._drive_info - drv.thread.join() - except AttributeError: - pass + if self._thread: + self._thread.join() - def _stop(self): - self._move_steps = 0 - self.setFastPoll(False) - self._stop_thread() - self.read_status() + 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 + 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) + 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) + self._move_steps(steps) + + def _thread_wrapper(self, func, *args): + try: + func(*args) + except Stopped as e: + self._status = IDLE, str(e) + except Exception as e: + self._status = ERROR, f'{type(e).__name__} - {e}' + finally: + self._hw.setAxisOutput(self.axis, enable=0, autoDisable=0) + self.setFastPoll(False) + self._thread = None + + def _start_thread(self, *args): + thread = threading.Thread(target=self._thread_wrapper, args=args) + self._thread = thread + thread.start() def write_target(self, target): - # self.check_limits(value) - # self._try_count = 0 - self._move_steps = 0 - self._start_motor(target) - # self._history = [self.value] - self.setFastPoll(True, 0.25) + self._stop_thread() + self._hw.setTargetRange(self.axis, 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._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' + self.read_status() + + @Command(IntRange()) + def move(self, value): + """relative move by number of steps""" + self.check_value(self.value, value) + self._start_thread(self._run_move, value) + + 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) + # wait for output is really on + for i in range(100): + self._wait(0.001) + self._get_status() + if self._output: + break + else: + raise ValueError('can not switch on output') + for i in range(abs(steps)): + if not self._thread: + raise Stopped('stopped') + self._hw.startSingleStep(self.axis, steps < 0) + self._wait(1 / self.frequency) + self._get_status() + if not self._output and i: + steps = i + self._step_size = 0 + 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) + + @Command(IntRange(0)) + def calib_steps(self, delta): + """relative move by number of steps""" + self._stop_thread() + self._start_thread(self._run_calib, delta) + + def _run_calib(self, steps): + self.target = self.value = self._read_pos() + maxfwd = 0 + maxbwd = 0 + cntfwd = 0 + cntbwd = 0 + for i in range(10): + if self.value < self.target: + self._move_steps(steps) + self._move_steps(steps) + while self.value < self.target: + self._move_steps(steps) + if self._step_size: + maxfwd = max(maxfwd, self._step_size) + cntfwd += 1 + else: + self._move_steps(-steps) + self._move_steps(-steps) + while self.value > self.target: + self._move_steps(-steps) + if self._step_size: + maxbwd = max(maxbwd, self._step_size) + cntbwd += 1 + 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' + @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) - - @Command() - def stop(self): - self._idle_status = IDLE, 'stopped' if self.isBusy() else '' - self._stop() - self.status = self._idle_status - - @Command(IntRange()) - def move(self, value): - """relative move by number of steps""" - self._direction = 1 if value > 0 else -1 - self.check_value(self.value) - if DIRECTION_NAME[self._direction] in self._error_state: - raise BadValueError('can not move (%s)' % self._error_state) - self._move_steps = value - self._idle_status = IDLE, '' - self.read_status() - self.setFastPoll(True, 1/self.frequency) - - @Command(IntRange()) - def automove(self, flag): - """switch automove and output""" - self._hw.setAxisOutput(self.axis, enable=flag, autoDisable=0) - self._hw.startAutoMove(self.axis, enable=flag, relative=0) \ No newline at end of file