From 0b9e227669cdf50d86e042bb3501254bc05ba159 Mon Sep 17 00:00:00 2001
From: Markus Zolliker <markus.zolliker@psi.ch>
Date: Fri, 16 Sep 2022 08:14:14 +0200
Subject: [PATCH] state after discussion with users

- cirterium for "no substantial forece change" must be improved
---
 secop_psi/uniax.py | 18 ++++++++++++------
 1 file changed, 12 insertions(+), 6 deletions(-)

diff --git a/secop_psi/uniax.py b/secop_psi/uniax.py
index 22443be..51a2182 100644
--- a/secop_psi/uniax.py
+++ b/secop_psi/uniax.py
@@ -52,6 +52,8 @@ 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)
+    motor_play = Parameter('summed steps without substantial change', FloatRange(), default=0)
+    max_play = Parameter('max. summed steps without substantial change', FloatRange(), readonly=False, default=70)
     pollinterval = 0.1
     fast_pollfactor = 1
 
@@ -64,7 +66,6 @@ class Uniax(PersistentMixin, Drivable):
     _action = None
     _last_force = 0
     _expected_step = 1
-    _fail_cnt = 0
     _in_cnt = 0
     _init_action = False
     _zero_pos_tol = None
@@ -248,12 +249,13 @@ class Uniax(PersistentMixin, Drivable):
         if abs(target - force) < self.tolerance:
             self._in_cnt += 1
             if self._in_cnt >= 3:
+                self.motor_play = 0
                 self.next_action(self.within_tolerance)
                 return
         else:
             self._in_cnt = 0
         if self.init_action():
-            self._fail_cnt = 0
+            self.motor_play = 0
             self.write_adjusting(True)
             self.status = 'BUSY', 'adjusting force'
         elif not self._filtered:
@@ -263,16 +265,20 @@ class Uniax(PersistentMixin, Drivable):
             if self._expected_step:
                 # compare detected / expected step
                 q = force_step / self._expected_step
+                mstep = self._expected_step * self.slope
                 if q < 0.1:
-                    self._fail_cnt += 1
+                    self.motor_play += mstep
                 elif q > 0.5:
-                    self._fail_cnt = max(0, self._fail_cnt - 1)
-                if self._fail_cnt >= 10:
+                    if abs(self.motor_play) <= abs(mstep):
+                        self.motor_play = 0
+                    else:
+                        self.motor_play = self.motor_play * (1 - abs(mstep / self.motor_play))
+                if abs(self.motor_play) >= 10:
                     if force < self.hysteresis:
                         self.log.warning('adjusting failed - try to find zero pos')
                         self.set_zero_pos(target, None)
                         self.next_action(self.find)
-                    elif self._fail_cnt > 20:
+                    elif abs(self.motor_play) > self.max_play:
                         self.stop()
                         self.status = 'ERROR', 'force seems not to change substantially'
                         self.log.error(self.status[1])