From 84d0d44f380e16934aca5ab644e7a4612c463703 Mon Sep 17 00:00:00 2001 From: Markus Zolliker Date: Mon, 15 May 2023 08:33:14 +0200 Subject: [PATCH] phytron with HasStates test version Change-Id: Ifbaa35421a64faad32c87bbcf758c236f900cd0b --- frappy_psi/phytron.py | 143 +++++++++++++++++++++++++----------------- 1 file changed, 86 insertions(+), 57 deletions(-) diff --git a/frappy_psi/phytron.py b/frappy_psi/phytron.py index d4680af..cdbc024 100644 --- a/frappy_psi/phytron.py +++ b/frappy_psi/phytron.py @@ -28,6 +28,7 @@ from frappy.core import Done, Command, EnumType, FloatRange, IntRange, \ StringIO, StringType, IDLE, BUSY, ERROR, Limit from frappy.errors import CommunicationFailedError, HardwareError from frappy.features import HasOffset +from frappy.states import HasStates, status_code, Retry class PhytronIO(StringIO): @@ -54,7 +55,7 @@ class PhytronIO(StringIO): return reply[1:] -class Motor(HasOffset, PersistentMixin, HasIO, Drivable): +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) @@ -76,14 +77,21 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable): target_max = Limit() alive_time = PersistentParam('alive time for detecting restarts', FloatRange(), default=0, export=False) + sim_error = Parameter('run time error simulation', StringType(), readonly=False, default='') ioClass = PhytronIO - fast_poll = 0.1 - _backlash_pending = False - _mismatch_count = 0 _step_size = None # degree / step _reset_needed = False + STATUS_MAP = { + '08': (IDLE, ''), + '01': (ERROR, 'power stage failure'), + '02': (ERROR, 'power too low'), + '04': (ERROR, 'power stage over temperature'), + '07': (ERROR, 'no power stage'), + '80': (ERROR, 'encoder failure'), + } + def get(self, cmd): return self.communicate(f'{self.address:x}{self.axis}{cmd}') @@ -107,7 +115,7 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable): 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 + 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 @@ -119,53 +127,7 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable): return now def read_value(self): - prev_enc = self.encoder - pos = float(self.get('P20R')) * self.sign - if self.encoder_mode != 'NO': - enc = self.read_encoder() - else: - enc = pos - status = self.communicate(f'{self.address:x}SE') - 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': # 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 = BUSY, 'driving' - else: - self.log.error('encoder lag: %g not within %g..%g', - pos, e1, e2) - self.get('S') # stop - self.status = ERROR, 'encoder lag error' - self.value = pos - self.saveParameters() - self.setFastPoll(False) - else: - self.status = BUSY, 'driving' - else: - if self._backlash_pending: - # drive to real target - self.set('A', self.sign * self.target) - self._backlash_pending = False - return pos - if (self.encoder_mode == 'CHECK' and - abs(enc - pos) > self.encoder_tolerance): - if self._mismatch_count < 2: - self._mismatch_count += 1 - else: - self.log.error('encoder mismatch: abs(%g - %g) < %g', - enc, pos, self.encoder_tolerance) - self.status = ERROR, 'encoder does not match pos' - else: - self._mismatch_count = 0 - self.status = IDLE, '' - self.value = pos - self.read_encoder() # let encoder quickly match value - self.saveParameters() - self.setFastPoll(False) - return pos + return float(self.get('P20R')) * self.sign def read_encoder(self): if self.encoder_mode == 'NO': @@ -210,18 +172,82 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable): raise HardwareError(self.status[1]) if self.status[0] == ERROR: raise HardwareError('need reset') - 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.backlash)) + self.start_machine(self.predriving) else: self.set('A', self.sign * value) - self.setFastPoll(True, self.fast_poll) + self.start_machine(self.driving) return value + def read_status(self): + if self.sim_error: + sysstatus = self.sim_error + else: + sysstatus = self.communicate(f'{self.address:x}SE') + sysstatus = sysstatus[2:4] if self.axis == 'X' else sysstatus[6:8] + status = self.STATUS_MAP.get(sysstatus) or (ERROR, f'unknown error {sysstatus}') + if status[0] == ERROR: + return status + return super().read_status() # status from state machine + + def check_moving(self): + prev_enc = self.encoder + if self.encoder_mode != 'NO': + enc = self.read_encoder() + else: + enc = self.value + status = self.get('=H') + if status != 'N': # at target + return False + if self.encoder_mode != 'CHECK': + return True + e1, e2 = sorted((prev_enc, enc)) + if e1 - self.encoder_tolerance <= self.value <= e2 + self.encoder_tolerance: + return True + self.log.error('encoder lag: %g not within %g..%g', + self.value, e1, e2) + self.get('S') # stop + self.saveParameters() + raise HardwareError('encoder lag error') + + @status_code(BUSY) + def driving(self, sm): + if self.check_moving(): + return Retry + if self.backlash: + # drive to real target + self.set('A', self.sign * self.target) + return self.driving_to_final + return self.finishing + + @status_code(BUSY) + def driving_to_final(self, sm): + if self.check_moving(): + return Retry + return self.finishing + + @status_code(BUSY) + def finishing(self, sm): + 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): + if sm.mismatch_count > 2: + self.log.error('encoder mismatch: abs(%g - %g) < %g', + enc, pos, self.encoder_tolerance) + raise HardwareError('encoder does not match pos') + sm.mismatch_count += 1 + return Retry + self.saveParameters() + return self.final_status(IDLE, '') + def stop(self): self.get('S') @@ -230,7 +256,7 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable): """Reset error, set position to encoder""" self.read_value() if self.status[0] == ERROR or self._reset_needed: - newenc = enc = self.encoder + newenc = enc = self.read_encoder() pos = self.value if abs(enc - pos) > self.encoder_tolerance or self.encoder_mode == 'NO': if self.circumference: @@ -242,11 +268,14 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable): else: mid = enc newenc += round((mid - enc) / self.circumference) * self.circumference - if newenc != enc: + 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.status = 'IDLE', 'after error reset' self._reset_needed = False # TODO: