rework of uniax
- try to deal with anomaly at 17 N - reduce pid on overshoot - use new statemachine
This commit is contained in:
parent
07995e2235
commit
5e3bf96df1
@ -42,10 +42,15 @@ digits = 2
|
||||
scale_factor = 0.0156
|
||||
offset = 15
|
||||
|
||||
[res_io]
|
||||
description = io to lakeshore
|
||||
class = secop_psi.ls340res.LscIO
|
||||
uri = tcp://192.168.127.254:3003
|
||||
|
||||
[res]
|
||||
description = temperature on uniax stick
|
||||
class = secop_psi.ls340res.ResChannel
|
||||
uri = tcp://192.168.127.254:3003
|
||||
io = res_io
|
||||
channel = A
|
||||
|
||||
[T]
|
||||
|
||||
@ -25,11 +25,11 @@ 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(Exception):
|
||||
class Error(SECoPError):
|
||||
pass
|
||||
|
||||
|
||||
@ -38,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')
|
||||
@ -57,21 +57,21 @@ 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=0)
|
||||
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=70)
|
||||
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
|
||||
|
||||
_mot_target = None # for detecting manual motor manipulations
|
||||
_filter_start = 0
|
||||
_filtered = False
|
||||
_cnt = 0
|
||||
_sum = 0
|
||||
_cnt_rderr = 0
|
||||
_cnt_wrerr = 0
|
||||
_in_cnt = 0
|
||||
_zero_pos_tol = None
|
||||
_state = None
|
||||
_force = None # raw force
|
||||
|
||||
def earlyInit(self):
|
||||
super().earlyInit()
|
||||
@ -94,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:
|
||||
@ -106,10 +111,6 @@ 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():
|
||||
@ -118,6 +119,40 @@ class Uniax(PersistentMixin, Drivable):
|
||||
return True
|
||||
return False
|
||||
|
||||
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
|
||||
|
||||
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 reset_filter(self, now=0.0):
|
||||
self._sum = self._cnt = 0
|
||||
self._filter_start = now or time.time()
|
||||
|
||||
def zero_pos(self, value):
|
||||
"""get high_pos or low_pos, depending on sign of value
|
||||
|
||||
@ -148,23 +183,143 @@ 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 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.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 reset_progress(self, state):
|
||||
state.prev_force = self.value
|
||||
state.prev_pos = self.motor.value
|
||||
state.prev_time = time.time()
|
||||
|
||||
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, 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 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:
|
||||
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.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 within_tolerance(self, state):
|
||||
"""within tolerance"""
|
||||
if state.init:
|
||||
self.status = 'IDLE', 'within tolerance'
|
||||
return Retry()
|
||||
if self.motor_busy():
|
||||
return Retry()
|
||||
force_step = self.target - self.value
|
||||
if abs(force_step) < self.tolerance * 0.5:
|
||||
self.current_step = 0
|
||||
else:
|
||||
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)
|
||||
if self.value * direction > self.hysteresis or self.value * direction > self.target * direction:
|
||||
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(self.value) > self.hysteresis:
|
||||
if abs_force > self.hysteresis:
|
||||
self.set_zero_pos(self.value, self.motor.read_value())
|
||||
return self.adjust
|
||||
if self.value * direction < -self.hysteresis:
|
||||
if abs_force < -self.hysteresis:
|
||||
state.force_before_free = self.value
|
||||
return self.free
|
||||
if self.motor_busy():
|
||||
@ -199,33 +354,16 @@ class Uniax(PersistentMixin, Drivable):
|
||||
self.drive_relative(direction * 360)
|
||||
return Retry()
|
||||
|
||||
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.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.motor.stop()
|
||||
self.write_adjusting(False)
|
||||
|
||||
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()
|
||||
if abs(self.value) > abs(state.force_before_free) + self.tolerance:
|
||||
self.stop()
|
||||
self.status = 'ERROR', 'force increase while freeing'
|
||||
self.log.error(self.status[1])
|
||||
return None
|
||||
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:
|
||||
@ -233,93 +371,12 @@ class Uniax(PersistentMixin, Drivable):
|
||||
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.stop()
|
||||
self.status = 'ERROR', 'freeing failed'
|
||||
self.log.error(self.status[1])
|
||||
return None
|
||||
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 within_tolerance(self, state):
|
||||
"""within tolerance"""
|
||||
if self.motor_busy():
|
||||
return Retry()
|
||||
if abs(self.target - self.value) > self.tolerance:
|
||||
return self.adjust
|
||||
self.status = 'IDLE', 'within tolerance'
|
||||
|
||||
def adjust(self, state):
|
||||
"""adjust force"""
|
||||
if state.init:
|
||||
state.prev_force = None
|
||||
if self.motor_busy():
|
||||
return
|
||||
if abs(self.target - self.value) < self.tolerance:
|
||||
self._in_cnt += 1
|
||||
if self._in_cnt >= 3:
|
||||
return self.within_tolerance
|
||||
else:
|
||||
self._in_cnt = 0
|
||||
if state.prev_force is None:
|
||||
state.prev_force = self.value
|
||||
state.prev_pos = self.motor.pos
|
||||
self.write_adjusting(True)
|
||||
self.status = 'BUSY', 'adjusting force'
|
||||
elif not self._filtered:
|
||||
return
|
||||
else:
|
||||
if abs(self.value - state.prev_force) > self.substantial_force:
|
||||
state.prev_force = self.value
|
||||
state.prev_pos = self.motor.value
|
||||
else:
|
||||
motor_dif = abs(self.value - state.prev_pos)
|
||||
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 motor_dif > self.motor_max_play:
|
||||
raise Error('force seems not to change substantially')
|
||||
force_step = (self.target - self.value) * self.pid_i
|
||||
self.drive_relative(force_step * self.slope)
|
||||
return Retry()
|
||||
|
||||
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()
|
||||
if self.motor_busy():
|
||||
# do not filter while driving
|
||||
self.reset_filter()
|
||||
self._filtered = False
|
||||
else:
|
||||
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)
|
||||
self._filtered = True
|
||||
if abs(force) > self.limit + self.hysteresis:
|
||||
self.motor.stop()
|
||||
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())
|
||||
return force
|
||||
|
||||
def write_target(self, target):
|
||||
if abs(target) > self.limit:
|
||||
raise BadValueError('force above limit')
|
||||
@ -332,11 +389,19 @@ class Uniax(PersistentMixin, Drivable):
|
||||
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._state.start(self.adjust)
|
||||
else:
|
||||
self._state.start(self.find)
|
||||
return target
|
||||
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):
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user