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

phytron with HasStates

Browse Source 
test version

Change-Id: Ifbaa35421a64faad32c87bbcf758c236f900cd0b
This commit is contained in:
zolliker 2023-05-15 08:33:14 +02:00
parent fa9525fd4c
commit 84d0d44f38
1 changed files with 86 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

143
frappy_psi/phytron.py
View File

@ -28,6 +28,7 @@ from frappy.core import Done, Command, EnumType, FloatRange, IntRange, \
StringIO, StringType, IDLE, BUSY, ERROR, Limit
from frappy.errors import CommunicationFailedError, HardwareError
from frappy.features import HasOffset
from frappy.states import HasStates, status_code, Retry
class PhytronIO(StringIO):
@ -54,7 +55,7 @@ class PhytronIO(StringIO):
return reply[1:]
class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
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)
@ -76,14 +77,21 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
target_max = Limit()
alive_time = PersistentParam('alive time for detecting restarts',
FloatRange(), default=0, export=False)
sim_error = Parameter('run time error simulation', StringType(), readonly=False, default='')
ioClass = PhytronIO
fast_poll = 0.1
_backlash_pending = False
_mismatch_count = 0
_step_size = None # degree / step
_reset_needed = False
STATUS_MAP = {
'08': (IDLE, ''),
'01': (ERROR, 'power stage failure'),
'02': (ERROR, 'power too low'),
'04': (ERROR, 'power stage over temperature'),
'07': (ERROR, 'no power stage'),
'80': (ERROR, 'encoder failure'),
}
def get(self, cmd):
return self.communicate(f'{self.address:x}{self.axis}{cmd}')
@ -107,7 +115,7 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
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
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
@ -119,53 +127,7 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
return now
def read_value(self):
prev_enc = self.encoder
pos = float(self.get('P20R')) * self.sign
if self.encoder_mode != 'NO':
enc = self.read_encoder()
else:
enc = pos
status = self.communicate(f'{self.address:x}SE')
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': # 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 = BUSY, 'driving'
else:
self.log.error('encoder lag: %g not within %g..%g',
pos, e1, e2)
self.get('S') # stop
self.status = ERROR, 'encoder lag error'
self.value = pos
self.saveParameters()
self.setFastPoll(False)
else:
self.status = BUSY, 'driving'
else:
if self._backlash_pending:
# drive to real target
self.set('A', self.sign * self.target)
self._backlash_pending = False
return pos
if (self.encoder_mode == 'CHECK' and
abs(enc - pos) > self.encoder_tolerance):
if self._mismatch_count < 2:
self._mismatch_count += 1
else:
self.log.error('encoder mismatch: abs(%g - %g) < %g',
enc, pos, self.encoder_tolerance)
self.status = ERROR, 'encoder does not match pos'
else:
self._mismatch_count = 0
self.status = IDLE, ''
self.value = pos
self.read_encoder() # let encoder quickly match value
self.saveParameters()
self.setFastPoll(False)
return pos
return float(self.get('P20R')) * self.sign
def read_encoder(self):
if self.encoder_mode == 'NO':
@ -210,18 +172,82 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
raise HardwareError(self.status[1])
if self.status[0] == ERROR:
raise HardwareError('need reset')
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.backlash))
self.start_machine(self.predriving)
else:
self.set('A', self.sign * value)
self.setFastPoll(True, self.fast_poll)
self.start_machine(self.driving)
return value
def read_status(self):
if self.sim_error:
sysstatus = self.sim_error
else:
sysstatus = self.communicate(f'{self.address:x}SE')
sysstatus = sysstatus[2:4] if self.axis == 'X' else sysstatus[6:8]
status = self.STATUS_MAP.get(sysstatus) or (ERROR, f'unknown error {sysstatus}')
if status[0] == ERROR:
return status
return super().read_status() # status from state machine
def check_moving(self):
prev_enc = self.encoder
if self.encoder_mode != 'NO':
enc = self.read_encoder()
else:
enc = self.value
status = self.get('=H')
if status != 'N': # at target
return False
if self.encoder_mode != 'CHECK':
return True
e1, e2 = sorted((prev_enc, enc))
if e1 - self.encoder_tolerance <= self.value <= e2 + self.encoder_tolerance:
return True
self.log.error('encoder lag: %g not within %g..%g',
self.value, e1, e2)
self.get('S') # stop
self.saveParameters()
raise HardwareError('encoder lag error')
@status_code(BUSY)
def driving(self, sm):
if self.check_moving():
return Retry
if self.backlash:
# drive to real target
self.set('A', self.sign * self.target)
return self.driving_to_final
return self.finishing
@status_code(BUSY)
def driving_to_final(self, sm):
if self.check_moving():
return Retry
return self.finishing
@status_code(BUSY)
def finishing(self, sm):
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):
if sm.mismatch_count > 2:
self.log.error('encoder mismatch: abs(%g - %g) < %g',
enc, pos, self.encoder_tolerance)
raise HardwareError('encoder does not match pos')
sm.mismatch_count += 1
return Retry
self.saveParameters()
return self.final_status(IDLE, '')
def stop(self):
self.get('S')
@ -230,7 +256,7 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
"""Reset error, set position to encoder"""
self.read_value()
if self.status[0] == ERROR or self._reset_needed:
newenc = enc = self.encoder
newenc = enc = self.read_encoder()
pos = self.value
if abs(enc - pos) > self.encoder_tolerance or self.encoder_mode == 'NO':
if self.circumference:
@ -242,11 +268,14 @@ class Motor(HasOffset, PersistentMixin, HasIO, Drivable):
else:
mid = enc
newenc += round((mid - enc) / self.circumference) * self.circumference
if newenc != enc:
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.status = 'IDLE', 'after error reset'
self._reset_needed = False
# TODO: