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, \
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