# -*- coding: utf-8 -*- # ***************************************************************************** # # This program is free software; you can redistribute it and/or modify it under # the terms of the GNU General Public License as published by the Free Software # Foundation; either version 2 of the License, or (at your option) any later # version. # # This program is distributed in the hope that it will be useful, but WITHOUT # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS # FOR A PARTICULAR PURPOSE. See the GNU General Public License for more # details. # # You should have received a copy of the GNU General Public License along with # this program; if not, write to the Free Software Foundation, Inc., # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA # # Module authors: # M. Zolliker # # ***************************************************************************** """use transducer and motor to adjust force""" import time from secop.lib import clamp from secop.core import Drivable, Parameter, FloatRange, EnumType, Done, \ Attached, Command, PersistentMixin, PersistentParam, BoolType class Uniax(PersistentMixin, Drivable): value = Parameter(unit='N') motor = Attached() transducer = Attached() tolerance = Parameter('force tolerance', FloatRange(), readonly=False, default=0.1) pid_p = PersistentParam('proportial term', FloatRange(unit='deg/N'), readonly=False, default=0.25, persistent='auto') filter_interval = Parameter('filter time', FloatRange(0, 60, unit='s'), readonly=False, default=1) current_sign = Parameter('', EnumType(negative=-1, undefined=0, positive=1), default=0) current_step = Parameter('', FloatRange(), default=0) force_offset = PersistentParam('transducer offset', FloatRange(), readonly=False, default=0, initwrite=True, persistent='auto') hysteresis = PersistentParam('force hysteresis', FloatRange(0), readonly=False, default=5, persistent='auto') release_step = PersistentParam('step when releasing force', FloatRange(0), readonly=False, default=20, persistent='auto') adjusting = Parameter('', BoolType(), readonly=False, default=False) adjusting_current = PersistentParam('current when adjusting force', FloatRange(0, 2.8, unit='A'), readonly=False, default=0.5, persistent='auto') adjusting_step = PersistentParam('max. motor step when adjusting force', FloatRange(0, unit='deg'), readonly=False, default=5, persistent='auto') safe_current = PersistentParam('current when moving far', FloatRange(0, 2.8, unit='A'), readonly=False, default=0.2, persistent='auto') low_pos = PersistentParam('max. position for positive forces', FloatRange(), readonly=False, default=0) high_pos = PersistentParam('min. position for negative forces', FloatRange(), readonly=False, default=0) pollinterval = 0.1 fast_pollfactor = 1 _driver = None # whe defined a gerenator to be called for driving _target = None # freshly set target _mot_target = None # for detecting manual motor manipulations _filter_start = 0 _cnt = 0 _sum = 0 _cnt_rderr = 0 _cnt_wrerr = 0 def pos_range(self, target_sign, value=None): attr = 'high_pos' if target_sign > 0 else 'low_pos' if value is not None: value -= self.release_step * target_sign setattr(self, attr, value) return getattr(self, attr) def drive_to(self, pos): mot = self._motor self.current_step = pos - mot.value for itry in range(3): try: print('drive by %.2f' % self.current_step) self._mot_target = self._motor.write_target(pos) self._cnt_wrerr = max(0, self._cnt_wrerr - 1) break except Exception as e: print('driver_to', e) self._cnt_wrerr += 1 if self._cnt_wrerr > 5: raise print('RESET') self._motor.reset() def drive_generator(self, target): self._mot_target = self._motor.target mot = self._motor target_sign = -1 if target < 0 else 1 force = self.value # TODO: do this only when abs(target) > hysteresis? if target_sign != self.current_sign and force * target_sign < self.hysteresis: self.write_adjusting(False) pos_lim = None while target_sign * (target_sign * self.hysteresis - force) > self.tolerance: self.status = 'BUSY', 'find active motor range' # we have to increase abs(force) if not mot.isBusy() or target_sign * (mot.target - mot.value) < 0: self.drive_to(mot.value + 360 * target_sign) force = yield if target_sign * force < self.hysteresis: pos_lim = mot.value print('found active range') while mot.isBusy(): mot.stop() force = yield print('stopped') if target_sign * (force - self.hysteresis) > 0: self.current_sign = target_sign elif pos_lim is not None: self.pos_range(target_sign, pos_lim) if abs(force) < self.hysteresis and self.low_pos < mot.value < self.high_pos: self.current_sign = 0 if target_sign * (self.value - target) > self.tolerance: self.status = 'BUSY', 'release force' self.write_adjusting(True) # abs(force) too high while target_sign * (target - self.value) < self.hysteresis and self.value * target_sign > 0: self.drive_to(mot.value - target_sign * min(self.adjusting_step, self.release_step)) while mot.isBusy(): self.reset_filter() yield if target_sign * (force + self.hysteresis) < 0: print('force', force) self.terminate('ERROR', 'force too high when moving back') return print('released', self.value, target) self.write_adjusting(True) force = yield while target_sign * (target - self.value) > self.tolerance: self.status = 'BUSY', 'adjusting force' # slowly increasing abs(force) step = self.pid_p * (target - self.value) if abs(step) > self.adjusting_step: step = target_sign * self.adjusting_step self.drive_to(mot.value + step) while mot.isBusy(): self.reset_filter() yield self.write_adjusting(False) self.status = 'IDLE', 'reached target' return def reset_filter(self, now=0): self._sum = self._cnt = 0 self._filter_start = now or time.time() def read_value(self): if self._target is not None: self._driver = self.drive_generator(self._target) self._driver.send(None) # start self._target = None mot = self._motor try: value = 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.terminate('ERROR', 'too many read errors: %s' % e) return Done now = time.time() newvalue = False if self._cnt > 0 and now > self._filter_start + self.filter_interval: self.value = self._sum / self._cnt self.reset_filter(now) newvalue = True self._sum += value self._cnt += 1 if self._driver: try: if self._mot_target != mot.target and mot.isBusy(): print('mottarget', mot.target, self._mot_target) self.terminate('ERROR', 'stopped due to direct motor manipulation') return Done if self.adjusting: if newvalue: print(' %.2f' % self.value) # next step only when a new filtered value is available self._driver.send(self.value) else: print(' %.2f' % value) self._driver.send(value) except StopIteration: self._driver = None except Exception as e: self.terminate('ERROR', str(e)) self._driver = None return Done def write_target(self, target): if abs(target - self.value) <= self.tolerance: if self.isBusy(): self.stop() self.status = 'IDLE', 'already at target' return target self._cnt_rderr = 0 self._cnt_wrerr = 0 self.status = 'BUSY', 'changed target' self._target = target return target def terminate(self, *status): self.stop() self.status = status print(status) @Command() def stop(self): self._driver = None if self._motor.isBusy(): self._motor.stop() self.status = 'IDLE', 'stopped' self._filterd = True def write_force_offset(self, value): self.force_offset = value # self.saveParameters() self._transducer.write_offset(value) return Done def write_adjusting(self, value): mot = self._motor if value: mot_current = self.adjusting_current mot.write_move_limit(self.adjusting_step) else: mot_current = self.safe_current mot.write_safe_current(mot_current) if abs(mot_current - mot.maxcurrent) > 0.01: # resolution of current: 2.8 / 250 mot.write_maxcurrent(mot_current) return value