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

fix simulation

Browse Source 
+ some fixed in sim_uniax

Change-Id: Ia8703ed988aa904bb2694339f0d3175b28fcb33e
This commit is contained in:
zolliker 2023-09-19 16:05:52 +02:00
parent 72fbed289d
commit d2e7ce7242
4 changed files with 41 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cfg/sim_uniax_cfg.py
View File

@ -46,7 +46,7 @@ Mod('res',
), ),
value=Param( value=Param(
default=99.0, default=99.0,
datatype={'type': 'double', 'unit': 'Ohm'}, unit='Ohm',
), ),
) )

9
frappy/simulation.py
View File

@ -32,12 +32,13 @@ from frappy.modules import Drivable, Module, Parameter, Readable, Writable, Comm
class SimBase: class SimBase:
def __new__(cls, devname, logger, cfgdict, dispatcher): def __new__(cls, devname, logger, cfgdict, dispatcher):
extra_params = cfgdict.pop('extra_params', '') or cfgdict.pop('.extra_params', '') extra_params = cfgdict.pop('extra_params', '')['value']
if isinstance(extra_params, str):
extra_params = [v.strip() for v in extra_params.split(',')]
attrs = {} attrs = {}
if extra_params: if extra_params:
for k in extra_params['value'].split(','): for k in extra_params:
k = k.strip() attrs[k] = Parameter(f'extra_param: {k}',
attrs[k] = Parameter(f'extra_param: {k.strip()}',
datatype=FloatRange(), datatype=FloatRange(),
default=0.0) default=0.0)

2
frappy_psi/simdpm.py
View File

@ -38,7 +38,7 @@ class DPM3(Readable):
_pos = 0 _pos = 0
def read_value(self): def read_value(self):
mot = self._motor mot = self.motor
d = self.friction * self.slope d = self.friction * self.slope
self._pos = clamp(self._pos, mot.value - d, mot.value + d) self._pos = clamp(self._pos, mot.value - d, mot.value + d)
f = (mot.value - self._pos) / self.slope f = (mot.value - self._pos) / self.slope

63
frappy_psi/uniax.py
View File

@ -26,9 +26,9 @@ import math
from frappy.core import Drivable, Parameter, FloatRange, Done, \ from frappy.core import Drivable, Parameter, FloatRange, Done, \
Attached, Command, PersistentMixin, PersistentParam, BoolType Attached, Command, PersistentMixin, PersistentParam, BoolType
from frappy.errors import BadValueError, SECoPError from frappy.errors import BadValueError, SECoPError
from frappy.lib.statemachine import Retry, StateMachine, Restart from frappy.lib.statemachine import Retry, StateMachine, Stop
# TODO: fix with new state machine! # TODO: to be tested, or better migrated to new state machine!
class Error(SECoPError): class Error(SECoPError):
@ -46,11 +46,11 @@ class Uniax(PersistentMixin, Drivable):
pid_i = PersistentParam('integral', FloatRange(), readonly=False, default=0.5, persistent='auto') pid_i = PersistentParam('integral', FloatRange(), readonly=False, default=0.5, persistent='auto')
filter_interval = Parameter('filter time', FloatRange(0, 60, unit='s'), readonly=False, default=5) filter_interval = Parameter('filter time', FloatRange(0, 60, unit='s'), readonly=False, default=5)
current_step = Parameter('', FloatRange(unit='deg'), default=0) current_step = Parameter('', FloatRange(unit='deg'), default=0)
force_offset = PersistentParam('transducer offset', FloatRange(unit='N'), readonly=False, default=0, force_offset = PersistentParam('transducer offset', FloatRange(unit='N'), readonly=False, value=0,
initwrite=True, persistent='auto') persistent='auto')
hysteresis = PersistentParam('force hysteresis', FloatRange(0, 190, unit='N'), readonly=False, default=5, hysteresis = PersistentParam('force hysteresis', FloatRange(0, 190, unit='N'), readonly=False, default=5,
persistent='auto') persistent='auto')
adjusting = Parameter('', BoolType(), readonly=False, default=False, initwrite=True) adjusting = Parameter('', BoolType(), readonly=False, value=False)
adjusting_current = PersistentParam('current when adjusting force', FloatRange(0, 2.8, unit='A'), readonly=False, adjusting_current = PersistentParam('current when adjusting force', FloatRange(0, 2.8, unit='A'), readonly=False,
default=0.5, persistent='auto') default=0.5, persistent='auto')
safe_step = PersistentParam('max. motor step when adjusting force', FloatRange(0, unit='deg'), readonly=False, safe_step = PersistentParam('max. motor step when adjusting force', FloatRange(0, unit='deg'), readonly=False,
@ -116,11 +116,15 @@ class Uniax(PersistentMixin, Drivable):
def motor_busy(self): def motor_busy(self):
mot = self.motor mot = self.motor
if mot.isBusy(): if mot.isBusy():
if mot.target != self._mot_target: if self._mot_target is not None and mot.target != self._mot_target:
raise Error('control stopped - motor moved directly') raise Error('control stopped - motor moved directly')
return True return True
return False return False
def motor_stop(self):
self.motor.stop()
self._mot_target = None
def read_value(self): def read_value(self):
try: try:
self._force = force = self.transducer.read_value() self._force = force = self.transducer.read_value()
@ -142,7 +146,7 @@ class Uniax(PersistentMixin, Drivable):
force = self._sum / self._cnt force = self._sum / self._cnt
self.reset_filter(now) self.reset_filter(now)
if abs(force) > self.limit + self.hysteresis: if abs(force) > self.limit + self.hysteresis:
self.motor.stop() self.motor_stop()
self.status = 'ERROR', 'above max limit' self.status = 'ERROR', 'above max limit'
self.log.error(self.status[1]) self.log.error(self.status[1])
self.read_target() self.read_target()
@ -188,8 +192,8 @@ class Uniax(PersistentMixin, Drivable):
def cleanup(self, state): def cleanup(self, state):
"""in case of error, set error status""" """in case of error, set error status"""
if state.stopped: # stop or restart if state.next_task: # stop or restart
if state.stopped is Restart: if not isinstance(state.next_task, Stop):
return return
self.status = 'IDLE', 'stopped' self.status = 'IDLE', 'stopped'
self.log.warning('stopped') self.log.warning('stopped')
@ -200,7 +204,7 @@ class Uniax(PersistentMixin, Drivable):
else: else:
self.log.error('%r raised in state %r', str(state.last_error), state.status_string) self.log.error('%r raised in state %r', str(state.last_error), state.status_string)
self.read_target() # make target invalid self.read_target() # make target invalid
self.motor.stop() self.motor_stop()
self.write_adjusting(False) self.write_adjusting(False)
def reset_progress(self, state): def reset_progress(self, state):
@ -234,7 +238,7 @@ class Uniax(PersistentMixin, Drivable):
state.direction = math.copysign(1, self.target - self.value) state.direction = math.copysign(1, self.target - self.value)
state.pid_fact = 1 state.pid_fact = 1
if self.motor_busy(): if self.motor_busy():
return Retry() return Retry
self.value = self._force self.value = self._force
force_step = self.target - self.value force_step = self.target - self.value
if abs(force_step) < self.tolerance: if abs(force_step) < self.tolerance:
@ -255,6 +259,7 @@ class Uniax(PersistentMixin, Drivable):
self.status = 'BUSY', 'adjusting force' self.status = 'BUSY', 'adjusting force'
elif not self.check_progress(state): elif not self.check_progress(state):
if abs(self.value) < self.hysteresis: if abs(self.value) < self.hysteresis:
motor_dif = abs(self.motor.value - state.prev_pos)
if motor_dif > self.motor_play: if motor_dif > self.motor_play:
self.log.warning('adjusting failed - try to find zero pos') self.log.warning('adjusting failed - try to find zero pos')
self.set_zero_pos(self.target, None) self.set_zero_pos(self.target, None)
@ -265,15 +270,15 @@ class Uniax(PersistentMixin, Drivable):
self.log.warning('no substantial progress since %d sec', self.timeout) self.log.warning('no substantial progress since %d sec', self.timeout)
self.status = 'IDLE', 'adjusting timeout' self.status = 'IDLE', 'adjusting timeout'
self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * state.pid_fact) self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * state.pid_fact)
return Retry() return Retry
def within_tolerance(self, state): def within_tolerance(self, state):
"""within tolerance""" """within tolerance"""
if state.init: if state.init:
self.status = 'IDLE', 'within tolerance' self.status = 'IDLE', 'within tolerance'
return Retry() return Retry
if self.motor_busy(): if self.motor_busy():
return Retry() return Retry
force_step = self.target - self.value force_step = self.target - self.value
if abs(force_step) < self.tolerance * 0.5: if abs(force_step) < self.tolerance * 0.5:
self.current_step = 0 self.current_step = 0
@ -282,16 +287,16 @@ class Uniax(PersistentMixin, Drivable):
self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * 0.1) self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * 0.1)
if abs(force_step) > self.tolerance: if abs(force_step) > self.tolerance:
return self.out_of_tolerance return self.out_of_tolerance
return Retry() return Retry
def out_of_tolerance(self, state): def out_of_tolerance(self, state):
"""out of tolerance""" """out of tolerance"""
if state.init: if state.init:
self.status = 'WARN', 'out of tolerance' self.status = 'WARN', 'out of tolerance'
state.in_since = 0 state.in_since = 0
return Retry() return Retry
if self.motor_busy(): if self.motor_busy():
return Retry() return Retry
force_step = self.target - self._force force_step = self.target - self._force
if abs(force_step) < self.tolerance: if abs(force_step) < self.tolerance:
if state.in_since == 0: if state.in_since == 0:
@ -299,10 +304,10 @@ class Uniax(PersistentMixin, Drivable):
if state.now > state.in_since + 10: if state.now > state.in_since + 10:
return self.within_tolerance return self.within_tolerance
if abs(force_step) < self.tolerance * 0.5: if abs(force_step) < self.tolerance * 0.5:
return Retry() return Retry
self.check_progress(state) self.check_progress(state)
self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * 0.1) self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * 0.1)
return Retry() return Retry
def find(self, state): def find(self, state):
"""find active (engaged) range""" """find active (engaged) range"""
@ -315,7 +320,7 @@ class Uniax(PersistentMixin, Drivable):
if abs_force > self.hysteresis or abs_force > self.target * direction: if abs_force > self.hysteresis or abs_force > self.target * direction:
if self.motor_busy(): if self.motor_busy():
self.log.info('motor stopped - substantial force detected: %g', self.value) self.log.info('motor stopped - substantial force detected: %g', self.value)
self.motor.stop() self.motor_stop()
elif state.prev_direction == 0: elif state.prev_direction == 0:
return self.adjust return self.adjust
if abs_force > self.hysteresis: if abs_force > self.hysteresis:
@ -326,9 +331,9 @@ class Uniax(PersistentMixin, Drivable):
return self.free return self.free
if self.motor_busy(): if self.motor_busy():
if direction == -state.prev_direction: # target direction changed if direction == -state.prev_direction: # target direction changed
self.motor.stop() self.motor_stop()
state.init_find = True # restart find state.init_find = True # restart find
return Retry() return Retry
zero_pos = self.zero_pos(self.target) zero_pos = self.zero_pos(self.target)
if state.prev_direction: # find already started if state.prev_direction: # find already started
if abs(self.motor.target - self.motor.value) > self.motor.tolerance: if abs(self.motor.target - self.motor.value) > self.motor.tolerance:
@ -336,7 +341,7 @@ class Uniax(PersistentMixin, Drivable):
self.write_adjusting(True) self.write_adjusting(True)
self.log.info('one step to %g', self.motor.value + self.safe_step) self.log.info('one step to %g', self.motor.value + self.safe_step)
self.drive_relative(direction * self.safe_step) self.drive_relative(direction * self.safe_step)
return Retry() return Retry
else: else:
state.prev_direction = math.copysign(1, self.target) state.prev_direction = math.copysign(1, self.target)
side_name = 'negative' if direction == -1 else 'positive' side_name = 'negative' if direction == -1 else 'positive'
@ -347,14 +352,14 @@ class Uniax(PersistentMixin, Drivable):
self.write_adjusting(False) self.write_adjusting(False)
self.log.info('change side to %g', zero_pos) self.log.info('change side to %g', zero_pos)
self.drive_relative(zero_pos - self.motor.value) self.drive_relative(zero_pos - self.motor.value)
return Retry() return Retry
# we are already at or beyond zero_pos # we are already at or beyond zero_pos
return self.adjust return self.adjust
self.write_adjusting(False) self.write_adjusting(False)
self.status = 'BUSY', 'find %s side' % side_name self.status = 'BUSY', 'find %s side' % side_name
self.log.info('one turn to %g', self.motor.value + direction * 360) self.log.info('one turn to %g', self.motor.value + direction * 360)
self.drive_relative(direction * 360) self.drive_relative(direction * 360)
return Retry() return Retry
def free(self, state): def free(self, state):
"""free from high force at other end""" """free from high force at other end"""
@ -362,7 +367,7 @@ class Uniax(PersistentMixin, Drivable):
state.free_way = None state.free_way = None
self.reset_progress(state) self.reset_progress(state)
if self.motor_busy(): if self.motor_busy():
return Retry() return Retry
self.value = self._force self.value = self._force
if abs(self.value) > abs(state.force_before_free) + self.hysteresis: if abs(self.value) > abs(state.force_before_free) + self.hysteresis:
raise Error('force increase while freeing') raise Error('force increase while freeing')
@ -377,7 +382,7 @@ class Uniax(PersistentMixin, Drivable):
raise Error('freeing failed') raise Error('freeing failed')
state.free_way += self.safe_step state.free_way += self.safe_step
self.drive_relative(direction * self.safe_step) self.drive_relative(direction * self.safe_step)
return Retry() return Retry
def write_target(self, target): def write_target(self, target):
if abs(target) > self.limit: if abs(target) > self.limit:
@ -399,7 +404,7 @@ class Uniax(PersistentMixin, Drivable):
return Done return Done
def read_target(self): def read_target(self):
if self._state.state is None: if self._state.statefunc is None:
if self.status[1]: if self.status[1]:
raise Error(self.status[1]) raise Error(self.status[1])
raise Error('inactive') raise Error('inactive')
@ -409,7 +414,7 @@ class Uniax(PersistentMixin, Drivable):
def stop(self): def stop(self):
if self.motor.isBusy(): if self.motor.isBusy():
self.log.info('stop motor') self.log.info('stop motor')
self.motor.stop() self.motor_stop()
self.status = 'IDLE', 'stopped' self.status = 'IDLE', 'stopped'
self._state.stop() self._state.stop()