rework of uniax

- try to deal with anomaly at 17 N
- reduce pid on overshoot
- use new statemachine

cfg/uniax.cfg

@@ -42,10 +42,15 @@ digits = 2
 scale_factor = 0.0156
 offset = 15
 
+[res_io]
+description = io to lakeshore
+class = secop_psi.ls340res.LscIO
+uri = tcp://192.168.127.254:3003
+
 [res]
 description = temperature on uniax stick
 class = secop_psi.ls340res.ResChannel
-uri = tcp://192.168.127.254:3003
+io = res_io
 channel = A
 
 [T]

secop_psi/uniax.py

@@ -25,11 +25,11 @@ import time
 import math
 from secop.core import Drivable, Parameter, FloatRange, Done, \
     Attached, Command, PersistentMixin, PersistentParam, BoolType
-from secop.errors import BadValueError
+from secop.errors import BadValueError, SECoPError
 from secop.lib.statemachine import Retry, StateMachine, Restart
 
 
-class Error(Exception):
+class Error(SECoPError):
     pass
 
 
@@ -38,11 +38,11 @@ class Uniax(PersistentMixin, Drivable):
     motor = Attached()
     transducer = Attached()
     limit = Parameter('abs limit of force', FloatRange(0, 190, unit='N'), readonly=False, default=150)
-    tolerance = Parameter('force tolerance', FloatRange(0, 10, unit='N'), readonly=False, default=0.1)
+    tolerance = Parameter('force tolerance', FloatRange(0, 10, unit='N'), readonly=False, default=0.2)
     slope = PersistentParam('spring constant', FloatRange(unit='deg/N'), readonly=False,
                             default=0.5, persistent='auto')
     pid_i = PersistentParam('integral', FloatRange(), readonly=False, default=0.5, persistent='auto')
-    filter_interval = Parameter('filter time', FloatRange(0, 60, unit='s'), readonly=False, default=1)
+    filter_interval = Parameter('filter time', FloatRange(0, 60, unit='s'), readonly=False, default=5)
     current_step = Parameter('', FloatRange(unit='deg'), default=0)
     force_offset = PersistentParam('transducer offset', FloatRange(unit='N'), readonly=False, default=0,
                                    initwrite=True, persistent='auto')
@@ -57,21 +57,21 @@ class Uniax(PersistentMixin, Drivable):
                                    default=0.2, persistent='auto')
     low_pos = Parameter('max. position for positive forces', FloatRange(unit='deg'), readonly=False, needscfg=False)
     high_pos = Parameter('min. position for negative forces', FloatRange(unit='deg'), readonly=False, needscfg=False)
-    substantial_force = Parameter('min. force change expected within motor play', FloatRange(), default=0)
+    substantial_force = Parameter('min. force change expected within motor play', FloatRange(), default=1)
     motor_play = Parameter('acceptable motor play within hysteresis', FloatRange(), readonly=False, default=10)
-    motor_max_play = Parameter('acceptable motor play outside hysteresis', FloatRange(), readonly=False, default=70)
+    motor_max_play = Parameter('acceptable motor play outside hysteresis', FloatRange(), readonly=False, default=90)
+    timeout = Parameter('driving finishes when no progress within this delay', FloatRange(), readonly=False, default=300)
     pollinterval = 0.1
 
     _mot_target = None  # for detecting manual motor manipulations
     _filter_start = 0
-    _filtered = False
     _cnt = 0
     _sum = 0
     _cnt_rderr = 0
     _cnt_wrerr = 0
-    _in_cnt = 0
     _zero_pos_tol = None
     _state = None
+    _force = None  # raw force
 
     def earlyInit(self):
         super().earlyInit()
@@ -94,7 +94,12 @@ class Uniax(PersistentMixin, Drivable):
         self.current_step = step
         for _ in range(ntry):
             try:
-                self._mot_target = self.motor.write_target(mot.value + step)
+                if abs(mot.value - mot.target) < mot.tolerance:
+                    # make sure rounding erros do not suppress small steps
+                    newpos = mot.target + step
+                else:
+                    newpos = mot.value + step
+                self._mot_target = self.motor.write_target(newpos)
                 self._cnt_wrerr = max(0, self._cnt_wrerr - 1)
                 return True
             except Exception as e:
@@ -106,10 +111,6 @@ class Uniax(PersistentMixin, Drivable):
                 self.motor.reset()
         return False
 
-    def reset_filter(self, now=0.0):
-        self._sum = self._cnt = 0
-        self._filter_start = now or time.time()
-
     def motor_busy(self):
         mot = self.motor
         if mot.isBusy():
@@ -118,6 +119,40 @@ class Uniax(PersistentMixin, Drivable):
             return True
         return False
 
+    def read_value(self):
+        try:
+            self._force = force = self.transducer.read_value()
+            self._cnt_rderr = max(0, self._cnt_rderr - 1)
+        except Exception as e:
+            self._cnt_rderr += 1
+            if self._cnt_rderr > 10:
+                self.stop()
+                self.status = 'ERROR', 'too many read errors: %s' % e
+                self.log.error(self.status[1])
+                self.read_target()
+            return Done
+
+        now = time.time()
+        self._sum += force
+        self._cnt += 1
+        if now < self._filter_start + self.filter_interval:
+            return Done
+        force = self._sum / self._cnt
+        self.reset_filter(now)
+        if abs(force) > self.limit + self.hysteresis:
+            self.motor.stop()
+            self.status = 'ERROR', 'above max limit'
+            self.log.error(self.status[1])
+            self.read_target()
+            return Done
+        if self.zero_pos(force) is None and abs(force) > self.hysteresis:
+            self.set_zero_pos(force, self.motor.read_value())
+        return force
+
+    def reset_filter(self, now=0.0):
+        self._sum = self._cnt = 0
+        self._filter_start = now or time.time()
+
     def zero_pos(self, value):
         """get high_pos or low_pos, depending on sign of value
 
@@ -148,23 +183,143 @@ class Uniax(PersistentMixin, Drivable):
         self._zero_pos_tol[name] = tol
         self.log.info('set %s = %.1f +- %.1f (@%g N)' % (name, pos, tol, force))
         setattr(self, name, pos)
-        return pos
+
+    def cleanup(self, state):
+        """in case of error, set error status"""
+        if state.stopped:  # stop or restart
+            if state.stopped is Restart:
+                return
+            self.status = 'IDLE', 'stopped'
+            self.log.warning('stopped')
+        else:
+            self.status = 'ERROR', str(state.last_error)
+            if isinstance(state.last_error, Error):
+                self.log.error('%s', state.last_error)
+            else:
+                self.log.error('%r raised in state %r', str(state.last_error), state.status_string)
+        self.read_target()  # make target invalid
+        self.motor.stop()
+        self.write_adjusting(False)
+
+    def reset_progress(self, state):
+        state.prev_force = self.value
+        state.prev_pos = self.motor.value
+        state.prev_time = time.time()
+
+    def check_progress(self, state):
+        force_step = self.target - self.value
+        direction = math.copysign(1, force_step)
+        try:
+            force_progress = direction * (self.value - state.prev_force)
+        except AttributeError:  # prev_force undefined?
+            self.reset_progress(state)
+            return True
+        if force_progress >= self.substantial_force:
+            self.reset_progress(state)
+        else:
+            motor_dif = abs(self.motor.value - state.prev_pos)
+            if motor_dif > self.motor_play:
+                if motor_dif > self.motor_max_play:
+                    raise Error('force seems not to change substantially %g %g (%g %g)' % (self.value, self.motor.value, state.prev_force, state.prev_pos))
+                return False
+        return True
+
+    def adjust(self, state):
+        """adjust force"""
+        if state.init:
+            state.phase = 0  # just initialized
+            state.in_since = 0
+            state.direction = math.copysign(1, self.target - self.value)
+            state.pid_fact = 1
+        if self.motor_busy():
+            return Retry()
+        self.value = self._force
+        force_step = self.target - self.value
+        if abs(force_step) < self.tolerance:
+            if state.in_since == 0:
+                state.in_since = state.now
+            if state.now > state.in_since + 10:
+                return self.within_tolerance
+        else:
+            if force_step * state.direction < 0:
+                if state.pid_fact == 1:
+                    self.log.info('overshoot -> adjust with reduced pid_i')
+                state.pid_fact = 0.1
+            state.in_since = 0
+        if state.phase == 0:
+            state.phase = 1
+            self.reset_progress(state)
+            self.write_adjusting(True)
+            self.status = 'BUSY', 'adjusting force'
+        elif not self.check_progress(state):
+            if abs(self.value) < self.hysteresis:
+                if motor_dif > self.motor_play:
+                    self.log.warning('adjusting failed - try to find zero pos')
+                    self.set_zero_pos(self.target, None)
+                    return self.find
+            elif time.time() > state.prev_time + self.timeout:
+                if state.phase == 1:
+                   state.phase = 2
+                   self.log.warning('no substantial progress since %d sec', self.timeout)
+                   self.status = 'IDLE', 'adjusting timeout'
+        self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * state.pid_fact)
+        return Retry()
+
+    def within_tolerance(self, state):
+        """within tolerance"""
+        if state.init:
+            self.status = 'IDLE', 'within tolerance'
+            return Retry()
+        if self.motor_busy():
+            return Retry()
+        force_step = self.target - self.value
+        if abs(force_step) < self.tolerance * 0.5:
+            self.current_step = 0
+        else:
+            self.check_progress(state)
+            self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * 0.1)
+        if abs(force_step) > self.tolerance:
+            return self.out_of_tolerance
+        return Retry()
+
+    def out_of_tolerance(self, state):
+        """out of tolerance"""
+        if state.init:
+            self.status = 'WARN', 'out of tolerance'
+            state.in_since = 0
+            return Retry()
+        if self.motor_busy():
+            return Retry()
+        force_step = self.target - self._force
+        if abs(force_step) < self.tolerance:
+            if state.in_since == 0:
+                state.in_since = state.now
+            if state.now > state.in_since + 10:
+                return self.within_tolerance
+            if abs(force_step) < self.tolerance * 0.5:
+                return Retry()
+        self.check_progress(state)
+        self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * 0.1)
+        return Retry()
 
     def find(self, state):
         """find active (engaged) range"""
         if state.init:
             state.prev_direction = 0  # find not yet started
+            self.reset_progress(state)
         direction = math.copysign(1, self.target)
-        if self.value * direction > self.hysteresis or self.value * direction > self.target * direction:
+        self.value = self._force
+        abs_force = self.value * direction
+        if abs_force > self.hysteresis or abs_force > self.target * direction:
             if self.motor_busy():
                 self.log.info('motor stopped - substantial force detected: %g', self.value)
                 self.motor.stop()
             elif state.prev_direction == 0:
                 return self.adjust
-            if abs(self.value) > self.hysteresis:
+            if abs_force > self.hysteresis:
                 self.set_zero_pos(self.value, self.motor.read_value())
             return self.adjust
-        if self.value * direction < -self.hysteresis:
+        if abs_force < -self.hysteresis:
             state.force_before_free = self.value
             return self.free
         if self.motor_busy():
@@ -199,33 +354,16 @@ class Uniax(PersistentMixin, Drivable):
         self.drive_relative(direction * 360)
         return Retry()
 
-    def cleanup(self, state):
-        """in case of error, set error status"""
-        if state.stopped:  # stop or restart
-            if state.stopped is Restart:
-                return
-            self.status = 'IDLE', 'stopped'
-            self.log.warning('stopped')
-        else:
-            self.status = 'ERROR', str(state.last_error)
-            if isinstance(state.last_error, Error):
-                self.log.error('%s', state.last_error)
-            else:
-                self.log.error('%r raised in state %r', str(state.last_error), state.status_string)
-        self.motor.stop()
-        self.write_adjusting(False)
-
     def free(self, state):
         """free from high force at other end"""
         if state.init:
             state.free_way = None
+            self.reset_progress(state)
         if self.motor_busy():
             return Retry()
-        if abs(self.value) > abs(state.force_before_free) + self.tolerance:
+        self.value = self._force
-            self.stop()
+        if abs(self.value) > abs(state.force_before_free) + self.hysteresis:
-            self.status = 'ERROR', 'force increase while freeing'
+            raise Error('force increase while freeing')
-            self.log.error(self.status[1])
-            return None
         if abs(self.value) < self.hysteresis:
             return self.find
         if state.free_way is None:
@@ -233,93 +371,12 @@ class Uniax(PersistentMixin, Drivable):
             self.log.info('free from high force %g', self.value)
             self.write_adjusting(True)
         direction = math.copysign(1, self.target)
-        if state.free_way > (abs(state.force_before_free) + self.hysteresis) * self.slope:
+        if state.free_way > abs(state.force_before_free + self.hysteresis) * self.slope + self.motor_max_play:
-            self.stop()
+            raise Error('freeing failed')
-            self.status = 'ERROR', 'freeing failed'
-            self.log.error(self.status[1])
-            return None
         state.free_way += self.safe_step
         self.drive_relative(direction * self.safe_step)
         return Retry()
 
-    def within_tolerance(self, state):
-        """within tolerance"""
-        if self.motor_busy():
-            return Retry()
-        if abs(self.target - self.value) > self.tolerance:
-            return self.adjust
-        self.status = 'IDLE', 'within tolerance'
-
-    def adjust(self, state):
-        """adjust force"""
-        if state.init:
-            state.prev_force = None
-        if self.motor_busy():
-            return
-        if abs(self.target - self.value) < self.tolerance:
-            self._in_cnt += 1
-            if self._in_cnt >= 3:
-                return self.within_tolerance
-        else:
-            self._in_cnt = 0
-        if state.prev_force is None:
-            state.prev_force = self.value
-            state.prev_pos = self.motor.pos
-            self.write_adjusting(True)
-            self.status = 'BUSY', 'adjusting force'
-        elif not self._filtered:
-            return
-        else:
-            if abs(self.value - state.prev_force) > self.substantial_force:
-                state.prev_force = self.value
-                state.prev_pos = self.motor.value
-            else:
-                motor_dif = abs(self.value - state.prev_pos)
-                if abs(self.value) < self.hysteresis:
-                    if motor_dif > self.motor_play:
-                        self.log.warning('adjusting failed - try to find zero pos')
-                        self.set_zero_pos(self.target, None)
-                        return self.find
-                elif motor_dif > self.motor_max_play:
-                    raise Error('force seems not to change substantially')
-        force_step = (self.target - self.value) * self.pid_i
-        self.drive_relative(force_step * self.slope)
-        return Retry()
-
-    def read_value(self):
-        try:
-            force = self.transducer.read_value()
-            self._cnt_rderr = max(0, self._cnt_rderr - 1)
-        except Exception as e:
-            self._cnt_rderr += 1
-            if self._cnt_rderr > 10:
-                self.stop()
-                self.status = 'ERROR', 'too many read errors: %s' % e
-                self.log.error(self.status[1])
-            return Done
-
-        now = time.time()
-        if self.motor_busy():
-            # do not filter while driving
-            self.reset_filter()
-            self._filtered = False
-        else:
-            self._sum += force
-            self._cnt += 1
-            if now < self._filter_start + self.filter_interval:
-                return Done
-            force = self._sum / self._cnt
-            self.reset_filter(now)
-            self._filtered = True
-        if abs(force) > self.limit + self.hysteresis:
-            self.motor.stop()
-            self.status = 'ERROR', 'above max limit'
-            self.log.error(self.status[1])
-            return Done
-        if self.zero_pos(force) is None and abs(force) > self.hysteresis and self._filtered:
-            self.set_zero_pos(force, self.motor.read_value())
-        return force
-
     def write_target(self, target):
         if abs(target) > self.limit:
             raise BadValueError('force above limit')
@@ -332,11 +389,19 @@ class Uniax(PersistentMixin, Drivable):
         self._cnt_rderr = 0
         self._cnt_wrerr = 0
         self.status = 'BUSY', 'changed target'
+        self.target = target
         if self.value * math.copysign(1, target) > self.hysteresis:
             self._state.start(self.adjust)
         else:
             self._state.start(self.find)
-        return target
+        return Done
+
+    def read_target(self):
+        if self._state.state is None:
+            if self.status[1]:
+                raise Error(self.status[1])
+            raise Error('inactive')
+        return self.target
 
     @Command()
     def stop(self):