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

frappy_psi.phytron: implement absolute encoder properly

Browse Source 
- issues with power down detection
This commit is contained in:
zolliker
2026-05-21 10:00:50 +02:00
parent e5a78b84a7
commit 95cf793178
1 changed files with 93 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
frappy_psi/phytron.py
+93 -63
View File
@@ -22,9 +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, IDLE, BUSY, ERROR, WARN, Limit, BoolType
from frappy.core import Done, Command, HasIO, Parameter, Property, Drivable, \
Limit, PersistentMixin, PersistentParam, StringIO, IDLE, BUSY, ERROR, WARN, \
nopoll
from frappy.datatypes import EnumType, FloatRange, IntRange, BoolType, \
NoneOr, StringType
from frappy.errors import CommunicationFailedError, HardwareError, ImpossibleError
from frappy.features import HasOffset
from frappy.states import HasStates, status_code, Retry
@@ -62,14 +65,13 @@ class PhytronIO(StringIO):
class Motor(HasOffset, HasStates, 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)
circumference = Property('circumference for rotations or zero for linear', NoneOr(FloatRange(0)), value=None)
encoder_mode = Parameter('how to treat the encoder', EnumType('encoder', NO=0, READ=1, CHECK=2),
default=1, readonly=False)
encoder_type = Property('encoder type', EnumType('encoder_type', relative=0, absolute=1), default=0)
check_limit_switches = Parameter('whether limit switches are checked',BoolType(),
default=0, readonly=False)
value = PersistentParam('angle', FloatRange(unit='deg'))
status = PersistentParam()
value = Parameter('position', FloatRange(unit='deg'))
target = Parameter('target angle', FloatRange(unit='$'), readonly=False)
speed = Parameter('', FloatRange(0, 20, unit='$/s'), readonly=False)
accel = Parameter('', FloatRange(2, 250, unit='$/s/s'), readonly=False)
@@ -91,12 +93,23 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable):
_prev_diff = 0 # for checking progress
_intermediate_target = 0
_stopped_at = 0
_pos = _enc = _prev_enc = 0 # internal pos/encoder values, previous encoder
_ignore_mask = 0
_limits_touched = 0
_limits_check_mask = LIMITS_MASK
status_bits = ['power stage error', 'undervoltage', 'overtemperature', 'active',
'lower switch active', 'upper switch active', 'step failure', 'encoder error']
def checkProperties(self):
super().checkProperties()
if self.propertyValues['circumference'] is None:
if self.parameters['value'].datatype.unit == 'deg':
self.propertyValues['circumference'] = 360
self.log.info('rotational axis')
else:
self.propertyValues['circumference'] = 0
self.log.info('linear axis')
def get(self, cmd):
return self.communicate(f'{self.address:x}{self.axis}{cmd}')
@@ -118,30 +131,46 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable):
def initialReads(self):
super().initialReads()
self.clear_errors()
self.reset()
def read_alive_time(self):
def _check_power_cycle(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
power_cycled = False
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 clear_errors
self._blocking_error = 'lost position'
if self.encoder_mode == 'NO' or self.encoder_type == 'relative':
self._blocking_error = 'power cycle detected - lost position'
else:
self.log.info('fix after power cycle')
power_cycled = True
else: # do silently
self.clear_errors()
self.log.warn('first time running -> forget offset')
self.offset = 0
power_cycled = True
self.alive_time = now
self.saveParameters()
return now
return power_cycled
def read_alive_time(self):
if self._check_power_cycle():
self.reset()
return self.alive_time
def _get_pos_enc(self):
# get position and encoder position
self._pos = float(self.get('P20R')) * self.sign
self._prev_enc = self._enc
self._enc = self._pos if self.encoder_mode == 'NO' else float(self.get('P22R')) * self.sign
def read_value(self):
return float(self.get('P20R')) * self.sign
self.read_encoder()
return self._pos
def read_encoder(self):
if self.encoder_mode == 'NO':
return self.value
return float(self.get('P22R')) * self.sign
return self._enc
def write_sign(self, value):
self.sign = value
@@ -199,6 +228,9 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable):
self.get('S')
def doPoll(self):
self._get_pos_enc()
if self._pos == 0 and self._check_power_cycle():
self.reset()
super().doPoll()
if self._running and not self.isBusy():
if time.time() > self._stopped_at + 5:
@@ -244,12 +276,10 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable):
- in case motor is moving properly return Retry
- in case motor is moving in the wrong direction, return self.stopping_for_restart
- in case encoder does not follow steps, raise a blocking error
assumes that read_value() was called before in order to update value and encoder
this is the case when called at a step in a state machine
"""
prev_enc = self.encoder
if self.encoder_mode != 'NO':
enc = self.read_encoder()
else:
enc = self.value
if not self._running: # at target (self._running is updated in self.read_status())
return False
diff = abs(self.value - self._intermediate_target)
@@ -263,11 +293,11 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable):
self._prev_diff = diff
if self.encoder_mode != 'CHECK':
return Retry
tol = self.encoder_tolerance + abs(enc - prev_enc)
if abs(self.value - enc) <= tol:
tol = self.encoder_tolerance + abs(self._enc - self._prev_enc)
if abs(self._pos - self._enc) <= tol:
return Retry
self.log.error('encoder lag: steps %g deviate by more than %g from encoder %g',
self.value, tol, enc)
self._pos, tol, self._enc)
self.hw_stop()
self.saveParameters()
self._blocking_error = 'encoder lag error'
@@ -323,13 +353,11 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable):
if sm.init:
sm.mismatch_count = 0
# finish
pos = self.read_value()
enc = self.read_encoder()
if (self.encoder_mode == 'CHECK' and
abs(enc - pos) > self.encoder_tolerance):
abs(self.encoder - self.value) > self.encoder_tolerance):
if sm.mismatch_count > 2:
self.log.error('encoder mismatch: abs(%g - %g) < %g',
enc, pos, self.encoder_tolerance)
self.log.error('encoder mismatch: abs(%g - %g) == %g > %g',
self.encoder, self.value, abs(self.encoder - self.value), self.encoder_tolerance)
self._blocking_error = 'encoder does not match pos'
raise HardwareError(self._blocking_error)
sm.mismatch_count += 1
@@ -349,41 +377,43 @@ class Motor(HasOffset, HasStates, PersistentMixin, HasIO, Drivable):
self.start_machine(self.stopping, status=(BUSY, 'stopping'))
@Command
def reset(self):
"""the same as clear_errors"""
def clear_errors(self):
"""the same as reset, name is required by SECoP"""
self.clear_errors()
@Command
def clear_errors(self):
def reset(self):
"""Reset error, set position to encoder"""
self._get_pos_enc()
pos = self._pos
newenc = enc = self._enc
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 and self.encoder_mode != 'NO':
self.log.info(f'fix encoder {enc:g} -> {newenc:g}')
self.set('P22S', newenc * self.sign)
if abs(newenc - pos) > self.encoder_tolerance:
self.log.info(f'fix pos {pos} to encoder {newenc}')
self.set('P20S', newenc * self.sign) # set pos to encoder
self._get_pos_enc()
self.read_value()
if self._blocking_error:
newenc = enc = self.read_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 and self.encoder_mode != 'NO':
self.log.info(f'enc {enc} -> {newenc}')
self.set('P22S', newenc * self.sign)
if newenc != pos:
self.log.info(f'pos {pos} -> {newenc}')
self.set('P20S', newenc * self.sign) # set pos to encoder
self.read_value()
self.read_status()
if self._limits_touched == LIMITS_MASK:
raise ValueError('both limit switches pressed')
if self.status[0] == IDLE:
self.status = IDLE, 'after error reset'
else:
self.status = WARN, self.status[1]
self._ignore_mask = self._limits_touched
self._blocking_error = None
self.target = self.value # clear error in target
self.read_encoder()
sm = self._state_machine
if sm.idle_status is None or sm.idle_status[0] == IDLE:
sm.idle_status = IDLE, 'after error reset'
else:
sm.idle_status = WARN, sm.idle_status[1]
self.read_status()
if self._limits_touched == LIMITS_MASK:
raise ValueError('both limit switches pressed')
self._ignore_mask = self._limits_touched
self._blocking_error = None
self.target = self.value # clear error in target (TODO: check if this is needed)