add limits to phytron

Change-Id: I431374efab7a858db6a652f78533b13b12ba6d57

secop_psi/phytron.py

@@ -20,11 +20,13 @@
 #
 # *****************************************************************************
 
-"""driver for pythron motors"""
+"""driver for phytron motors"""
 
 from secop.core import Done, Command, EnumType, FloatRange, IntRange, \
-    HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, StringIO, StringType
-from secop.errors import CommunicationFailedError, HardwareError
+    HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, \
+    StringIO, StringType, TupleOf
+from secop.errors import CommunicationFailedError, HardwareError, BadValueError
+from secop.lib import clamp
 
 
 class PhytronIO(StringIO):
@@ -65,14 +67,24 @@ class Motor(PersistentMixin, HasIO, Drivable):
     encoder = Parameter('encoder reading', FloatRange(unit='deg'))
     sameside_offset = Parameter('offset when always approaching from the same side',
                                 FloatRange(unit='deg'), readonly=False, default=0)
+    abslimits = Parameter('abs limits (raw values)', default=(0, 0),
+                          datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg')))
+    userlimits = PersistentParam('user limits', readonly=False, default=(0, 0), initwrite=True,
+                                 datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg')))
 
     ioClass = PhytronIO
     fast_poll = 0.1
     _sameside_pending = False
     _mismatch_count = 0
+    _rawlimits = None
 
     def earlyInit(self):
         super().earlyInit()
+        if self.abslimits == (0, 0):
+            self.abslimits = -9e99, 9e99
+        if self.userlimits == (0, 0):
+            self._rawlimits = self.abslimits
+            self.read_userlimits()
         self.loadParameters()
 
     def get(self, cmd):
@@ -151,10 +163,20 @@ class Motor(PersistentMixin, HasIO, Drivable):
             raise HardwareError('speed factor does not match')
         return float(self.set_get('P15S', int(value * self.speed_factor), 'P15R')) / self.speed_factor
 
+    def _check_limits(self, *values):
+        for name, (mn, mx) in ('user', self._rawlimits), ('abs', self.abslimits):
+            mn -= self.offset
+            mx -= self.offset
+            for v in values:
+                if not (mn <= v <= mx):
+                    raise BadValueError('%s limits violation: %g <= %g <= %g' % (name, mn, v, mx))
+                v += self.offset
+
     def write_target(self, value):
         if self.status[0] == self.Status.ERROR:
             raise HardwareError('need reset')
         self.status = self.Status.BUSY, 'changed target'
+        self._check_limits(value, value + self.sameside_offset)
         if self.sameside_offset:
             # drive first to target + sameside_offset
             # we do not optimize when already driving from the right side
@@ -165,8 +187,18 @@ class Motor(PersistentMixin, HasIO, Drivable):
         self.setFastPoll(True, self.fast_poll)
         return value
 
+    def read_userlimits(self):
+        return self._rawlimits[0] - self.offset, self._rawlimits[1] - self.offset
+
+    def write_userlimits(self, value):
+        self._rawlimits = [clamp(self.abslimits[0], v + self.offset, self.abslimits[1]) for v in value]
+        value = self.read_userlimits()
+        self.saveParameters()
+        return value
+
     def write_offset(self, value):
         self.offset = value
+        self.read_userlimits()
         self.saveParameters()
         return Done
 
@@ -182,6 +214,7 @@ class Motor(PersistentMixin, HasIO, Drivable):
             pos = self.value + self.offset
             if abs(enc - pos) > self.encoder_tolerance:
                 if enc < 0:
+                    # assume we have a rotation (not a linear motor)
                     while enc < 0:
                         self.offset += 360
                         enc += 360