From 0d3df7be922e551295d8c6868bdbcfa150e928bf Mon Sep 17 00:00:00 2001
From: Markus Zolliker <markus.zolliker@psi.ch>
Date: Wed, 6 Oct 2021 09:08:29 +0200
Subject: [PATCH] sameside approaching

Change-Id: I8b73d0ae0475bc727b49edd6f245c2204c51695b
---
 secop_psi/phytron.py | 45 +++++++++++++++++++++++++++++++++++++-------
 1 file changed, 38 insertions(+), 7 deletions(-)

diff --git a/secop_psi/phytron.py b/secop_psi/phytron.py
index 3502333..0070ef1 100644
--- a/secop_psi/phytron.py
+++ b/secop_psi/phytron.py
@@ -48,11 +48,16 @@ class Motor(PersistentMixin, HasIodev, Drivable):
     value = Parameter('angle', FloatRange(unit='deg'), poll=True)
     target = Parameter('target angle', FloatRange(unit='deg'), readonly=False)
     speed = Parameter('', FloatRange(0, 20, unit='deg/s'), readonly=False, poll=True)
+    accel = Parameter('', FloatRange(2, 250, unit='deg/s/s'), readonly=False, poll=True)
     encoder_tolerance = Parameter('', FloatRange(unit='deg'), readonly=False, default=0.01)
     zero = PersistentParam('', FloatRange(unit='deg'), readonly=False, default=0)
     encoder = Parameter('encoder reading', FloatRange(unit='deg'), poll=True)
+    sameside_offset = Parameter('offset when always approaching from the same side',
+                                FloatRange(unit='deg'), readonly=True, default=0)
 
     iodevClass = PhytronIO
+    fast_pollfactor = 0.02
+    _sameside_pending = False
 
     def earlyInit(self):
         self.loadParameters()
@@ -68,6 +73,7 @@ class Motor(PersistentMixin, HasIodev, Drivable):
         return self.get(query)
 
     def read_value(self):
+        prev_enc = self.encoder
         pos = float(self.get('P20R')) + self.zero
         if self.encoder_mode != 'NO':
             enc = self.read_encoder()
@@ -75,13 +81,22 @@ class Motor(PersistentMixin, HasIodev, Drivable):
             enc = pos
         status = self.get('=H')
         if status == 'N':
-            if (self.encoder_mode == 'CHECK' and
-                    abs(enc - pos) > self.speed * 0.1 + self.encoder_tolerance):
-                self.get('SN')  # stop
-                self.status = self.Status.ERROR, 'encoder does not match pos'
+            if self.encoder_mode == 'CHECK':
+                e1, e2 = sorted((prev_enc, enc))
+                if e1 - self.encoder_tolerance <= pos <= e2 + self.encoder_tolerance:
+                    self.status = self.Status.BUSY, 'driving'
+                else:
+                    self.log.error('encoder lag: %g not within %g..%g' % (pos, e1, e2))
+                    self.get('S')  # stop
+                    self.status = self.Status.ERROR, 'encoder lag error'
             else:
                 self.status = self.Status.BUSY, 'driving'
         else:
+            if self._sameside_pending:
+                # drive to real target
+                self.set('A', self.target - self.zero)
+                self._sameside_pending = False
+                return pos
             if (self.encoder_mode == 'CHECK' and
                     abs(enc - pos) > self.encoder_tolerance):
                 self.status = self.Status.ERROR, 'encoder does not match pos'
@@ -102,11 +117,26 @@ class Motor(PersistentMixin, HasIodev, Drivable):
             raise HardwareError('speed factor does not match')
         return float(self.set_get('P14S', int(value * self.speed_factor), 'P14R')) / self.speed_factor
 
+    def read_accel(self):
+        return float(self.get('P15R')) / self.speed_factor
+
+    def write_accel(self, value):
+        if abs(float(self.get('P03R')) * self.speed_factor - 1) > 0.001:
+            raise HardwareError('speed factor does not match')
+        return float(self.set_get('P15S', int(value * self.speed_factor), 'P15R')) / self.speed_factor
+
     def write_target(self, value):
         if self.status[0] == self.Status.ERROR:
             raise HardwareError('need reset')
         self.status = self.Status.BUSY, 'changed target'
-        self.set('A', value - self.zero)
+        if self.sameside_offset:
+            # drive first to target + sameside_offset
+            # we do not optimize when already driving from the right side
+            self._sameside_pending = True
+            self.set('A', value - self.zero + self.sameside_offset)
+        else:
+            self.set('A', value - self.zero)
+        return value
 
     def write_zero(self, value):
         self.zero = value
@@ -114,7 +144,7 @@ class Motor(PersistentMixin, HasIodev, Drivable):
         return Done
 
     def stop(self):
-        self.get('SN')
+        self.get('S')
 
     @Command
     def reset(self):
@@ -130,7 +160,8 @@ class Motor(PersistentMixin, HasIodev, Drivable):
                     self.set('P22S', enc)
                     self.saveParameters()
                 self.set('P20S', enc)  # set pos to encoder
-        self.status = self.Status.IDLE, ''
+            self.read_value()
+        # self.status = self.Status.IDLE, ''
 
 # TODO:
 #  '=E' electronics status