[WIP] phytron improvements
- Limits - offset - power cycle behaviour Change-Id: Id2f717c362cd7e1e37f180c8130b0e086e724389
This commit is contained in:
parent
3fcd72b189
commit
3314241631
@ -22,11 +22,12 @@
|
||||
|
||||
"""driver for phytron motors"""
|
||||
|
||||
import time
|
||||
from frappy.core import Done, Command, EnumType, FloatRange, IntRange, \
|
||||
HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, \
|
||||
StringIO, StringType, TupleOf
|
||||
from frappy.errors import CommunicationFailedError, HardwareError, BadValueError
|
||||
from frappy.lib import clamp
|
||||
StringIO, StringType, IDLE, BUSY, ERROR, Limit
|
||||
from frappy.errors import CommunicationFailedError, HardwareError
|
||||
from frappy.features import HasOffset
|
||||
|
||||
|
||||
class PhytronIO(StringIO):
|
||||
@ -54,43 +55,35 @@ class PhytronIO(StringIO):
|
||||
return reply[1:]
|
||||
|
||||
|
||||
class Motor(PersistentMixin, HasIO, Drivable):
|
||||
class Motor(HasOffset, 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)
|
||||
|
||||
encoder_mode = Parameter('how to treat the encoder', EnumType('encoder', NO=0, READ=1, CHECK=2),
|
||||
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)
|
||||
speed = Parameter('', FloatRange(0, 20, unit='deg/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)
|
||||
offset = PersistentParam('', FloatRange(unit='deg'), readonly=False, default=0)
|
||||
sign = PersistentParam('', IntRange(-1,1), readonly=False, default=1)
|
||||
encoder = Parameter('encoder reading', FloatRange(unit='deg'))
|
||||
backlash = Parameter("""backlash compensation\n
|
||||
offset for always approaching from the same side""",
|
||||
FloatRange(unit='deg'), readonly=False, default=0)
|
||||
abslimits = Parameter('abs limits (raw values)', default=(0, 0),
|
||||
datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg')))
|
||||
userlimits = PersistentParam('user limits', readonly=False, default=(0, 0), initwrite=True,
|
||||
datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg')))
|
||||
target_min = Limit()
|
||||
target_max = Limit()
|
||||
alive_time = PersistentParam('alive time for detecting restarts',
|
||||
FloatRange(), default=0) # export=False
|
||||
|
||||
ioClass = PhytronIO
|
||||
fast_poll = 0.1
|
||||
_backlash_pending = False
|
||||
_mismatch_count = 0
|
||||
_rawlimits = None
|
||||
_step_size = None # degree / step
|
||||
|
||||
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()
|
||||
_reset_needed = False
|
||||
|
||||
def get(self, 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)
|
||||
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):
|
||||
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':
|
||||
enc = self.read_encoder()
|
||||
else:
|
||||
@ -122,23 +130,25 @@ class Motor(PersistentMixin, HasIO, Drivable):
|
||||
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':
|
||||
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 = self.Status.BUSY, 'driving'
|
||||
self.status = BUSY, 'driving'
|
||||
else:
|
||||
self.log.error('encoder lag: %g not within %g..%g',
|
||||
pos, e1, e2)
|
||||
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)
|
||||
else:
|
||||
self.status = self.Status.BUSY, 'driving'
|
||||
self.status = BUSY, 'driving'
|
||||
else:
|
||||
if self._backlash_pending:
|
||||
# drive to real target
|
||||
self.set('A', self.sign * (self.target + self.offset))
|
||||
self.set('A', self.sign * self.target)
|
||||
self._backlash_pending = False
|
||||
return pos
|
||||
if (self.encoder_mode == 'CHECK' and
|
||||
@ -148,17 +158,19 @@ class Motor(PersistentMixin, HasIO, Drivable):
|
||||
else:
|
||||
self.log.error('encoder mismatch: abs(%g - %g) < %g',
|
||||
enc, pos, self.encoder_tolerance)
|
||||
self.status = self.Status.ERROR, 'encoder does not match pos'
|
||||
self.status = ERROR, 'encoder does not match pos'
|
||||
else:
|
||||
self._mismatch_count = 0
|
||||
self.status = self.Status.IDLE, ''
|
||||
self.status = IDLE, ''
|
||||
self.value = pos
|
||||
self.saveParameters()
|
||||
self.setFastPoll(False)
|
||||
return pos
|
||||
|
||||
def read_encoder(self):
|
||||
if self.encoder_mode == 'NO':
|
||||
return self.value
|
||||
return float(self.get('P22R')) * self.sign - self.offset
|
||||
return float(self.get('P22R')) * self.sign
|
||||
|
||||
def write_sign(self, value):
|
||||
self.sign = value
|
||||
@ -187,68 +199,56 @@ class Motor(PersistentMixin, HasIO, Drivable):
|
||||
self.get_step_size()
|
||||
return float(self.set_get('P15S', round(value / self._step_size), 'P15R')) * self._step_size
|
||||
|
||||
def _check_limits(self, *values):
|
||||
for name, (mn, mx) in ('user', self._rawlimits), ('abs', self.abslimits):
|
||||
mn -= self.offset
|
||||
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 check_target(self, value):
|
||||
self.checkLimits(value)
|
||||
self.checkLimits(value + self.backlash)
|
||||
|
||||
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')
|
||||
self.status = self.Status.BUSY, 'changed target'
|
||||
self._check_limits(value, value + self.backlash)
|
||||
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.offset + self.backlash))
|
||||
self.set('A', self.sign * (value + self.backlash))
|
||||
else:
|
||||
self.set('A', self.sign * (value + self.offset))
|
||||
self.set('A', self.sign * value)
|
||||
self.setFastPoll(True, self.fast_poll)
|
||||
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):
|
||||
self.get('S')
|
||||
|
||||
@Command
|
||||
def reset(self):
|
||||
def reset_error(self):
|
||||
"""Reset error, set position to encoder"""
|
||||
self.read_value()
|
||||
if self.status[0] == self.Status.ERROR:
|
||||
enc = self.encoder + self.offset
|
||||
pos = self.value + self.offset
|
||||
if abs(enc - pos) > self.encoder_tolerance:
|
||||
if enc < 0:
|
||||
# assume we have a rotation (not a linear motor)
|
||||
while enc < 0:
|
||||
self.offset += 360
|
||||
enc += 360
|
||||
self.set('P22S', enc * self.sign)
|
||||
self.saveParameters()
|
||||
self.set('P20S', enc * self.sign) # set pos to encoder
|
||||
if self.status[0] == ERROR or self._reset_needed:
|
||||
newenc = enc = self.encoder
|
||||
pos = self.value
|
||||
if abs(enc - pos) > self.encoder_tolerance or self.encoder_mode == 'NO':
|
||||
if self.circumference:
|
||||
# bring encoder value either within or as close as possible to the given range
|
||||
if enc < self.target_min:
|
||||
mid = self.target_min + 0.5 * min(self.target_max - self.target_min, self.circumference)
|
||||
elif enc > self.target_max:
|
||||
mid = self.target_max - 0.5 * min(self.target_max - self.target_min, self.circumference)
|
||||
else:
|
||||
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.status = self.Status.IDLE, ''
|
||||
self._reset_needed = False
|
||||
|
||||
# TODO:
|
||||
# '=E' electronics status
|
||||
# '=I+' / '=I-': limit switches
|
||||
# use P37 to determine if restarted
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user