frappy_psi.electromagnet: add sample position class

frappy_psi/electromagnet.py

@@ -20,7 +20,7 @@
 
 import struct
 from frappy.core import BytesIO, HasIO, Drivable, Parameter, Property, IntRange, FloatRange, BoolType, \
-    IDLE, BUSY, ERROR
+    Attached, IDLE, BUSY, ERROR
 from frappy.errors import CommunicationFailedError
 
 
@@ -233,3 +233,34 @@ class Magnet(HasIO, Drivable):
     def doPoll(self):
         self._data = self.io.get_data()
         super().doPoll()
+
+
+class SamplePosition(Drivable):
+
+    motor = Attached()
+
+    target = Parameter('target sample position', datatype=IntRange(0,9), readonly=False)                              # TODO: set range
+    value = Parameter('sample position', datatype=IntRange(0,9))                                                      # TODO: set range
+    pos_offset = Parameter('position of first sample holder in motor units', datatype=FloatRange(), default=10.0, readonly=False)     # TODO: set default
+    pos_diff = Parameter('distance between two sample positions in motor units', datatype=FloatRange(), default=20.0, readonly=False) # TODO: set default
+    tolerance = Parameter('position tolerance', datatype=FloatRange(), default=0.1)                                   # TODO: set tolerance
+    _pos = None
+
+    def read_value(self):
+        if not self.motor.isBusy():
+            val = self.motor.read_value()
+            self._pos = (val - self.pos_offset) / self.pos_diff    # convert motor units to sample position
+            return round(self._pos)
+        return self.value
+
+    def write_target(self, target):
+        val = self.pos_offset + target * self.pos_diff
+        reply = self.motor.write_target(val)
+        return reply
+
+    def read_status(self):
+        if self.motor.isBusy():
+            return BUSY, 'motor is running'
+        if abs(self._pos - self.value) > self.tolerance: 
+            return ERROR, 'intermediate position'
+        return IDLE, ''