diff --git a/frappy_psi/trinamic.py b/frappy_psi/trinamic.py index 5cb71fa..86b128e 100644 --- a/frappy_psi/trinamic.py +++ b/frappy_psi/trinamic.py @@ -26,11 +26,11 @@ import struct from frappy.core import BoolType, Command, EnumType, FloatRange, IntRange, \ HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, Done, \ - IDLE, BUSY, ERROR + IDLE, BUSY, ERROR, Limit from frappy.io import BytesIO from frappy.errors import CommunicationFailedError, HardwareError, RangeError, IsBusyError from frappy.rwhandler import ReadHandler, WriteHandler -from frappy.lib import formatStatusBits +from frappy.lib import formatStatusBits, clamp MOTOR_STOP = 3 MOVE = 4 @@ -68,6 +68,7 @@ HW_ARGS = { 'error_bits': (208, 1), 'free_wheeling': (204, 0.01), 'power_down_delay': (214, 0.01), + 'stall_thr': (174, 1), } # special handling (adjust zero): @@ -94,50 +95,55 @@ class Motor(PersistentMixin, HasIO, Drivable): move_limit = Parameter('max. angle to drive in one go when current above safe_current', FloatRange(unit='$'), - readonly=False, default=360, group='more') + readonly=False, value=360, group='more') safe_current = Parameter('motor current allowed for big steps', FloatRange(unit='A'), - readonly=False, default=0.2, group='more') + readonly=False, value=0.2, group='more') tolerance = Parameter('positioning tolerance', FloatRange(unit='$'), - readonly=False, default=0.9) + readonly=False, value=0.9) encoder_tolerance = writable('the allowed deviation between steppos and encoder\n\nmust be > tolerance', - FloatRange(0, 360., unit='$', fmtstr='%.3f'), group='more') + FloatRange(0, 360., unit='$', fmtstr='%.3f'), value=3.6, group='more') has_encoder = Parameter('whether encoder is used or not', BoolType(), readonly=False, default=True, group='more') - speed = writable('max. speed', FloatRange(0, MAX_SPEED, unit='$/sec', fmtstr='%.1f'), default=40) + speed = writable('max. speed', FloatRange(0, MAX_SPEED, unit='$/sec', fmtstr='%.1f'), value=40) minspeed = writable('min. speed', FloatRange(0, MAX_SPEED, unit='$/sec', fmtstr='%.1f'), - default=SPEED_SCALE, group='motorparam') + value=SPEED_SCALE, group='motorparam') currentspeed = Parameter('current speed', FloatRange(-MAX_SPEED, MAX_SPEED, unit='$/sec', fmtstr='%.1f'), group='motorparam') maxcurrent = writable('', FloatRange(0, 2.8, unit='A', fmtstr='%.2f'), - default=1.4, group='motorparam') + value=1.4, group='motorparam') standby_current = writable('', FloatRange(0, 2.8, unit='A', fmtstr='%.2f'), - default=0.1, group='motorparam') + value=0.1, group='motorparam') acceleration = writable('', FloatRange(4.6 * ACCEL_SCALE, MAX_ACCEL, unit='deg/s^2', fmtstr='%.1f'), - default=150., group='motorparam') + value=150., group='motorparam') target_reached = Parameter('', BoolType(), group='hwstatus') move_status = Parameter('', EnumType(ok=0, stalled=1, encoder_deviation=2, stalled_and_encoder_deviation=3), group='hwstatus') error_bits = Parameter('', IntRange(0, 255), group='hwstatus') - home = Parameter('state of home switch (input 3)', BoolType()) + home = Parameter('state of home switch (input 3)', BoolType(), group='more') has_home = Parameter('enable home and activate pullup resistor', BoolType(), default=True, group='more') - auto_reset = Parameter('automatic reset after failure', BoolType(), readonly=False, default=False) + auto_reset = Parameter('automatic reset after failure', BoolType(), group='more', readonly=False, default=False) free_wheeling = writable('', FloatRange(0, 60., unit='sec', fmtstr='%.2f'), - default=0.1, group='motorparam') + value=0.1, group='motorparam') power_down_delay = writable('', FloatRange(0, 60., unit='sec', fmtstr='%.2f'), - default=0.1, group='motorparam') + value=0.1, group='motorparam') baudrate = Parameter('', EnumType({f'{v}': i for i, v in enumerate(BAUDRATES)}), readonly=False, default=0, visibility=3, group='more') + max_retry = Parameter('maximum number of retries', IntRange(0, 99), readonly=False, default=0) + stall_thr = Parameter('stallGuard threshold', IntRange(-64, 63), readonly=False, value=0) pollinterval = Parameter(group='more') + target_min = Limit() + target_max = Limit() ioClass = BytesIO - fast_poll = 0.05 + fast_poll = 0.5 _run_mode = 0 # 0: idle, 1: driving, 2: stopping _calc_timeout = True _need_reset = None _last_change = 0 _loading = False # True when loading parameters + _drv_try = 0 def comm(self, cmd, adr, value=0, bank=0): """set or get a parameter @@ -206,11 +212,12 @@ class Motor(PersistentMixin, HasIO, Drivable): set status to error when encoder has changed be more than encoder_tolerance """ # calculate nearest, most probable value - adjusted_encoder = encoder_from_hw + round((self.encoder - encoder_from_hw) / 360.) * 360 - if abs(self.encoder - adjusted_encoder) >= self.encoder_tolerance: + candidate = clamp(self.target_min, self.encoder, self.target_max) + adjusted_encoder = encoder_from_hw + round((candidate - encoder_from_hw) / 360.) * 360 + if abs(candidate - adjusted_encoder) >= self.encoder_tolerance: # encoder modulo 360 has changed - self.log.error('saved encoder value (%.2f) does not match reading (%.2f %.2f)', - self.encoder, encoder_from_hw, adjusted_encoder) + self.log.error('saved encoder value (%.2f) does not match reading (%.2f)', + candidate, adjusted_encoder) if adjusted_encoder != encoder_from_hw: self.log.info('take next closest encoder value (%.2f)', adjusted_encoder) self._need_reset = True @@ -307,16 +314,16 @@ class Motor(PersistentMixin, HasIO, Drivable): if self.steppos != oldpos: self._last_change = now return BUSY, 'stopping' if self._run_mode == 2 else 'moving' - if now < self._last_change + 0.3 and not (self.read_target_reached() or self.read_move_status()): - return BUSY, 'stopping' if self._run_mode == 2 else 'moving' - if self.target_reached: - reason = '' - elif self.move_status: + if now < self._last_change + 0.25 + self.fast_poll: + return BUSY, 'stopping' if self._run_mode == 2 else 'moving' + if self.read_move_status(): reason = self.move_status.name - elif self.error_bits: - reason = formatStatusBits(self.error_bits, ( + elif self.read_error_bits() & 127: + reason = formatStatusBits(self.error_bits & 127, ( 'stallGuard', 'over_temp', 'over_temp_warn', 'short_A', 'short_B', 'open_load_A', 'open_load_B', 'standstill')) + elif self.target_reached: + reason = '' else: reason = 'unknown' self.setFastPoll(False) @@ -330,13 +337,18 @@ class Motor(PersistentMixin, HasIO, Drivable): if reason: self.log.warning('target reached, but move_status = %s', reason) return IDLE, '' + if self._drv_try < self.max_retry: + self._drv_try += 1 + self.log.info('try again %.1f %s', diff, reason) + self.start_motor(self.target) + return BUSY, f'try again' if self.auto_reset: self._need_reset = True return IDLE, f'stalled: {reason}' self.log.error('out of tolerance by %.3g (%s)', diff, reason) return ERROR, f'out of tolerance ({reason})' - def write_target(self, target): + def start_motor(self, target): for _ in range(2): # for auto reset self.read_value() # make sure encoder and steppos are fresh if self.maxcurrent >= self.safe_current + CURRENT_SCALE and ( @@ -369,6 +381,11 @@ class Motor(PersistentMixin, HasIO, Drivable): self.setFastPoll(True, self.fast_poll) self.log.debug('move to %.1f', target) self.comm(MOVE, 0, (target - self.zero) / ANGLE_SCALE) + + def write_target(self, target): + self._drv_try = 0 + self._stable_since = 0 + self.start_motor(target) self.status = BUSY, 'changed target' return target