diff --git a/cfg/uniax.cfg b/cfg/uniax.cfg index c1cbf18..1d8d755 100644 --- a/cfg/uniax.cfg +++ b/cfg/uniax.cfg @@ -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] diff --git a/secop_psi/uniax.py b/secop_psi/uniax.py index 095c1d2..e4e0813 100644 --- a/secop_psi/uniax.py +++ b/secop_psi/uniax.py @@ -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):