From 33142416314b8030cd33f24ffdea4b5582a34a66 Mon Sep 17 00:00:00 2001 From: Markus Zolliker Date: Fri, 5 May 2023 13:16:41 +0200 Subject: [PATCH] [WIP] phytron improvements - Limits - offset - power cycle behaviour Change-Id: Id2f717c362cd7e1e37f180c8130b0e086e724389 --- frappy_psi/phytron.py | 144 +++++++++++++++++++++--------------------- 1 file changed, 72 insertions(+), 72 deletions(-) diff --git a/frappy_psi/phytron.py b/frappy_psi/phytron.py index fe619fc..ca43725 100644 --- a/frappy_psi/phytron.py +++ b/frappy_psi/phytron.py @@ -22,11 +22,12 @@ """driver for phytron motors""" +import time from frappy.core import Done, Command, EnumType, FloatRange, IntRange, \ HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, \ - StringIO, StringType, TupleOf -from frappy.errors import CommunicationFailedError, HardwareError, BadValueError -from frappy.lib import clamp + StringIO, StringType, IDLE, BUSY, ERROR, Limit +from frappy.errors import CommunicationFailedError, HardwareError +from frappy.features import HasOffset class PhytronIO(StringIO): @@ -54,43 +55,35 @@ class PhytronIO(StringIO): return reply[1:] -class Motor(PersistentMixin, HasIO, Drivable): +class Motor(HasOffset, 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) encoder_mode = Parameter('how to treat the encoder', EnumType('encoder', NO=0, READ=1, CHECK=2), default=1, readonly=False) - value = Parameter('angle', FloatRange(unit='deg')) + value = PersistentParam('angle', FloatRange(unit='deg')) + status = PersistentParam() target = Parameter('target angle', FloatRange(unit='deg'), readonly=False) speed = Parameter('', FloatRange(0, 20, unit='deg/s'), readonly=False) accel = Parameter('', FloatRange(2, 250, unit='deg/s/s'), readonly=False) encoder_tolerance = Parameter('', FloatRange(unit='deg'), readonly=False, default=0.01) - offset = PersistentParam('', FloatRange(unit='deg'), readonly=False, default=0) sign = PersistentParam('', IntRange(-1,1), readonly=False, default=1) encoder = Parameter('encoder reading', FloatRange(unit='deg')) backlash = Parameter("""backlash compensation\n offset for 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'))) + target_min = Limit() + target_max = Limit() + alive_time = PersistentParam('alive time for detecting restarts', + FloatRange(), default=0) # export=False ioClass = PhytronIO fast_poll = 0.1 _backlash_pending = False _mismatch_count = 0 - _rawlimits = None _step_size = None # degree / step - - 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() + _reset_needed = False def get(self, cmd): return self.communicate('%x%s%s' % (self.address, self.axis, cmd)) @@ -111,9 +104,24 @@ class Motor(PersistentMixin, HasIO, Drivable): self.set(cmd, value) return self.get(query) + def read_alive_time(self): + 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 + 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 + self._reset_needed = True + else: # do reset silently + self.reset_error() + self.alive_time = now + self.saveParameters() + return now + def read_value(self): prev_enc = self.encoder - pos = float(self.get('P20R')) * self.sign - self.offset + pos = float(self.get('P20R')) * self.sign if self.encoder_mode != 'NO': enc = self.read_encoder() else: @@ -122,23 +130,25 @@ class Motor(PersistentMixin, HasIO, Drivable): 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': + 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 = self.Status.BUSY, 'driving' + self.status = BUSY, 'driving' else: self.log.error('encoder lag: %g not within %g..%g', pos, e1, e2) self.get('S') # stop - self.status = self.Status.ERROR, 'encoder lag error' + self.status = ERROR, 'encoder lag error' + self.value = pos + self.saveParameters() self.setFastPoll(False) else: - self.status = self.Status.BUSY, 'driving' + self.status = BUSY, 'driving' else: if self._backlash_pending: # drive to real target - self.set('A', self.sign * (self.target + self.offset)) + self.set('A', self.sign * self.target) self._backlash_pending = False return pos if (self.encoder_mode == 'CHECK' and @@ -148,17 +158,19 @@ class Motor(PersistentMixin, HasIO, Drivable): else: self.log.error('encoder mismatch: abs(%g - %g) < %g', enc, pos, self.encoder_tolerance) - self.status = self.Status.ERROR, 'encoder does not match pos' + self.status = ERROR, 'encoder does not match pos' else: self._mismatch_count = 0 - self.status = self.Status.IDLE, '' + self.status = IDLE, '' + self.value = pos + self.saveParameters() self.setFastPoll(False) return pos def read_encoder(self): if self.encoder_mode == 'NO': return self.value - return float(self.get('P22R')) * self.sign - self.offset + return float(self.get('P22R')) * self.sign def write_sign(self, value): self.sign = value @@ -187,68 +199,56 @@ class Motor(PersistentMixin, HasIO, Drivable): self.get_step_size() return float(self.set_get('P15S', round(value / self._step_size), 'P15R')) * self._step_size - 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 check_target(self, value): + self.checkLimits(value) + self.checkLimits(value + self.backlash) def write_target(self, value): - if self.status[0] == self.Status.ERROR: + self.read_alive_time() + if self._reset_needed: + self.status = ERROR, 'reset needed after power up (probably position lost)' + raise HardwareError(self.status[1]) + if self.status[0] == ERROR: raise HardwareError('need reset') - self.status = self.Status.BUSY, 'changed target' - self._check_limits(value, value + self.backlash) + 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.offset + self.backlash)) + self.set('A', self.sign * (value + self.backlash)) else: - self.set('A', self.sign * (value + self.offset)) + self.set('A', self.sign * value) 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 - def stop(self): self.get('S') @Command - def reset(self): + def reset_error(self): """Reset error, set position to encoder""" self.read_value() - if self.status[0] == self.Status.ERROR: - enc = self.encoder + self.offset - 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 - self.set('P22S', enc * self.sign) - self.saveParameters() - self.set('P20S', enc * self.sign) # set pos to encoder + if self.status[0] == ERROR or self._reset_needed: + newenc = enc = self.encoder + pos = self.value + if abs(enc - pos) > self.encoder_tolerance or self.encoder_mode == 'NO': + if self.circumference: + # bring encoder value either within or as close as possible to the given range + if enc < self.target_min: + mid = self.target_min + 0.5 * min(self.target_max - self.target_min, self.circumference) + elif enc > self.target_max: + mid = self.target_max - 0.5 * min(self.target_max - self.target_min, self.circumference) + else: + mid = enc + newenc += round((mid - enc) / self.circumference) * self.circumference + if newenc != enc: + self.set('P22S', newenc * self.sign) + if newenc != pos: + self.set('P20S', newenc * self.sign) # set pos to encoder self.read_value() - # self.status = self.Status.IDLE, '' + self._reset_needed = False # TODO: # '=E' electronics status # '=I+' / '=I-': limit switches -# use P37 to determine if restarted