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

Merge branch 'wip' of gitlab.psi.ch-samenv:samenv/frappy into wip

Browse Source
This commit is contained in:
zolliker
2022-10-04 15:13:23 +02:00
parent 498dbd4179 00b0a54f12
commit 971c1dcfee
19 changed files with 607 additions and 357 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
secop_psi/ips_mercury.py
View File

@ -20,11 +20,13 @@
# *****************************************************************************
"""oxford instruments mercury IPS power supply"""
import time
from secop.core import Parameter, EnumType, FloatRange, BoolType
from secop.lib.enum import Enum
from secop.errors import BadValueError
from secop.errors import BadValueError, HardwareError
from secop_psi.magfield import Magfield
from secop_psi.mercury import MercuryChannel, off_on, Mapped
from secop.lib.statemachine import Retry
Action = Enum(hold=0, run_to_set=1, run_to_zero=2, clamped=3)
hold_rtoz_rtos_clmp = Mapped(HOLD=Action.hold, RTOS=Action.run_to_set,
@ -37,6 +39,12 @@ class Field(MercuryChannel, Magfield):
setpoint = Parameter('field setpoint', FloatRange(unit='T'), default=0)
voltage = Parameter('leads voltage', FloatRange(unit='V'), default=0)
atob = Parameter('field to amp', FloatRange(0, unit='A/T'), default=0)
I1 = Parameter('master current', FloatRange(unit='A'), default=0)
I2 = Parameter('slave 2 current', FloatRange(unit='A'), default=0)
I3 = Parameter('slave 3 current', FloatRange(unit='A'), default=0)
V1 = Parameter('master voltage', FloatRange(unit='V'), default=0)
V2 = Parameter('slave 2 voltage', FloatRange(unit='V'), default=0)
V3 = Parameter('slave 3 voltage', FloatRange(unit='V'), default=0)
forced_persistent_field = Parameter(
'manual indication that persistent field is bad', BoolType(), readonly=False, default=False)
@ -46,13 +54,17 @@ class Field(MercuryChannel, Magfield):
slave_currents = None
__init = True
def doPoll(self):
super().doPoll()
self.read_current()
def read_value(self):
self.current = self.query('PSU:SIG:FLD')
pf = self.query('PSU:SIG:PFLD')
if self.__init:
self.__init = False
self.persistent_field = pf
if self.switch_heater != 0 or self._field_mismatch is None:
if self.switch_heater == self.switch_heater.on or self._field_mismatch is None:
self.forced_persistent_field = False
self._field_mismatch = False
return self.current
@ -84,7 +96,13 @@ class Field(MercuryChannel, Magfield):
return self.change('PSU:ACTN', value, hold_rtoz_rtos_clmp)
def read_switch_heater(self):
return self.query('PSU:SIG:SWHT', off_on)
value = self.query('PSU:SIG:SWHT', off_on)
now = time.time()
if value != self.switch_heater:
if now < (self.switch_time[self.switch_heater] or 0) + 10:
# probably switch heater was changed, but IPS reply is not yet updated
return self.switch_heater
return value
def write_switch_heater(self, value):
return self.change('PSU:SIG:SWHT', value, off_on)
@ -104,7 +122,11 @@ class Field(MercuryChannel, Magfield):
current = self.query('PSU:SIG:CURR')
for i in range(self.nslaves + 1):
if i:
self.slave_currents[i].append(self.query('DEV:PSU.M%d:PSU:SIG:CURR' % i))
curri = self.query('DEV:PSU.M%d:PSU:SIG:CURR' % i)
volti = self.query('DEV:PSU.M%d:PSU:SIG:VOLT' % i)
setattr(self, 'I%d' % i, curri)
setattr(self, 'V%d' % i, volti)
self.slave_currents[i].append(curri)
else:
self.slave_currents[i].append(current)
min_i = min(self.slave_currents[i])
@ -128,16 +150,22 @@ class Field(MercuryChannel, Magfield):
try:
self.set_and_go(self.persistent_field)
except (HardwareError, AssertionError):
state.switch_undef = self.switch_on_time or state.now
state.switch_undef = self.switch_time[self.switch_heater.on] or state.now
return self.wait_for_switch
return self.ramp_to_field
def ramp_to_field(self, state):
if self.action != 'run_to_set':
self.status = Status.PREPARING, 'restart ramp to field'
return self.start_ramp_to_field
return super().ramp_to_field(state)
def wait_for_switch(self, state):
if self.now - self.switch_undef < self.wait_switch_on:
if state.now - state.switch_undef < self.wait_switch_on:
return Retry()
self.set_and_go(self.persistent_field)
return self.ramp_to_field
def start_ramp_to_target(self, state):
self.set_and_go(self.target)
return self.ramp_to_target

72
secop_psi/magfield.py
View File

@ -44,6 +44,9 @@ Status = Enum(
FINALIZING=390,
)
OFF = 0
ON = 1
class Magfield(HasLimits, Drivable):
value = Parameter('magnetic field', datatype=FloatRange(unit='T'))
@ -52,7 +55,7 @@ class Magfield(HasLimits, Drivable):
'persistent mode', EnumType(Mode), readonly=False, default=Mode.PERSISTENT)
tolerance = Parameter(
'tolerance', FloatRange(0, unit='$'), readonly=False, default=0.0002)
switch_heater = Parameter('switch heater', EnumType(off=0, on=1),
switch_heater = Parameter('switch heater', EnumType(off=OFF, on=ON),
readonly=False, default=0)
persistent_field = Parameter(
'persistent field', FloatRange(unit='$'), readonly=False)
@ -73,34 +76,23 @@ class Magfield(HasLimits, Drivable):
# ArrayOf(TupleOf(FloatRange(unit='$'), FloatRange(unit='$/min'))), readonly=False)
# TODO: the following parameters should be changed into properties after tests
wait_switch_on = Parameter(
'wait time to ensure switch is on', FloatRange(0, unit='s'), readonly=False, default=61)
'wait time to ensure switch is on', FloatRange(0, unit='s'), readonly=False, default=60)
wait_switch_off = Parameter(
'wait time to ensure switch is off', FloatRange(0, unit='s'), readonly=False, default=61)
'wait time to ensure switch is off', FloatRange(0, unit='s'), readonly=False, default=60)
wait_stable_leads = Parameter(
'wait time to ensure current is stable', FloatRange(0, unit='s'), readonly=False, default=6)
wait_stable_field = Parameter(
'wait time to ensure field is stable', FloatRange(0, unit='s'), readonly=False, default=31)
'wait time to ensure field is stable', FloatRange(0, unit='s'), readonly=False, default=30)
persistent_limit = Parameter(
'above this limit, lead currents are not driven to 0',
FloatRange(0, unit='$'), readonly=False, default=99)
_state = None
__init = True
_last_target = None
switch_on_time = None
switch_off_time = None
switch_time = None, None
def doPoll(self):
if self.__init:
self.__init = False
if self.read_switch_heater() and self.mode == Mode.PERSISTENT:
self.read_value() # check for persistent field mismatch
# switch off heater from previous live or manual intervention
self.write_target(self.persistent_field)
else:
self._last_target = self.persistent_field
else:
self.read_value()
self.read_value()
self._state.cycle()
def checkProperties(self):
@ -117,6 +109,19 @@ class Magfield(HasLimits, Drivable):
self.registerCallbacks(self) # for update_switch_heater
self._state = StateMachine(logger=self.log, threaded=False, cleanup=self.cleanup_state)
def startModule(self, start_events):
start_events.queue(self.startupCheck)
super().startModule(start_events)
def startupCheck(self):
if self.read_switch_heater() and self.mode == Mode.PERSISTENT:
self.read_value() # check for persistent field mismatch
# switch off heater from previous live or manual intervention
self.write_mode(self.mode)
self.write_target(self.persistent_field)
else:
self._last_target = self.persistent_field
def write_target(self, target):
self.check_limits(target)
self.target = target
@ -185,14 +190,11 @@ class Magfield(HasLimits, Drivable):
def update_switch_heater(self, value):
"""is called whenever switch heater was changed"""
if value != 0:
self.switch_off_time = None
if self.switch_on_time is None:
self.switch_on_time = time.time()
else:
self.switch_on_time = None
if self.switch_off_time is None:
self.switch_off_time = time.time()
switch_time = self.switch_time[value]
if switch_time is None:
switch_time = time.time()
self.switch_time = [None, None]
self.switch_time[value] = switch_time
def start_switch_on(self, state):
"""switch heater on"""
@ -213,12 +215,10 @@ class Magfield(HasLimits, Drivable):
abs(self.target - self.persistent_field) <= self.tolerance): # short cut
return self.check_switch_off
self.read_switch_heater()
if self.switch_on_time is None:
if state.now - self.switch_off_time > 10:
self.log.warning('switch turned off manually?')
return self.start_switch_on
return Retry()
if state.now - self.switch_on_time < self.wait_switch_on:
if self.switch_time[ON] is None:
self.log.warning('switch turned off manually?')
return self.start_switch_on
if state.now - self.switch_time[ON] < self.wait_switch_on:
return Retry()
self._last_target = self.target
return self.start_ramp_to_target
@ -279,12 +279,10 @@ class Magfield(HasLimits, Drivable):
return self.start_switch_on
self.persistent_field = self.value
self.read_switch_heater()
if self.switch_off_time is None:
if state.now - self.switch_on_time > 10:
self.log.warning('switch turned on manually?')
return self.start_switch_off
return Retry()
if state.now - self.switch_off_time < self.wait_switch_off:
if self.switch_time[OFF] is None:
self.log.warning('switch turned on manually?')
return self.start_switch_off
if state.now - self.switch_time[OFF] < self.wait_switch_off:
return Retry()
if abs(self.value) > self.persistent_limit:
self.status = Status.IDLE, 'leads current at field, switch off'

2
secop_psi/mercury.py
View File

@ -345,7 +345,7 @@ class HeaterOutput(HasInput, MercuryChannel, Writable):
class TemperatureLoop(TemperatureSensor, Loop, Drivable):
channel_type = 'TEMP'
output_module = Attached(HasInput, mandatory=False)
ramp = Parameter('ramp rate', FloatRange(0, unit='K/min'), readonly=False)
ramp = Parameter('ramp rate', FloatRange(0, unit='$/min'), readonly=False)
enable_ramp = Parameter('enable ramp rate', BoolType(), readonly=False)
setpoint = Parameter('working setpoint (differs from target when ramping)', FloatRange(0, unit='$'))
tolerance = Parameter(default=0.1)

11
secop_psi/phytron.py
View File

@ -35,7 +35,9 @@ class PhytronIO(StringIO):
identification = [('0IVR', 'MCC Minilog .*')]
def communicate(self, command):
for ntry in range(5, 0, -1):
ntry = 5
warn = None
for itry in range(ntry):
try:
_, _, reply = super().communicate('\x02' + command).partition('\x02')
if reply[0] == '\x06': # ACK
@ -43,9 +45,12 @@ class PhytronIO(StringIO):
raise CommunicationFailedError('missing ACK %r (cmd: %r)'
% (reply, command))
except Exception as e:
if ntry == 1:
if itry < ntry - 1:
warn = e
else:
raise
self.log.warning('%s - retry', e)
if warn:
self.log.warning('needed %d retries after %r', itry, warn)
return reply[1:]

12
secop_psi/softcal.py
View File

@ -27,7 +27,8 @@ from os.path import basename, dirname, exists, join
import numpy as np
from scipy.interpolate import splev, splrep # pylint: disable=import-error
from secop.core import Attached, BoolType, Parameter, Readable, StringType, FloatRange
from secop.core import Attached, BoolType, Parameter, Readable, StringType, \
FloatRange, Done
def linear(x):
@ -182,7 +183,6 @@ class Sensor(Readable):
description = 'a calibrated sensor value'
_value_error = None
enablePoll = False
def checkProperties(self):
if 'description' not in self.propertyValues:
@ -196,6 +196,9 @@ class Sensor(Readable):
if self.description == '_':
self.description = '%r calibrated with curve %r' % (self.rawsensor, self.calib)
def doPoll(self):
self.read_status()
def write_calib(self, value):
self._calib = CalCurve(value)
return value
@ -221,3 +224,8 @@ class Sensor(Readable):
def read_value(self):
return self._calib(self.rawsensor.read_value())
def read_status(self):
self.update_status(self.rawsensor.status)
return Done

460
secop_psi/uniax.py
View File

@ -25,7 +25,12 @@ import time
import math
from secop.core import Drivable, Parameter, FloatRange, Done, \
Attached, Command, PersistentMixin, PersistentParam, BoolType
from secop.errors import BadValueError
from secop.errors import BadValueError, SECoPError
from secop.lib.statemachine import Retry, StateMachine, Restart
class Error(SECoPError):
pass
class Uniax(PersistentMixin, Drivable):
@ -33,11 +38,11 @@ class Uniax(PersistentMixin, Drivable):
motor = Attached()
transducer = Attached()
limit = Parameter('abs limit of force', FloatRange(0, 190, unit='N'), readonly=False, default=150)
tolerance = Parameter('force tolerance', FloatRange(0, 10, unit='N'), readonly=False, default=0.1)
tolerance = Parameter('force tolerance', FloatRange(0, 10, unit='N'), readonly=False, default=0.2)
slope = PersistentParam('spring constant', FloatRange(unit='deg/N'), 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=1)
filter_interval = Parameter('filter time', FloatRange(0, 60, unit='s'), readonly=False, default=5)
current_step = Parameter('', FloatRange(unit='deg'), default=0)
force_offset = PersistentParam('transducer offset', FloatRange(unit='N'), readonly=False, default=0,
initwrite=True, persistent='auto')
@ -52,8 +57,11 @@ class Uniax(PersistentMixin, Drivable):
default=0.2, persistent='auto')
low_pos = Parameter('max. position for positive forces', FloatRange(unit='deg'), readonly=False, needscfg=False)
high_pos = Parameter('min. position for negative forces', FloatRange(unit='deg'), readonly=False, needscfg=False)
substantial_force = Parameter('min. force change expected within motor play', FloatRange(), default=1)
motor_play = Parameter('acceptable motor play within hysteresis', FloatRange(), readonly=False, default=10)
motor_max_play = Parameter('acceptable motor play outside hysteresis', FloatRange(), readonly=False, default=90)
timeout = Parameter('driving finishes when no progress within this delay', FloatRange(), readonly=False, default=300)
pollinterval = 0.1
fast_pollfactor = 1
_mot_target = None # for detecting manual motor manipulations
_filter_start = 0
@ -61,19 +69,21 @@ class Uniax(PersistentMixin, Drivable):
_sum = 0
_cnt_rderr = 0
_cnt_wrerr = 0
_action = None
_last_force = 0
_expected_step = 1
_fail_cnt = 0
_in_cnt = 0
_init_action = False
_zero_pos_tol = None
_find_target = 0
_state = None
_force = None # raw force
def earlyInit(self):
super().earlyInit()
self._zero_pos_tol = {}
self._action = self.idle
def initModule(self):
super().initModule()
self._state = StateMachine(logger=self.log, threaded=False, cleanup=self.cleanup)
def doPoll(self):
self.read_value()
self._state.cycle()
def drive_relative(self, step, ntry=3):
"""drive relative, try 3 times"""
@ -84,7 +94,12 @@ class Uniax(PersistentMixin, Drivable):
self.current_step = step
for _ in range(ntry):
try:
self._mot_target = self.motor.write_target(mot.value + step)
if abs(mot.value - mot.target) < mot.tolerance:
# make sure rounding erros do not suppress small steps
newpos = mot.target + step
else:
newpos = mot.value + step
self._mot_target = self.motor.write_target(newpos)
self._cnt_wrerr = max(0, self._cnt_wrerr - 1)
return True
except Exception as e:
@ -96,39 +111,52 @@ class Uniax(PersistentMixin, Drivable):
self.motor.reset()
return False
def reset_filter(self, now=0.0):
self._sum = self._cnt = 0
self._filter_start = now or time.time()
def motor_busy(self):
mot = self.motor
if mot.isBusy():
if mot.target != self._mot_target:
self.action = self.idle
raise Error('control stopped - motor moved directly')
return True
return False
def next_action(self, action):
"""call next action
def read_value(self):
try:
self._force = force = self.transducer.read_value()
self._cnt_rderr = max(0, self._cnt_rderr - 1)
except Exception as e:
self._cnt_rderr += 1
if self._cnt_rderr > 10:
self.stop()
self.status = 'ERROR', 'too many read errors: %s' % e
self.log.error(self.status[1])
self.read_target()
return Done
:param action: function to be called next time
:param do_now: do next action in the same cycle
"""
self._action = action
self._init_action = True
self.log.info('action %r', action.__name__)
now = time.time()
self._sum += force
self._cnt += 1
if now < self._filter_start + self.filter_interval:
return Done
force = self._sum / self._cnt
self.reset_filter(now)
if abs(force) > self.limit + self.hysteresis:
self.motor.stop()
self.status = 'ERROR', 'above max limit'
self.log.error(self.status[1])
self.read_target()
return Done
if self.zero_pos(force) is None and abs(force) > self.hysteresis:
self.set_zero_pos(force, self.motor.read_value())
return force
def init_action(self):
"""return true when called the first time after next_action"""
if self._init_action:
self._init_action = False
return True
return False
def reset_filter(self, now=0.0):
self._sum = self._cnt = 0
self._filter_start = now or time.time()
def zero_pos(self, value,):
def zero_pos(self, value):
"""get high_pos or low_pos, depending on sign of value
:param force: when not 0, return an estimate for a good starting position
:param value: return an estimate for a good starting position
"""
name = 'high_pos' if value > 0 else 'low_pos'
@ -155,207 +183,233 @@ class Uniax(PersistentMixin, Drivable):
self._zero_pos_tol[name] = tol
self.log.info('set %s = %.1f +- %.1f (@%g N)' % (name, pos, tol, force))
setattr(self, name, pos)
return pos
def find(self, force, target):
"""find active (engaged) range"""
sign = math.copysign(1, target)
if force * sign > self.hysteresis or force * sign > target * sign:
if self.motor_busy():
self.log.info('motor stopped - substantial force detected: %g', force)
self.motor.stop()
elif self.init_action():
self.next_action(self.adjust)
return
if abs(force) > self.hysteresis:
self.set_zero_pos(force, self.motor.read_value())
self.next_action(self.adjust)
return
if force * sign < -self.hysteresis:
self._previous_force = force
self.next_action(self.free)
return
if self.motor_busy():
if sign * self._find_target < 0: # target sign changed
self.motor.stop()
self.next_action(self.find) # restart find
def cleanup(self, state):
"""in case of error, set error status"""
if state.stopped: # stop or restart
if state.stopped is Restart:
return
self.status = 'IDLE', 'stopped'
self.log.warning('stopped')
else:
self._find_target = target
zero_pos = self.zero_pos(target)
side_name = 'positive' if target > 0 else 'negative'
if not self.init_action():
if abs(self.motor.target - self.motor.value) > self.motor.tolerance:
# no success on last find try, try short and strong step
self.write_adjusting(True)
self.log.info('one step to %g', self.motor.value + self.safe_step)
self.drive_relative(sign * self.safe_step)
return
if zero_pos is not None:
self.status = 'BUSY', 'change to %s side' % side_name
zero_pos += sign * (self.hysteresis * self.slope - self.motor.tolerance)
if (self.motor.value - zero_pos) * sign < -self.motor.tolerance:
self.write_adjusting(False)
self.log.info('change side to %g', zero_pos)
self.drive_relative(zero_pos - self.motor.value)
return
# we are already at or beyond zero_pos
self.next_action(self.adjust)
return
self.write_adjusting(False)
self.status = 'BUSY', 'find %s side' % side_name
self.log.info('one turn to %g', self.motor.value + sign * 360)
self.drive_relative(sign * 360)
self.status = 'ERROR', str(state.last_error)
if isinstance(state.last_error, Error):
self.log.error('%s', state.last_error)
else:
self.log.error('%r raised in state %r', str(state.last_error), state.status_string)
self.read_target() # make target invalid
self.motor.stop()
self.write_adjusting(False)
def free(self, force, target):
"""free from high force at other end"""
if self.motor_busy():
return
if abs(force) > abs(self._previous_force) + self.tolerance:
self.stop()
self.status = 'ERROR', 'force increase while freeing'
self.log.error(self.status[1])
return
if abs(force) < self.hysteresis:
self.next_action(self.find)
return
if self.init_action():
self._free_way = 0
self.log.info('free from high force %g', force)
self.write_adjusting(True)
sign = math.copysign(1, target)
if self._free_way > (abs(self._previous_force) + self.hysteresis) * self.slope:
self.stop()
self.status = 'ERROR', 'freeing failed'
self.log.error(self.status[1])
return
self._free_way += self.safe_step
self.drive_relative(sign * self.safe_step)
def reset_progress(self, state):
state.prev_force = self.value
state.prev_pos = self.motor.value
state.prev_time = time.time()
def within_tolerance(self, force, target):
"""within tolerance"""
if self.motor_busy():
return
if abs(target - force) > self.tolerance:
self.next_action(self.adjust)
elif self.init_action():
self.status = 'IDLE', 'within tolerance'
def check_progress(self, state):
force_step = self.target - self.value
direction = math.copysign(1, force_step)
try:
force_progress = direction * (self.value - state.prev_force)
except AttributeError: # prev_force undefined?
self.reset_progress(state)
return True
if force_progress >= self.substantial_force:
self.reset_progress(state)
else:
motor_dif = abs(self.motor.value - state.prev_pos)
if motor_dif > self.motor_play:
if motor_dif > self.motor_max_play:
raise Error('force seems not to change substantially %g %g (%g %g)' % (self.value, self.motor.value, state.prev_force, state.prev_pos))
return False
return True
def adjust(self, force, target):
def adjust(self, state):
"""adjust force"""
if state.init:
state.phase = 0 # just initialized
state.in_since = 0
state.direction = math.copysign(1, self.target - self.value)
state.pid_fact = 1
if self.motor_busy():
return
if abs(target - force) < self.tolerance:
self._in_cnt += 1
if self._in_cnt >= 3:
self.next_action(self.within_tolerance)
return
return Retry()
self.value = self._force
force_step = self.target - self.value
if abs(force_step) < self.tolerance:
if state.in_since == 0:
state.in_since = state.now
if state.now > state.in_since + 10:
return self.within_tolerance
else:
self._in_cnt = 0
if self.init_action():
self._fail_cnt = 0
if force_step * state.direction < 0:
if state.pid_fact == 1:
self.log.info('overshoot -> adjust with reduced pid_i')
state.pid_fact = 0.1
state.in_since = 0
if state.phase == 0:
state.phase = 1
self.reset_progress(state)
self.write_adjusting(True)
self.status = 'BUSY', 'adjusting force'
elif not self._filtered:
return
else:
force_step = force - self._last_force
if self._expected_step:
# compare detected / expected step
q = force_step / self._expected_step
if q < 0.1:
self._fail_cnt += 1
elif q > 0.5:
self._fail_cnt = max(0, self._fail_cnt - 1)
if self._fail_cnt >= 10:
if force < self.hysteresis:
self.log.warning('adjusting failed - try to find zero pos')
self.set_zero_pos(target, None)
self.next_action(self.find)
elif self._fail_cnt > 20:
self.stop()
self.status = 'ERROR', 'force seems not to change substantially'
self.log.error(self.status[1])
return
self._last_force = force
force_step = (target - force) * self.pid_i
if abs(target - force) < self.tolerance * 0.5:
self._expected_step = 0
return
self._expected_step = force_step
step = force_step * self.slope
self.drive_relative(step)
elif not self.check_progress(state):
if abs(self.value) < self.hysteresis:
if motor_dif > self.motor_play:
self.log.warning('adjusting failed - try to find zero pos')
self.set_zero_pos(self.target, None)
return self.find
elif time.time() > state.prev_time + self.timeout:
if state.phase == 1:
state.phase = 2
self.log.warning('no substantial progress since %d sec', self.timeout)
self.status = 'IDLE', 'adjusting timeout'
self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * state.pid_fact)
return Retry()
def idle(self, *args):
if self.init_action():
self.write_adjusting(False)
if self.status[0] == 'BUSY':
self.status = 'IDLE', 'stopped'
def read_value(self):
try:
force = self.transducer.read_value()
self._cnt_rderr = max(0, self._cnt_rderr - 1)
except Exception as e:
self._cnt_rderr += 1
if self._cnt_rderr > 10:
self.stop()
self.status = 'ERROR', 'too many read errors: %s' % e
self.log.error(self.status[1])
return Done
now = time.time()
def within_tolerance(self, state):
"""within tolerance"""
if state.init:
self.status = 'IDLE', 'within tolerance'
return Retry()
if self.motor_busy():
# do not filter while driving
self.value = force
self.reset_filter()
self._filtered = False
return Retry()
force_step = self.target - self.value
if abs(force_step) < self.tolerance * 0.5:
self.current_step = 0
else:
self._sum += force
self._cnt += 1
if now < self._filter_start + self.filter_interval:
return Done
force = self._sum / self._cnt
self.value = force
self.reset_filter(now)
self._filtered = True
if abs(force) > self.limit + self.hysteresis:
self.status = 'ERROR', 'above max limit'
self.log.error(self.status[1])
return Done
if self.zero_pos(force) is None and abs(force) > self.hysteresis and self._filtered:
self.set_zero_pos(force, self.motor.read_value())
self._action(self.value, self.target)
return Done
self.check_progress(state)
self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * 0.1)
if abs(force_step) > self.tolerance:
return self.out_of_tolerance
return Retry()
def out_of_tolerance(self, state):
"""out of tolerance"""
if state.init:
self.status = 'WARN', 'out of tolerance'
state.in_since = 0
return Retry()
if self.motor_busy():
return Retry()
force_step = self.target - self._force
if abs(force_step) < self.tolerance:
if state.in_since == 0:
state.in_since = state.now
if state.now > state.in_since + 10:
return self.within_tolerance
if abs(force_step) < self.tolerance * 0.5:
return Retry()
self.check_progress(state)
self.drive_relative(force_step * self.slope * self.pid_i * min(1, state.delta()) * 0.1)
return Retry()
def find(self, state):
"""find active (engaged) range"""
if state.init:
state.prev_direction = 0 # find not yet started
self.reset_progress(state)
direction = math.copysign(1, self.target)
self.value = self._force
abs_force = self.value * direction
if abs_force > self.hysteresis or abs_force > self.target * direction:
if self.motor_busy():
self.log.info('motor stopped - substantial force detected: %g', self.value)
self.motor.stop()
elif state.prev_direction == 0:
return self.adjust
if abs_force > self.hysteresis:
self.set_zero_pos(self.value, self.motor.read_value())
return self.adjust
if abs_force < -self.hysteresis:
state.force_before_free = self.value
return self.free
if self.motor_busy():
if direction == -state.prev_direction: # target direction changed
self.motor.stop()
state.init_find = True # restart find
return Retry()
zero_pos = self.zero_pos(self.target)
if state.prev_direction: # find already started
if abs(self.motor.target - self.motor.value) > self.motor.tolerance:
# no success on last find try, try short and strong step
self.write_adjusting(True)
self.log.info('one step to %g', self.motor.value + self.safe_step)
self.drive_relative(direction * self.safe_step)
return Retry()
else:
state.prev_direction = math.copysign(1, self.target)
side_name = 'negative' if direction == -1 else 'positive'
if zero_pos is not None:
self.status = 'BUSY', 'change to %s side' % side_name
zero_pos += direction * (self.hysteresis * self.slope - self.motor.tolerance)
if (self.motor.value - zero_pos) * direction < -self.motor.tolerance:
self.write_adjusting(False)
self.log.info('change side to %g', zero_pos)
self.drive_relative(zero_pos - self.motor.value)
return Retry()
# we are already at or beyond zero_pos
return self.adjust
self.write_adjusting(False)
self.status = 'BUSY', 'find %s side' % side_name
self.log.info('one turn to %g', self.motor.value + direction * 360)
self.drive_relative(direction * 360)
return Retry()
def free(self, state):
"""free from high force at other end"""
if state.init:
state.free_way = None
self.reset_progress(state)
if self.motor_busy():
return Retry()
self.value = self._force
if abs(self.value) > abs(state.force_before_free) + self.hysteresis:
raise Error('force increase while freeing')
if abs(self.value) < self.hysteresis:
return self.find
if state.free_way is None:
state.free_way = 0
self.log.info('free from high force %g', self.value)
self.write_adjusting(True)
direction = math.copysign(1, self.target)
if state.free_way > abs(state.force_before_free + self.hysteresis) * self.slope + self.motor_max_play:
raise Error('freeing failed')
state.free_way += self.safe_step
self.drive_relative(direction * self.safe_step)
return Retry()
def write_target(self, target):
if abs(target) > self.limit:
raise BadValueError('force above limit')
if abs(target - self.value) <= self.tolerance:
if self.isBusy():
self.stop()
self.next_action(self.within_tolerance)
else:
if not self.isBusy():
self.status = 'IDLE', 'already at target'
self.next_action(self.within_tolerance)
return target
self._state.start(self.within_tolerance)
return target
self.log.info('new target %g', target)
self._cnt_rderr = 0
self._cnt_wrerr = 0
self.status = 'BUSY', 'changed target'
self.target = target
if self.value * math.copysign(1, target) > self.hysteresis:
self.next_action(self.adjust)
self._state.start(self.adjust)
else:
self.next_action(self.find)
return target
self._state.start(self.find)
return Done
def read_target(self):
if self._state.state is None:
if self.status[1]:
raise Error(self.status[1])
raise Error('inactive')
return self.target
@Command()
def stop(self):
self._action = self.idle
if self.motor.isBusy():
self.log.info('stop motor')
self.motor.stop()
self.next_action(self.idle)
self.status = 'IDLE', 'stopped'
self._state.stop()
def write_force_offset(self, value):
self.force_offset = value