phytron with HasStates

test version

Change-Id: Ifbaa35421a64faad32c87bbcf758c236f900cd0b

frappy_psi/phytron.py

@@ -28,6 +28,7 @@ from frappy.core import Done, Command, EnumType, FloatRange, IntRange, \
     StringIO, StringType, IDLE, BUSY, ERROR, Limit
 from frappy.errors import CommunicationFailedError, HardwareError
 from frappy.features import HasOffset
+from frappy.states import HasStates, status_code, Retry
 
 
 class PhytronIO(StringIO):
@@ -54,7 +55,7 @@ class PhytronIO(StringIO):
         return reply[1:]
 
 
-class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
+class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable):
     axis = Property('motor axis X or Y', StringType(), default='X')
     address = Property('address', IntRange(0, 15), default=0)
     circumference = Property('cirumference for rotations or zero for linear', FloatRange(0), default=360)
@@ -76,14 +77,21 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
     target_max = Limit()
     alive_time = PersistentParam('alive time for detecting restarts',
                                  FloatRange(), default=0, export=False)
+    sim_error = Parameter('run time error simulation', StringType(), readonly=False, default='')
 
     ioClass = PhytronIO
-    fast_poll = 0.1
-    _backlash_pending = False
-    _mismatch_count = 0
     _step_size = None  # degree / step
     _reset_needed = False
 
+    STATUS_MAP = {
+        '08': (IDLE, ''),
+        '01': (ERROR, 'power stage failure'),
+        '02': (ERROR, 'power too low'),
+        '04': (ERROR, 'power stage over temperature'),
+        '07': (ERROR, 'no power stage'),
+        '80': (ERROR, 'encoder failure'),
+    }
+
     def get(self, cmd):
         return self.communicate(f'{self.address:x}{self.axis}{cmd}')
 
@@ -107,7 +115,7 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
         now = time.time()
         axisbit = 1 << int(self.axis == 'Y')
         active_axes = int(self.get('P37R'))  # adr 37 is a custom address with no internal meaning
-        if not (axisbit & active_axes):  # power cycle detected and this axis not yet active
+        if not axisbit & active_axes:  # power cycle detected and this axis not yet active
             self.set('P37S', axisbit | active_axes)  # activate axis
             if now < self.alive_time + 7 * 24 * 3600:  # the device was running within last week
                 # inform the user about the loss of position by the need of doing reset_error
@@ -119,53 +127,7 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
         return now
 
     def read_value(self):
-        prev_enc = self.encoder
+        return float(self.get('P20R')) * self.sign
-        pos = float(self.get('P20R')) * self.sign
-        if self.encoder_mode != 'NO':
-            enc = self.read_encoder()
-        else:
-            enc = pos
-        status = self.communicate(f'{self.address:x}SE')
-        status = status[0:4] if self.axis == 'X' else status[4:8]
-        self.log.debug('run %s enc %s end %s', status[1], status[2], status[3])
-        status = self.get('=H')
-        if status == 'N':  # not at target
-            if self.encoder_mode == 'CHECK':
-                e1, e2 = sorted((prev_enc, enc))
-                if e1 - self.encoder_tolerance <= pos <= e2 + self.encoder_tolerance:
-                    self.status = BUSY, 'driving'
-                else:
-                    self.log.error('encoder lag: %g not within %g..%g',
-                                   pos, e1, e2)
-                    self.get('S')  # stop
-                    self.status = ERROR, 'encoder lag error'
-                    self.value = pos
-                    self.saveParameters()
-                    self.setFastPoll(False)
-            else:
-                self.status = BUSY, 'driving'
-        else:
-            if self._backlash_pending:
-                # drive to real target
-                self.set('A', self.sign * self.target)
-                self._backlash_pending = False
-                return pos
-            if (self.encoder_mode == 'CHECK' and
-                    abs(enc - pos) > self.encoder_tolerance):
-                if self._mismatch_count < 2:
-                    self._mismatch_count += 1
-                else:
-                    self.log.error('encoder mismatch: abs(%g - %g) < %g',
-                                   enc, pos, self.encoder_tolerance)
-                    self.status = ERROR, 'encoder does not match pos'
-            else:
-                self._mismatch_count = 0
-                self.status = IDLE, ''
-                self.value = pos
-                self.read_encoder()  # let encoder quickly match value
-                self.saveParameters()
-            self.setFastPoll(False)
-        return pos
 
     def read_encoder(self):
         if self.encoder_mode == 'NO':
@@ -210,18 +172,82 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
             raise HardwareError(self.status[1])
         if self.status[0] == ERROR:
             raise HardwareError('need reset')
-        self.status = BUSY, 'changed target'
         self.saveParameters()
         if self.backlash:
             # drive first to target + backlash
             # we do not optimize when already driving from the right side
-            self._backlash_pending = True
             self.set('A', self.sign * (value + self.backlash))
+            self.start_machine(self.predriving)
         else:
             self.set('A', self.sign * value)
-        self.setFastPoll(True, self.fast_poll)
+            self.start_machine(self.driving)
         return value
 
+    def read_status(self):
+        if self.sim_error:
+            sysstatus = self.sim_error
+        else:
+            sysstatus = self.communicate(f'{self.address:x}SE')
+            sysstatus = sysstatus[2:4] if self.axis == 'X' else sysstatus[6:8]
+        status = self.STATUS_MAP.get(sysstatus) or (ERROR, f'unknown error {sysstatus}')
+        if status[0] == ERROR:
+            return status
+        return super().read_status()  # status from state machine
+
+    def check_moving(self):
+        prev_enc = self.encoder
+        if self.encoder_mode != 'NO':
+            enc = self.read_encoder()
+        else:
+            enc = self.value
+        status = self.get('=H')
+        if status != 'N':  # at target
+            return False
+        if self.encoder_mode != 'CHECK':
+            return True
+        e1, e2 = sorted((prev_enc, enc))
+        if e1 - self.encoder_tolerance <= self.value <= e2 + self.encoder_tolerance:
+            return True
+        self.log.error('encoder lag: %g not within %g..%g',
+                       self.value, e1, e2)
+        self.get('S')  # stop
+        self.saveParameters()
+        raise HardwareError('encoder lag error')
+
+    @status_code(BUSY)
+    def driving(self, sm):
+        if self.check_moving():
+            return Retry
+        if self.backlash:
+            # drive to real target
+            self.set('A', self.sign * self.target)
+            return self.driving_to_final
+        return self.finishing
+
+    @status_code(BUSY)
+    def driving_to_final(self, sm):
+        if self.check_moving():
+            return Retry
+        return self.finishing
+
+    @status_code(BUSY)
+    def finishing(self, sm):
+        if sm.init:
+            sm.mismatch_count = 0
+        # finish
+        pos = self.read_value()
+        enc = self.read_encoder()
+        if (self.encoder_mode == 'CHECK' and
+                abs(enc - pos) > self.encoder_tolerance):
+            if sm.mismatch_count > 2:
+                self.log.error('encoder mismatch: abs(%g - %g) < %g',
+                               enc, pos, self.encoder_tolerance)
+                raise HardwareError('encoder does not match pos')
+            sm.mismatch_count += 1
+            return Retry
+        self.saveParameters()
+        return self.final_status(IDLE, '')
+
     def stop(self):
         self.get('S')
 
@@ -230,7 +256,7 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
         """Reset error, set position to encoder"""
         self.read_value()
         if self.status[0] == ERROR or self._reset_needed:
-            newenc = enc = self.encoder
+            newenc = enc = self.read_encoder()
             pos = self.value
             if abs(enc - pos) > self.encoder_tolerance or self.encoder_mode == 'NO':
                 if self.circumference:
@@ -242,11 +268,14 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
                     else:
                         mid = enc
                     newenc += round((mid - enc) / self.circumference) * self.circumference
-                if newenc != enc:
+                if newenc != enc and self.encoder_mode != 'NO':
+                    self.log.info(f'enc {enc} -> {newenc}')
                     self.set('P22S', newenc * self.sign)
             if newenc != pos:
+                self.log.info(f'pos {pos} -> {newenc}')
                 self.set('P20S', newenc * self.sign)  # set pos to encoder
             self.read_value()
+            self.status = 'IDLE', 'after error reset'
             self._reset_needed = False
 
 # TODO: