diff --git a/frappy_psi/electromagnet.py b/frappy_psi/electromagnet.py index 26ba2fcc..b0c8981f 100644 --- a/frappy_psi/electromagnet.py +++ b/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, ''