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

[WIP] phytron improvements

Browse Source 
- Limits
- offset
- power cycle behaviour

Change-Id: Id2f717c362cd7e1e37f180c8130b0e086e724389
This commit is contained in:
zolliker 2023-05-05 13:16:41 +02:00
parent 3fcd72b189
commit 3314241631
1 changed files with 72 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
frappy_psi/phytron.py
View File

@ -22,11 +22,12 @@
"""driver for phytron motors""" """driver for phytron motors"""
import time
from frappy.core import Done, Command, EnumType, FloatRange, IntRange, \ from frappy.core import Done, Command, EnumType, FloatRange, IntRange, \
HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, \ HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, \
StringIO, StringType, TupleOf StringIO, StringType, IDLE, BUSY, ERROR, Limit
from frappy.errors import CommunicationFailedError, HardwareError, BadValueError from frappy.errors import CommunicationFailedError, HardwareError
from frappy.lib import clamp from frappy.features import HasOffset
class PhytronIO(StringIO): class PhytronIO(StringIO):
@ -54,43 +55,35 @@ class PhytronIO(StringIO):
return reply[1:] return reply[1:]
class Motor(PersistentMixin, HasIO, Drivable): class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
axis = Property('motor axis X or Y', StringType(), default='X') axis = Property('motor axis X or Y', StringType(), default='X')
address = Property('address', IntRange(0, 15), default=0) address = Property('address', IntRange(0, 15), default=0)
circumference = Property('cirumference for rotations or zero for linear', FloatRange(0), default=360)
encoder_mode = Parameter('how to treat the encoder', EnumType('encoder', NO=0, READ=1, CHECK=2), encoder_mode = Parameter('how to treat the encoder', EnumType('encoder', NO=0, READ=1, CHECK=2),
default=1, readonly=False) default=1, readonly=False)
value = Parameter('angle', FloatRange(unit='deg')) value = PersistentParam('angle', FloatRange(unit='deg'))
status = PersistentParam()
target = Parameter('target angle', FloatRange(unit='deg'), readonly=False) target = Parameter('target angle', FloatRange(unit='deg'), readonly=False)
speed = Parameter('', FloatRange(0, 20, unit='deg/s'), readonly=False) speed = Parameter('', FloatRange(0, 20, unit='deg/s'), readonly=False)
accel = Parameter('', FloatRange(2, 250, unit='deg/s/s'), readonly=False) accel = Parameter('', FloatRange(2, 250, unit='deg/s/s'), readonly=False)
encoder_tolerance = Parameter('', FloatRange(unit='deg'), readonly=False, default=0.01) encoder_tolerance = Parameter('', FloatRange(unit='deg'), readonly=False, default=0.01)
offset = PersistentParam('', FloatRange(unit='deg'), readonly=False, default=0)
sign = PersistentParam('', IntRange(-1,1), readonly=False, default=1) sign = PersistentParam('', IntRange(-1,1), readonly=False, default=1)
encoder = Parameter('encoder reading', FloatRange(unit='deg')) encoder = Parameter('encoder reading', FloatRange(unit='deg'))
backlash = Parameter("""backlash compensation\n backlash = Parameter("""backlash compensation\n
offset for always approaching from the same side""", offset for always approaching from the same side""",
FloatRange(unit='deg'), readonly=False, default=0) FloatRange(unit='deg'), readonly=False, default=0)
abslimits = Parameter('abs limits (raw values)', default=(0, 0), target_min = Limit()
datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg'))) target_max = Limit()
userlimits = PersistentParam('user limits', readonly=False, default=(0, 0), initwrite=True, alive_time = PersistentParam('alive time for detecting restarts',
datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg'))) FloatRange(), default=0) # export=False
ioClass = PhytronIO ioClass = PhytronIO
fast_poll = 0.1 fast_poll = 0.1
_backlash_pending = False _backlash_pending = False
_mismatch_count = 0 _mismatch_count = 0
_rawlimits = None
_step_size = None # degree / step _step_size = None # degree / step
_reset_needed = False
def earlyInit(self):
super().earlyInit()
if self.abslimits == (0, 0):
self.abslimits = -9e99, 9e99
if self.userlimits == (0, 0):
self._rawlimits = self.abslimits
self.read_userlimits()
self.loadParameters()
def get(self, cmd): def get(self, cmd):
return self.communicate('%x%s%s' % (self.address, self.axis, cmd)) return self.communicate('%x%s%s' % (self.address, self.axis, cmd))
@ -111,9 +104,24 @@ class Motor(PersistentMixin, HasIO, Drivable):
self.set(cmd, value) self.set(cmd, value)
return self.get(query) return self.get(query)
def read_alive_time(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
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
self._reset_needed = True
else: # do reset silently
self.reset_error()
self.alive_time = now
self.saveParameters()
return now
def read_value(self): def read_value(self):
prev_enc = self.encoder prev_enc = self.encoder
pos = float(self.get('P20R')) * self.sign - self.offset pos = float(self.get('P20R')) * self.sign
if self.encoder_mode != 'NO': if self.encoder_mode != 'NO':
enc = self.read_encoder() enc = self.read_encoder()
else: else:
@ -122,23 +130,25 @@ class Motor(PersistentMixin, HasIO, Drivable):
status = status[0:4] if self.axis == 'X' else status[4:8] 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]) self.log.debug('run %s enc %s end %s', status[1], status[2], status[3])
status = self.get('=H') status = self.get('=H')
if status == 'N': if status == 'N': # not at target
if self.encoder_mode == 'CHECK': if self.encoder_mode == 'CHECK':
e1, e2 = sorted((prev_enc, enc)) e1, e2 = sorted((prev_enc, enc))
if e1 - self.encoder_tolerance <= pos <= e2 + self.encoder_tolerance: if e1 - self.encoder_tolerance <= pos <= e2 + self.encoder_tolerance:
self.status = self.Status.BUSY, 'driving' self.status = BUSY, 'driving'
else: else:
self.log.error('encoder lag: %g not within %g..%g', self.log.error('encoder lag: %g not within %g..%g',
pos, e1, e2) pos, e1, e2)
self.get('S') # stop self.get('S') # stop
self.status = self.Status.ERROR, 'encoder lag error' self.status = ERROR, 'encoder lag error'
self.value = pos
self.saveParameters()
self.setFastPoll(False) self.setFastPoll(False)
else: else:
self.status = self.Status.BUSY, 'driving' self.status = BUSY, 'driving'
else: else:
if self._backlash_pending: if self._backlash_pending:
# drive to real target # drive to real target
self.set('A', self.sign * (self.target + self.offset)) self.set('A', self.sign * self.target)
self._backlash_pending = False self._backlash_pending = False
return pos return pos
if (self.encoder_mode == 'CHECK' and if (self.encoder_mode == 'CHECK' and
@ -148,17 +158,19 @@ class Motor(PersistentMixin, HasIO, Drivable):
else: else:
self.log.error('encoder mismatch: abs(%g - %g) < %g', self.log.error('encoder mismatch: abs(%g - %g) < %g',
enc, pos, self.encoder_tolerance) enc, pos, self.encoder_tolerance)
self.status = self.Status.ERROR, 'encoder does not match pos' self.status = ERROR, 'encoder does not match pos'
else: else:
self._mismatch_count = 0 self._mismatch_count = 0
self.status = self.Status.IDLE, '' self.status = IDLE, ''
self.value = pos
self.saveParameters()
self.setFastPoll(False) self.setFastPoll(False)
return pos return pos
def read_encoder(self): def read_encoder(self):
if self.encoder_mode == 'NO': if self.encoder_mode == 'NO':
return self.value return self.value
return float(self.get('P22R')) * self.sign - self.offset return float(self.get('P22R')) * self.sign
def write_sign(self, value): def write_sign(self, value):
self.sign = value self.sign = value
@ -187,68 +199,56 @@ class Motor(PersistentMixin, HasIO, Drivable):
self.get_step_size() self.get_step_size()
return float(self.set_get('P15S', round(value / self._step_size), 'P15R')) * self._step_size return float(self.set_get('P15S', round(value / self._step_size), 'P15R')) * self._step_size
def _check_limits(self, *values): def check_target(self, value):
for name, (mn, mx) in ('user', self._rawlimits), ('abs', self.abslimits): self.checkLimits(value)
mn -= self.offset self.checkLimits(value + self.backlash)
mx -= self.offset
for v in values:
if not mn <= v <= mx:
raise BadValueError('%s limits violation: %g <= %g <= %g' % (name, mn, v, mx))
v += self.offset
def write_target(self, value): def write_target(self, value):
if self.status[0] == self.Status.ERROR: self.read_alive_time()
if self._reset_needed:
self.status = ERROR, 'reset needed after power up (probably position lost)'
raise HardwareError(self.status[1])
if self.status[0] == ERROR:
raise HardwareError('need reset') raise HardwareError('need reset')
self.status = self.Status.BUSY, 'changed target' self.status = BUSY, 'changed target'
self._check_limits(value, value + self.backlash) self.saveParameters()
if self.backlash: if self.backlash:
# drive first to target + backlash # drive first to target + backlash
# we do not optimize when already driving from the right side # we do not optimize when already driving from the right side
self._backlash_pending = True self._backlash_pending = True
self.set('A', self.sign * (value + self.offset + self.backlash)) self.set('A', self.sign * (value + self.backlash))
else: else:
self.set('A', self.sign * (value + self.offset)) self.set('A', self.sign * value)
self.setFastPoll(True, self.fast_poll) self.setFastPoll(True, self.fast_poll)
return value return value
def read_userlimits(self):
return self._rawlimits[0] - self.offset, self._rawlimits[1] - self.offset
def write_userlimits(self, value):
self._rawlimits = [clamp(self.abslimits[0], v + self.offset, self.abslimits[1]) for v in value]
value = self.read_userlimits()
self.saveParameters()
return value
def write_offset(self, value):
self.offset = value
self.read_userlimits()
self.saveParameters()
return Done
def stop(self): def stop(self):
self.get('S') self.get('S')
@Command @Command
def reset(self): def reset_error(self):
"""Reset error, set position to encoder""" """Reset error, set position to encoder"""
self.read_value() self.read_value()
if self.status[0] == self.Status.ERROR: if self.status[0] == ERROR or self._reset_needed:
enc = self.encoder + self.offset newenc = enc = self.encoder
pos = self.value + self.offset pos = self.value
if abs(enc - pos) > self.encoder_tolerance: if abs(enc - pos) > self.encoder_tolerance or self.encoder_mode == 'NO':
if enc < 0: if self.circumference:
# assume we have a rotation (not a linear motor) # bring encoder value either within or as close as possible to the given range
while enc < 0: if enc < self.target_min:
self.offset += 360 mid = self.target_min + 0.5 * min(self.target_max - self.target_min, self.circumference)
enc += 360 elif enc > self.target_max:
self.set('P22S', enc * self.sign) mid = self.target_max - 0.5 * min(self.target_max - self.target_min, self.circumference)
self.saveParameters() else:
self.set('P20S', enc * self.sign) # set pos to encoder mid = enc
newenc += round((mid - enc) / self.circumference) * self.circumference
if newenc != enc:
self.set('P22S', newenc * self.sign)
if newenc != pos:
self.set('P20S', newenc * self.sign) # set pos to encoder
self.read_value() self.read_value()
# self.status = self.Status.IDLE, '' self._reset_needed = False
# TODO: # TODO:
# '=E' electronics status # '=E' electronics status
# '=I+' / '=I-': limit switches # '=I+' / '=I-': limit switches
# use P37 to determine if restarted