frappy_psi.attocube: add lock protection to hw access

in order to avoid sporadic timeout problems

Change-Id: I36f67ae72b65e9c1f3179cae942b0a7d94584e55

frappy_psi/attocube.py

@@ -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
+    ERROR, WARN, BUSY, IDLE, nopoll, Limit
-# from frappy.features import HasSimpleOffset
 from frappy.datatypes import IntRange, FloatRange, StringType, BoolType
-from frappy.errors import BadValueError
+from frappy.errors import BadValueError, HardwareError
-sys.path.append('/home/l_samenv/Documents/anc350/Linux64/userlib/lib')
+from frappy_psi.pyanc350 import HwAxis
-from PyANC350v4 import Positioner
 
 
 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(),
+    timeout = Parameter('timeout after no progress detected', FloatRange(0),
-                       default=5, readonly=False, group='step_mode')
+                       default=1, readonly=False, group='step_mode')
-    steps_fwd = Parameter('forward steps / main unit', FloatRange(0),
+    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 = HwAxis(self.axis)
-        self._hw = Positioner()
-
-    def write_gear(self, value):
-        self._scale = self.SCALES[self.parameters['value'].datatype.unit] * self.gear
-        return value
 
     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)
+        statusbits = self._hw.getAxisStatus()
-        self._sensor_connected, self._output, moving, at_target, fwd_stuck, bwd_stuck, error = statusbits
+        self.sensor_connected, self._output, moving, at_target, fwd_stuck, bwd_stuck, error = statusbits
-        self._statusbits = 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, 'stopped by unknown reason'
-                return WARN, 'switched output off by unknown reason'
+        if self._moving_since is False:
-            return WARN, 'switched moving off by unknown reason'
+            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
+        deadline = time.time() + self.timeout
-        prev = 0
+        max_steps = self.maxstep * self.frequency
-        tol = self.tolerance
         while True:
+            for _ in range(2):
                 dif = target - self.value
-            if target > self._poslist[2] + tol:  # 78th percentile
+                steps_per_unit = self.steps_bwd if dif < 0 else self.steps_fwd
-                steps = max(1, min(dif, (dif + tol) * self.prop) * self.steps_fwd)
+                tol = max(self.tolerance, 0.6 / steps_per_unit)  # avoid a tolerance less than 60% of a step
-            elif target < self._poslist[-3] - tol:  # 22th percentile
+                if abs(dif) > tol * 3:
-                steps = min(-1, max(dif, (dif - tol) * self.prop) * self.steps_bwd)
+                    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'
+                steps = max(-max_steps, max(dif, (dif - tol) * self.prop) * steps_per_unit)
-                self.read_status()
+            if status or steps == 0:
-                return
+                self._status = status
-            if cnt <= 0:
+                break
-                self._status = ERROR, 'too many tries'
+            if round(steps) == 0:  # this should not happen
-                self.read_status()
+                self._status = WARN, 'steps=0'
-                return
+                break
-            if abs(steps) > prev * 0.7:
-                cnt -= 1
-            prev = abs(steps)
             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._try_cnt = 0
-            self._hw.setTargetPosition(self.axis, target / self._scale)
+            self.setFastPoll(True, self.fast_interval)
-            self._hw.setAxisOutput(self.axis, enable=0, autoDisable=0)
+            self._hw.setTargetPosition(target / self._scale)
-            self._hw.startAutoMove(self.axis, enable=1, relative=0)
+            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()
+        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._stop_thread()
-        self._start_thread(self._run_move, value)
+        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:
+            if cnt and not self._output:
-                steps = i
+                steps = cnt
-                self._step_size = 0
                 break
         self._wait(self.delay)
-        self.value = pos = self._read_pos()
+        self.value = self._read_pos()
-        if self._output:
+        self._step_size = (self.value - previous) / steps
-            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"""
+        """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)
+                while True:
                     self._move_steps(steps)
-                while self.value < self.target:
+                    if self._step_size and self._output:
-                    self._move_steps(steps)
-                if self._step_size:
                         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 True:
-                while self.value > self.target:
                     self._move_steps(-steps)
                     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()]
-        axistype = ['linear', 'gonio', 'rotator'][self._hw.getActuatorType(self.axis)]
+        name = self._hw.getActuatorName()
-        return '%s %s %.3gnF' % (self._hw.getActuatorName(self.axis), axistype, cap)
+        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

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)