linse/frappy
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

frappy_psi.trinamic: improve

Browse Source 
- try several times in case of no success (configurable)
- write default values to the controller
- add limits

Change-Id: I7974d7056bbad48091653378366943ccf3e6f945
This commit is contained in:
zolliker 2024-09-23 15:02:31 +02:00
parent dd0cb2e1dc
commit 7fdb2e7e2f
1 changed files with 45 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
frappy_psi/trinamic.py
View File

@ -26,11 +26,11 @@ import struct
from frappy.core import BoolType, Command, EnumType, FloatRange, IntRange, \ from frappy.core import BoolType, Command, EnumType, FloatRange, IntRange, \
HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, Done, \ HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, Done, \
IDLE, BUSY, ERROR IDLE, BUSY, ERROR, Limit
from frappy.io import BytesIO from frappy.io import BytesIO
from frappy.errors import CommunicationFailedError, HardwareError, RangeError, IsBusyError from frappy.errors import CommunicationFailedError, HardwareError, RangeError, IsBusyError
from frappy.rwhandler import ReadHandler, WriteHandler from frappy.rwhandler import ReadHandler, WriteHandler
from frappy.lib import formatStatusBits from frappy.lib import formatStatusBits, clamp
MOTOR_STOP = 3 MOTOR_STOP = 3
MOVE = 4 MOVE = 4
@ -68,6 +68,7 @@ HW_ARGS = {
'error_bits': (208, 1), 'error_bits': (208, 1),
'free_wheeling': (204, 0.01), 'free_wheeling': (204, 0.01),
'power_down_delay': (214, 0.01), 'power_down_delay': (214, 0.01),
'stall_thr': (174, 1),
} }
# special handling (adjust zero): # 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', move_limit = Parameter('max. angle to drive in one go when current above safe_current',
FloatRange(unit='$'), 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'), 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='$'), 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', 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(), has_encoder = Parameter('whether encoder is used or not', BoolType(),
readonly=False, default=True, group='more') 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'), 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'), currentspeed = Parameter('current speed', FloatRange(-MAX_SPEED, MAX_SPEED, unit='$/sec', fmtstr='%.1f'),
group='motorparam') group='motorparam')
maxcurrent = writable('', FloatRange(0, 2.8, unit='A', fmtstr='%.2f'), 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'), 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'), 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') target_reached = Parameter('', BoolType(), group='hwstatus')
move_status = Parameter('', EnumType(ok=0, stalled=1, encoder_deviation=2, stalled_and_encoder_deviation=3), move_status = Parameter('', EnumType(ok=0, stalled=1, encoder_deviation=2, stalled_and_encoder_deviation=3),
group='hwstatus') group='hwstatus')
error_bits = Parameter('', IntRange(0, 255), 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(), has_home = Parameter('enable home and activate pullup resistor', BoolType(),
default=True, group='more') 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'), 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'), 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)}), baudrate = Parameter('', EnumType({f'{v}': i for i, v in enumerate(BAUDRATES)}),
readonly=False, default=0, visibility=3, group='more') 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') pollinterval = Parameter(group='more')
target_min = Limit()
target_max = Limit()
ioClass = BytesIO ioClass = BytesIO
fast_poll = 0.05 fast_poll = 0.5
_run_mode = 0 # 0: idle, 1: driving, 2: stopping _run_mode = 0 # 0: idle, 1: driving, 2: stopping
_calc_timeout = True _calc_timeout = True
_need_reset = None _need_reset = None
_last_change = 0 _last_change = 0
_loading = False # True when loading parameters _loading = False # True when loading parameters
_drv_try = 0
def comm(self, cmd, adr, value=0, bank=0): def comm(self, cmd, adr, value=0, bank=0):
"""set or get a parameter """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 set status to error when encoder has changed be more than encoder_tolerance
""" """
# calculate nearest, most probable value # calculate nearest, most probable value
adjusted_encoder = encoder_from_hw + round((self.encoder - encoder_from_hw) / 360.) * 360 candidate = clamp(self.target_min, self.encoder, self.target_max)
if abs(self.encoder - adjusted_encoder) >= self.encoder_tolerance: 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 # encoder modulo 360 has changed
self.log.error('saved encoder value (%.2f) does not match reading (%.2f %.2f)', self.log.error('saved encoder value (%.2f) does not match reading (%.2f)',
self.encoder, encoder_from_hw, adjusted_encoder) candidate, adjusted_encoder)
if adjusted_encoder != encoder_from_hw: if adjusted_encoder != encoder_from_hw:
self.log.info('take next closest encoder value (%.2f)', adjusted_encoder) self.log.info('take next closest encoder value (%.2f)', adjusted_encoder)
self._need_reset = True self._need_reset = True
@ -307,16 +314,16 @@ class Motor(PersistentMixin, HasIO, Drivable):
if self.steppos != oldpos: if self.steppos != oldpos:
self._last_change = now self._last_change = now
return BUSY, 'stopping' if self._run_mode == 2 else 'moving' 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()): if now < self._last_change + 0.25 + self.fast_poll:
return BUSY, 'stopping' if self._run_mode == 2 else 'moving' return BUSY, 'stopping' if self._run_mode == 2 else 'moving'
if self.target_reached: if self.read_move_status():
reason = ''
elif self.move_status:
reason = self.move_status.name reason = self.move_status.name
elif self.error_bits: elif self.read_error_bits() & 127:
reason = formatStatusBits(self.error_bits, ( reason = formatStatusBits(self.error_bits & 127, (
'stallGuard', 'over_temp', 'over_temp_warn', 'short_A', 'short_B', 'stallGuard', 'over_temp', 'over_temp_warn', 'short_A', 'short_B',
'open_load_A', 'open_load_B', 'standstill')) 'open_load_A', 'open_load_B', 'standstill'))
elif self.target_reached:
reason = ''
else: else:
reason = 'unknown' reason = 'unknown'
self.setFastPoll(False) self.setFastPoll(False)
@ -330,13 +337,18 @@ class Motor(PersistentMixin, HasIO, Drivable):
if reason: if reason:
self.log.warning('target reached, but move_status = %s', reason) self.log.warning('target reached, but move_status = %s', reason)
return IDLE, '' 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: if self.auto_reset:
self._need_reset = True self._need_reset = True
return IDLE, f'stalled: {reason}' return IDLE, f'stalled: {reason}'
self.log.error('out of tolerance by %.3g (%s)', diff, reason) self.log.error('out of tolerance by %.3g (%s)', diff, reason)
return ERROR, f'out of tolerance ({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 for _ in range(2): # for auto reset
self.read_value() # make sure encoder and steppos are fresh self.read_value() # make sure encoder and steppos are fresh
if self.maxcurrent >= self.safe_current + CURRENT_SCALE and ( 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.setFastPoll(True, self.fast_poll)
self.log.debug('move to %.1f', target) self.log.debug('move to %.1f', target)
self.comm(MOVE, 0, (target - self.zero) / ANGLE_SCALE) 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' self.status = BUSY, 'changed target'
return target return target