[newport/smc]: improve error handling

by checking the errorstate after each command.
For this to work reliable, a delay between commands needs to be introduced,
as not all commands reply.
+ fix an oversight in _query, where the prefix (=cmd) was not stripped

Change-Id: I6a68f0b3644c183ce3b9ab8da98698eb1670c3fd
Reviewed-on: https://forge.frm2.tum.de/review/c/frm2/tango/entangle/+/23885

Tested-by: Jenkins Automated Tests <pedersen+jenkins@frm2.tum.de>
Tested-by: Enrico Faulhaber <enrico.faulhaber@frm2.tum.de>
Reviewed-by: Georg Brandl <g.brandl@fz-juelich.de>
Reviewed-by: Enrico Faulhaber <enrico.faulhaber@frm2.tum.de>

entangle/device/newport/smc.py

@@ -20,13 +20,81 @@
 # Module authors:
 #   Stefan Rainow <s.rainow@fz-juelich.de>
 #   Georg Brandl <g.brandl@fz-juelich.de>
+#   Enrico Faulhaber <enrico.faulhaber@frm2.tum.de>
 #
 # *****************************************************************************
 
 from entangle import base
 from entangle.core import ALARM, BUSY, FAULT, OFF, ON, Prop, intrange, subdev
-from entangle.core.errors import NotSupported
-
+from entangle.core.errors import NotSupported, InvalidOperation, TimedOut
+from entangle.lib.util import format_statusbits
+
+
+# sleep between commands, manual says: >= 10ms for single controller, >= 16ms for daisy chained
+DELAY = 0.020
+
+ERROR_CODES = dict(
+    A='Unknown message code or floating point controller address.',
+    B='Controller address not correct.',
+    C='Parameter missing or out of range.',
+    D='Execution not allowed.',
+    E='Home sequence already started.',
+    F='ESP stage name unknown.',
+    G='Target position or displacement out of limits.',
+    H='Execution not allowed in NOT REFERENCED state.',
+    I='Execution not allowed in CONFIGURATION state.',
+    J='Execution not allowed in DISABLE state.',
+    K='Execution not allowed in READY state.',
+    L='Execution not allowed in HOMING state.',
+    M='Execution not allowed in MOVING state.',
+    P='Command not allowed for SMC100PP version.',
+    S='Communication Time Out.',
+    U='Error during EEPROM access.',
+    X='Command not allowed for SMC100CC version.'
+)
+
+FLAGS = [
+    'Negative end of run',
+    'Positive end of run',
+    'Peak current limit',
+    'rms current limit',
+    'Short circuit detected',
+    'Following error',
+    'Time out while homing',
+    'Bad ESP stage',
+    'DC voltage too low',
+    '80W output power exceeded',
+    'undefined bit 10 set',
+    'undefined bit 11 set',
+    'undefined bit 12 set',
+    'undefined bit 13 set',
+    'undefined bit 14 set',
+    'undefined bit 15 set',
+]
+
+STATES = {
+    0x0a: (FAULT, 'NOT REFERENCED from reset'),
+    0x0b: (FAULT, 'NOT REFERENCED from homing'),
+    0x0c: (FAULT, 'NOT REFERENCED from configuration'),
+    0x0d: (FAULT, 'NOT REFERENCED from disable'),
+    0x0e: (FAULT, 'NOT REFERENCED from ready'),
+    0x0f: (FAULT, 'NOT REFERENCED from moving'),
+    0x10: (FAULT, 'NOT REFERENCED: ESP stage error'),
+    0x11: (FAULT, 'NOT REFERENCED from jogging'),
+    0x14: (BUSY, 'in configuration mode'),
+    0x1e: (BUSY, 'homing'), # commanded by RS232
+    0x1f: (BUSY, 'homing by local pad'), # commanded by local pad
+    0x28: (BUSY, 'moving'),
+    0x32: (ON, 'homing succeeded'), # after homing
+    0x33: (ON, ''), # after moving
+    0x34: (ON, ''), # after enableing out of disabled
+    0x35: (ON, ''), # after jogging
+    0x3c: (OFF, 'off'), # disabled after ready
+    0x3d: (OFF, 'off'), # disabled after moving
+    0x3e: (OFF, 'off'), # disbaled after jogging
+    0x46: (BUSY, 'JOGGING from ready'),
+    0x47: (BUSY, 'JOGGING from disable'),
+}
 
 class Motor(base.Motor):
     """Device for the Newport SMC motor controller."""
@@ -43,41 +111,53 @@ class Motor(base.Motor):
         'flowctrl': 'xonxoff',
     }
 
-    def _tell(self, cmd):
-        self._iodev.WriteLine('%s%s' % (self.address, cmd))
+    def _query(self, cmd, writeline=False):
+        cmd = '%d%s' % (self.address, cmd)
+        # always wait DELAY between comms.
+        # always check errorcode after a cmd
+        res = self._iodev.MultiCommunicate(([DELAY if writeline else -DELAY, -DELAY],
+                                            [cmd, '%dTE' % self.address]))
+        te_code = res[-1][-1]
+        if te_code in ERROR_CODES:
+            raise InvalidOperation(ERROR_CODES[te_code])
+        return None if writeline else res[0][len(cmd):]
 
-    def _query(self, cmd):
-        return self._iodev.Communicate('%s%s' % (self.address, cmd))
+    def _tell(self, cmd):
+        self._query(cmd, writeline=True)
 
     def init(self):
         self._iodev = self.connect_client('iodev', self.iodev, 'StringIO')
-        self.hw_version = self._query('VE')
+        self.hw_version = self._query('VE').strip()
 
-        self._tell('SR%f' % self.absmax)
-        self._tell('SL%f' % self.absmin)
+        if self.absmin != self.absmax and self.absmin != 0:
+            self._tell('SR%f' % self.absmax)
+            self._tell('SL%f' % self.absmin)
 
     def state(self):
-        state = self._query('TS')[-2:]
-
-        if state in ('0A', '0B', '0C', '0D', '0E', '0F'):
-            return FAULT, 'not referenced'
-        elif state == '14':
-            return BUSY, 'in configuration mode'
-        elif state in ('1E', '1F'):
-            return BUSY, 'homing'
-        elif state in ('28', '46', '47'):
-            return BUSY, 'moving'
-        elif state in ('32', '33', '34', '35'):
-            return ON, 'idle'
-        elif state in ('3C', '3D', '3E'):
-            return OFF, 'off'
-        # according to the manual these were all the possible states.
-        # just in case there are more:
-        return ALARM, state
+        res = self._query('TS')
+        flags = int(res[-6:-2], 16)
+
+        state = int(res[-2:], 16)
+
+        # status code, status text
+        if state in STATES:
+            stc, stt = STATES[state]
+        else:
+            stc, stt = ALARM, 'unknown state %s encountered!' % hex(state)
+
+        flags = format_statusbits(flags, FLAGS)
+        if flags:
+            stt = '%s, %s' % (flags, stt)
+        return stc, stt
 
     def Reset(self):
         self._tell('RS')
-        self._tell('OR')
+        for _ in range(10):
+            try:
+                self._query('VE')
+                self.Reference()
+            except (InvalidOperation, TimedOut):
+                pass
 
     def On(self):
         self._tell('MM1')
@@ -92,7 +172,7 @@ class Motor(base.Motor):
         raise NotSupported('MoveCont is not supported')
 
     def Reference(self):
-        pass
+        self._tell('OR')
 
     def read_speed(self):
         return float(self._query('VA?'))