reworked attocube

- step_mode: soft closed loop, stepwise, reading encoder after a delay
- calib_steps command to determine step size

Change-Id: I27bdffb4d564ac9c55a6473704ac2de6ad92bac8

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,
 )
 

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)
+    step_mode = Parameter('step mode (soft closed loop)', BoolType(),
-    settling_time = Property('time to be spent wihtin tolerance', FloatRange(0), default=0.25)
+                          default=False, readonly=False, group='step_mode')
-    fast_interval = Property('waiting time with in internal drive polls', FloatRange(0), default=0.001)
+    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
+    fast_interval = 0.25
-    _settling_time = 0.1
+
     _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
+    _thread = None
-    _idle_status = IDLE, ''
+    _moving_since = 0
-    _error_state = ''  # empty string: no error
+    _output = False
-    # _history = None
+    _sensor_connected = False
-    _check_sensor = False
+    _status = IDLE, ''
-    _drive_info = DriveInfo(0, 0, (IDLE, ''))
+    _statusbits = None
-    STATUSBIT_NAMES = ['sensor', 'output', 'moving', 'at_target', 'fwd_stuck', 'bwd_stuck', 'error']
 
     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):
+    def initialReads(self):
-        # self._history = [0]
+        self.read_info()
-        super().startModule(start_events)
+        super().initialReads()
-        start_events.queue(self.read_info)
 
-    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 <moving flag>, <error flag>, <reason>
 
         <moving flag> is True whn moving
@@ -99,85 +108,60 @@ class Axis(Drivable):
         <reason> 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
+        self._sensor_connected, self._output, moving, at_target, fwd_stuck, bwd_stuck, error = statusbits
-        info.statusbits = 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'
+            return ERROR, 'end of travel forward'
-        if at_target:
+        if self._moving_since:
-            return False, False, 'at target'
+            if time.time() < self._moving_since + 1:
-        if moving:
+                return BUSY, 'started'
-            #if not info.output:
+            if at_target:
-            #    return True, True, 'output off'
+                self.setFastPoll(False)
-            return True, False, ''
+                self._moving_since = 0
-        return False, False, ''
+                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):
+    def check_value(self, value, direction):
-        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):
         """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 self._thread:
-        if drv.thread:
+            return self.value
-            return drv.pos
+        return self._read_pos()
-        return self._hw.getPosition(self.axis) * self._scale
 
     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):
+    def read_output(self):
-        drv = self._drive_info
+        return self._output
-        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 _stop_thread(self):
-        try:
+        if self._thread:
-            drv = self._drive_info
+            self._thread.join()
-            drv.thread.join()
-        except AttributeError:
-            pass
 
-    def _stop(self):
+    def _run_drive(self, target):
-        self._move_steps = 0
+        self.value = self._read_pos()
-        self.setFastPoll(False)
+        self.status = self._status = BUSY, 'drive by steps'
-        self._stop_thread()
+        cnt = self.maxtry
-        self.read_status()
+        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._stop_thread()
-        # self._try_count = 0
+        self._hw.setTargetRange(self.axis, self.tolerance / self._scale)
-        self._move_steps = 0
+        if self.step_mode:
-        self._start_motor(target)
+            self.status = BUSY, 'changed target'
-        # self._history = [self.value]
+            self._start_thread(self._run_drive, target)
-        self.setFastPoll(True, 0.25)
+        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)