diff --git a/frappy_psi/phytron.py b/frappy_psi/phytron.py index ae0528f2..a0584dd1 100644 --- a/frappy_psi/phytron.py +++ b/frappy_psi/phytron.py @@ -22,9 +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, IDLE, BUSY, ERROR, WARN, Limit, BoolType +from frappy.core import Done, Command, HasIO, Parameter, Property, Drivable, \ + Limit, PersistentMixin, PersistentParam, StringIO, IDLE, BUSY, ERROR, WARN, \ + nopoll +from frappy.datatypes import EnumType, FloatRange, IntRange, BoolType, \ + NoneOr, StringType + from frappy.errors import CommunicationFailedError, HardwareError, ImpossibleError from frappy.features import HasOffset from frappy.states import HasStates, status_code, Retry @@ -62,14 +65,13 @@ class PhytronIO(StringIO): class Motor(HasOffset, HasStates, 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) - + circumference = Property('circumference for rotations or zero for linear', NoneOr(FloatRange(0)), value=None) encoder_mode = Parameter('how to treat the encoder', EnumType('encoder', NO=0, READ=1, CHECK=2), default=1, readonly=False) + encoder_type = Property('encoder type', EnumType('encoder_type', relative=0, absolute=1), default=0) check_limit_switches = Parameter('whether limit switches are checked',BoolType(), default=0, readonly=False) - value = PersistentParam('angle', FloatRange(unit='deg')) - status = PersistentParam() + value = Parameter('position', FloatRange(unit='deg')) target = Parameter('target angle', FloatRange(unit='$'), readonly=False) speed = Parameter('', FloatRange(0, 20, unit='$/s'), readonly=False) accel = Parameter('', FloatRange(2, 250, unit='$/s/s'), readonly=False) @@ -91,12 +93,23 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable): _prev_diff = 0 # for checking progress _intermediate_target = 0 _stopped_at = 0 + _pos = _enc = _prev_enc = 0 # internal pos/encoder values, previous encoder _ignore_mask = 0 _limits_touched = 0 _limits_check_mask = LIMITS_MASK status_bits = ['power stage error', 'undervoltage', 'overtemperature', 'active', 'lower switch active', 'upper switch active', 'step failure', 'encoder error'] + def checkProperties(self): + super().checkProperties() + if self.propertyValues['circumference'] is None: + if self.parameters['value'].datatype.unit == 'deg': + self.propertyValues['circumference'] = 360 + self.log.info('rotational axis') + else: + self.propertyValues['circumference'] = 0 + self.log.info('linear axis') + def get(self, cmd): return self.communicate(f'{self.address:x}{self.axis}{cmd}') @@ -118,30 +131,46 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable): def initialReads(self): super().initialReads() - self.clear_errors() + self.reset() - def read_alive_time(self): + def _check_power_cycle(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 + power_cycled = False 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 clear_errors - self._blocking_error = 'lost position' + if self.encoder_mode == 'NO' or self.encoder_type == 'relative': + self._blocking_error = 'power cycle detected - lost position' + else: + self.log.info('fix after power cycle') + power_cycled = True else: # do silently - self.clear_errors() + self.log.warn('first time running -> forget offset') + self.offset = 0 + power_cycled = True self.alive_time = now self.saveParameters() - return now + return power_cycled + + def read_alive_time(self): + if self._check_power_cycle(): + self.reset() + return self.alive_time + + def _get_pos_enc(self): + # get position and encoder position + self._pos = float(self.get('P20R')) * self.sign + self._prev_enc = self._enc + self._enc = self._pos if self.encoder_mode == 'NO' else float(self.get('P22R')) * self.sign def read_value(self): - return float(self.get('P20R')) * self.sign + self.read_encoder() + return self._pos def read_encoder(self): - if self.encoder_mode == 'NO': - return self.value - return float(self.get('P22R')) * self.sign + return self._enc def write_sign(self, value): self.sign = value @@ -199,6 +228,9 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable): self.get('S') def doPoll(self): + self._get_pos_enc() + if self._pos == 0 and self._check_power_cycle(): + self.reset() super().doPoll() if self._running and not self.isBusy(): if time.time() > self._stopped_at + 5: @@ -244,12 +276,10 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable): - in case motor is moving properly return Retry - in case motor is moving in the wrong direction, return self.stopping_for_restart - in case encoder does not follow steps, raise a blocking error + + assumes that read_value() was called before in order to update value and encoder + this is the case when called at a step in a state machine """ - prev_enc = self.encoder - if self.encoder_mode != 'NO': - enc = self.read_encoder() - else: - enc = self.value if not self._running: # at target (self._running is updated in self.read_status()) return False diff = abs(self.value - self._intermediate_target) @@ -263,11 +293,11 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable): self._prev_diff = diff if self.encoder_mode != 'CHECK': return Retry - tol = self.encoder_tolerance + abs(enc - prev_enc) - if abs(self.value - enc) <= tol: + tol = self.encoder_tolerance + abs(self._enc - self._prev_enc) + if abs(self._pos - self._enc) <= tol: return Retry self.log.error('encoder lag: steps %g deviate by more than %g from encoder %g', - self.value, tol, enc) + self._pos, tol, self._enc) self.hw_stop() self.saveParameters() self._blocking_error = 'encoder lag error' @@ -323,13 +353,11 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable): if sm.init: sm.mismatch_count = 0 # finish - pos = self.read_value() - enc = self.read_encoder() if (self.encoder_mode == 'CHECK' and - abs(enc - pos) > self.encoder_tolerance): + abs(self.encoder - self.value) > self.encoder_tolerance): if sm.mismatch_count > 2: - self.log.error('encoder mismatch: abs(%g - %g) < %g', - enc, pos, self.encoder_tolerance) + self.log.error('encoder mismatch: abs(%g - %g) == %g > %g', + self.encoder, self.value, abs(self.encoder - self.value), self.encoder_tolerance) self._blocking_error = 'encoder does not match pos' raise HardwareError(self._blocking_error) sm.mismatch_count += 1 @@ -349,41 +377,43 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable): self.start_machine(self.stopping, status=(BUSY, 'stopping')) @Command - def reset(self): - """the same as clear_errors""" + def clear_errors(self): + """the same as reset, name is required by SECoP""" self.clear_errors() @Command - def clear_errors(self): + def reset(self): """Reset error, set position to encoder""" + self._get_pos_enc() + pos = self._pos + newenc = enc = self._enc + 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 and self.encoder_mode != 'NO': + self.log.info(f'fix encoder {enc:g} -> {newenc:g}') + self.set('P22S', newenc * self.sign) + if abs(newenc - pos) > self.encoder_tolerance: + self.log.info(f'fix pos {pos} to encoder {newenc}') + self.set('P20S', newenc * self.sign) # set pos to encoder + self._get_pos_enc() self.read_value() - if self._blocking_error: - newenc = enc = self.read_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 and self.encoder_mode != 'NO': - self.log.info(f'enc {enc} -> {newenc}') - self.set('P22S', newenc * self.sign) - if newenc != pos: - self.log.info(f'pos {pos} -> {newenc}') - self.set('P20S', newenc * self.sign) # set pos to encoder - self.read_value() - self.read_status() - if self._limits_touched == LIMITS_MASK: - raise ValueError('both limit switches pressed') - if self.status[0] == IDLE: - self.status = IDLE, 'after error reset' - else: - self.status = WARN, self.status[1] - self._ignore_mask = self._limits_touched - self._blocking_error = None - self.target = self.value # clear error in target + self.read_encoder() + sm = self._state_machine + if sm.idle_status is None or sm.idle_status[0] == IDLE: + sm.idle_status = IDLE, 'after error reset' + else: + sm.idle_status = WARN, sm.idle_status[1] + self.read_status() + if self._limits_touched == LIMITS_MASK: + raise ValueError('both limit switches pressed') + self._ignore_mask = self._limits_touched + self._blocking_error = None + self.target = self.value # clear error in target (TODO: check if this is needed)