reworked attocube
- step_mode: soft closed loop, stepwise, reading encoder after a delay - calib_steps command to determine step size Change-Id: I27bdffb4d564ac9c55a6473704ac2de6ad92bac8
This commit is contained in:
parent
a2905d9fbc
commit
db29776dd5
@ -10,7 +10,9 @@ Mod('r',
|
||||
value = Param(unit='deg'),
|
||||
tolerance = 0.01,
|
||||
target_min = 0,
|
||||
target_max = 360
|
||||
target_max = 360,
|
||||
steps_fwd = 50,
|
||||
steps_bwd = 100,
|
||||
# gear = 1.2,
|
||||
)
|
||||
|
||||
|
||||
@ -21,15 +21,16 @@ import sys
|
||||
import time
|
||||
import threading
|
||||
from frappy.core import Drivable, Parameter, Command, Property, \
|
||||
ERROR, WARN, BUSY, IDLE, Done, nopoll, Limit
|
||||
ERROR, WARN, BUSY, IDLE, nopoll, Limit, TupleOf
|
||||
# from frappy.features import HasSimpleOffset
|
||||
from frappy.datatypes import IntRange, FloatRange, StringType, BoolType
|
||||
from frappy.errors import ConfigError, BadValueError
|
||||
from frappy.errors import BadValueError
|
||||
sys.path.append('/home/l_samenv/Documents/anc350/Linux64/userlib/lib')
|
||||
from PyANC350v4 import Positioner
|
||||
|
||||
|
||||
DIRECTION_NAME = {1: 'forward', -1: 'backward', 0: 'idle'}
|
||||
class Stopped(RuntimeError):
|
||||
"""thread was stopped"""
|
||||
|
||||
|
||||
class DriveInfo:
|
||||
@ -50,32 +51,41 @@ class Axis(Drivable):
|
||||
frequency = Parameter('frequency', FloatRange(1, unit='Hz'), readonly=False)
|
||||
amplitude = Parameter('amplitude', FloatRange(0, unit='V'), readonly=False)
|
||||
gear = Parameter('gear factor', FloatRange(), readonly=False, value=1)
|
||||
tolerance = Parameter('positioning tolerance', FloatRange(0, unit='$'), readonly=False, default=0.01)
|
||||
tolerance = Parameter('positioning tolerance', FloatRange(0, unit='$'),
|
||||
readonly=False, default=0.01)
|
||||
output = Parameter('enable output', BoolType(), readonly=False)
|
||||
info = Parameter('axis info', StringType())
|
||||
statusbits = Parameter('status bits', StringType())
|
||||
maxtry = Parameter('max. number of move tries', IntRange(), default=3, readonly=False)
|
||||
settling_time = Property('time to be spent wihtin tolerance', FloatRange(0), default=0.25)
|
||||
fast_interval = Property('waiting time with in internal drive polls', FloatRange(0), default=0.001)
|
||||
step_mode = Parameter('step mode (soft closed loop)', BoolType(),
|
||||
default=False, readonly=False, group='step_mode')
|
||||
maxtry = Parameter('max. number of move tries', IntRange(),
|
||||
default=5, readonly=False, group='step_mode')
|
||||
steps_fwd = Parameter('forward steps / main unit', FloatRange(0),
|
||||
default=0, readonly=False, group='step_mode')
|
||||
steps_bwd = Parameter('backward steps / main unit', FloatRange(0),
|
||||
default=0, readonly=False, group='step_mode')
|
||||
delay = Parameter('delay between tries within loop', FloatRange(0),
|
||||
readonly=False, default=0.05, group='step_mode')
|
||||
prop = Parameter('factor for control loop', FloatRange(0, 1),
|
||||
readonly=False, default=0.8, group='step_mode')
|
||||
target_min = Limit()
|
||||
target_max = Limit()
|
||||
|
||||
_bad_status_max = 0.05
|
||||
_settling_time = 0.1
|
||||
fast_interval = 0.25
|
||||
|
||||
_hw = None
|
||||
_scale = 1 # scale for custom units
|
||||
_move_steps = 0 # number of steps to move (used by move command)
|
||||
SCALES = {'deg': 1, 'm': 1, 'mm': 1000, 'um': 1000000, 'µm': 1000000}
|
||||
_moving = None # None when not moving, else status text
|
||||
_idle_status = IDLE, ''
|
||||
_error_state = '' # empty string: no error
|
||||
# _history = None
|
||||
_check_sensor = False
|
||||
_drive_info = DriveInfo(0, 0, (IDLE, ''))
|
||||
STATUSBIT_NAMES = ['sensor', 'output', 'moving', 'at_target', 'fwd_stuck', 'bwd_stuck', 'error']
|
||||
_thread = None
|
||||
_moving_since = 0
|
||||
_output = False
|
||||
_sensor_connected = False
|
||||
_status = IDLE, ''
|
||||
_statusbits = None
|
||||
|
||||
def initModule(self):
|
||||
super().initModule()
|
||||
self._stopped = threading.Event()
|
||||
# TODO: catch timeout
|
||||
self._hw = Positioner()
|
||||
|
||||
@ -83,15 +93,14 @@ class Axis(Drivable):
|
||||
self._scale = self.SCALES[self.parameters['value'].datatype.unit] * self.gear
|
||||
return value
|
||||
|
||||
def startModule(self, start_events):
|
||||
# self._history = [0]
|
||||
super().startModule(start_events)
|
||||
start_events.queue(self.read_info)
|
||||
def initialReads(self):
|
||||
self.read_info()
|
||||
super().initialReads()
|
||||
|
||||
def _get_state(self, info):
|
||||
def _get_status(self):
|
||||
"""get axis status
|
||||
|
||||
- update _drive_info.output ans _drive_info.sensor_connected
|
||||
- update _output and _sensor_connected
|
||||
- return <moving flag>, <error flag>, <reason>
|
||||
|
||||
<moving flag> is True whn moving
|
||||
@ -99,85 +108,60 @@ class Axis(Drivable):
|
||||
<reason> is an error text, when in error, 'at target' or '' otherwise
|
||||
"""
|
||||
statusbits = self._hw.getAxisStatus(self.axis)
|
||||
info.sensor_connected, info.output, moving, at_target, fwd_stuck, bwd_stuck, error = statusbits
|
||||
info.statusbits = statusbits
|
||||
self._sensor_connected, self._output, moving, at_target, fwd_stuck, bwd_stuck, error = statusbits
|
||||
self._statusbits = statusbits
|
||||
if error:
|
||||
return False, True, 'other error'
|
||||
return ERROR, 'other error'
|
||||
if bwd_stuck:
|
||||
return False, True, 'end of travel backward'
|
||||
return ERROR, 'end of travel backward'
|
||||
if bwd_stuck:
|
||||
return False, True, 'end of travel forward'
|
||||
if at_target:
|
||||
return False, False, 'at target'
|
||||
if moving:
|
||||
#if not info.output:
|
||||
# return True, True, 'output off'
|
||||
return True, False, ''
|
||||
return False, False, ''
|
||||
return ERROR, 'end of travel forward'
|
||||
if self._moving_since:
|
||||
if time.time() < self._moving_since + 1:
|
||||
return BUSY, 'started'
|
||||
if at_target:
|
||||
self.setFastPoll(False)
|
||||
self._moving_since = 0
|
||||
return IDLE, 'at target'
|
||||
if moving and self._output:
|
||||
return BUSY, 'moving'
|
||||
if not self._output:
|
||||
return WARN, 'switched output off by unknown reason'
|
||||
return WARN, 'switched moving off by unknown reason'
|
||||
return IDLE, ''
|
||||
|
||||
def _drive_thread(self, info):
|
||||
try:
|
||||
for ntry in range(self.maxtry, 0, -1):
|
||||
self._hw.setAxisOutput(self.axis, enable=True, autoDisable=0)
|
||||
self._hw.setTargetRange(self.axis, 0 / self._scale)
|
||||
self._hw.setTargetPosition(self.axis, info.target / self._scale)
|
||||
self._hw.startAutoMove(self.axis, enable=1, relative=0)
|
||||
inside_since = 0
|
||||
saved_inside_time = 0
|
||||
last_inside = 0
|
||||
start_time = time.time()
|
||||
status = None
|
||||
while info.thread:
|
||||
time.sleep(self.fast_interval)
|
||||
moving, in_error, reason = self._get_state(info)
|
||||
if time.time() > start_time + 0.05:
|
||||
if reason:
|
||||
if in_error:
|
||||
status = ERROR, reason
|
||||
else:
|
||||
status = IDLE, reason
|
||||
break
|
||||
if not moving:
|
||||
status = WARN, 'stopped by unknown reason'
|
||||
break
|
||||
info.pos = self._hw.getPosition(self.axis) * self._scale
|
||||
self.check_value(info.pos)
|
||||
if abs(info.pos - info.target) < self.tolerance:
|
||||
last_inside = time.time()
|
||||
if not inside_since:
|
||||
inside_since = last_inside - saved_inside_time
|
||||
if last_inside > inside_since + self._settling_time:
|
||||
info.status = IDLE, 'in tolerance'
|
||||
self.doPoll()
|
||||
return
|
||||
else:
|
||||
info.status = BUSY, 'driving'
|
||||
if inside_since:
|
||||
saved_inside_time = last_inside - inside_since
|
||||
inside_since = 0
|
||||
if ntry == 1:
|
||||
info.status = status
|
||||
else:
|
||||
info.status = BUSY, f'retry after {status[1]} ({ntry})'
|
||||
self.doPoll()
|
||||
except Exception as e:
|
||||
info.status = ERROR, repr(e)
|
||||
finally:
|
||||
info.thread = None
|
||||
|
||||
def check_value(self, value):
|
||||
def check_value(self, value, direction):
|
||||
"""check if value allows moving in current direction"""
|
||||
if self._drive_info.direction > 0:
|
||||
if direction > 0:
|
||||
if value > self.target_max:
|
||||
raise BadValueError('above upper limit')
|
||||
elif value < self.target_min:
|
||||
raise BadValueError('below lower limit')
|
||||
|
||||
def read_status(self):
|
||||
status = self._get_status()
|
||||
self.statusbits = ''.join(k for k, v in zip('SOMTFBE', self._statusbits) if v)
|
||||
if self.step_mode:
|
||||
return self._status
|
||||
return status
|
||||
|
||||
def _wait(self, delay):
|
||||
if self._stopped.wait(delay):
|
||||
raise Stopped()
|
||||
|
||||
def _read_pos(self):
|
||||
poslist = []
|
||||
for i in range(9):
|
||||
if i:
|
||||
self._wait(0.001)
|
||||
poslist.append(self._hw.getPosition(self.axis) * self._scale)
|
||||
self._poslist = sorted(poslist)
|
||||
return self._poslist[len(poslist) // 2] # median
|
||||
|
||||
def read_value(self):
|
||||
drv = self._drive_info
|
||||
if drv.thread:
|
||||
return drv.pos
|
||||
return self._hw.getPosition(self.axis) * self._scale
|
||||
if self._thread:
|
||||
return self.value
|
||||
return self._read_pos()
|
||||
|
||||
def read_frequency(self):
|
||||
return self._hw.getFrequency(self.axis)
|
||||
@ -193,77 +177,166 @@ class Axis(Drivable):
|
||||
self._hw.setAmplitude(self.axis, value)
|
||||
return self._hw.getAmplitude(self.axis)
|
||||
|
||||
#def write_tolerance(self, value):
|
||||
# self._hw.setTargetRange(self.axis, value / self._scale)
|
||||
# return value
|
||||
|
||||
def write_output(self, value):
|
||||
self._hw.setAxisOutput(self.axis, enable=value, autoDisable=0)
|
||||
if self._thread:
|
||||
if not value:
|
||||
self.stop()
|
||||
else:
|
||||
self._hw.setAxisOutput(self.axis, enable=value, autoDisable=0)
|
||||
self._output = value
|
||||
return value
|
||||
|
||||
def read_status(self):
|
||||
drv = self._drive_info
|
||||
if not drv.thread:
|
||||
self._get_state(drv)
|
||||
self.setFastPoll(False)
|
||||
self.statusbits = ''.join(k for k, v in zip('SOMTFBE', drv.statusbits) if v)
|
||||
return drv.status
|
||||
|
||||
def _start_motor(self, target):
|
||||
self._stop_thread()
|
||||
self._drive_info = DriveInfo(self.value, target)
|
||||
self._drive_info.thread = threading.Thread(target=self._drive_thread, args=(self._drive_info,))
|
||||
self._drive_info.thread.start()
|
||||
def read_output(self):
|
||||
return self._output
|
||||
|
||||
def _stop_thread(self):
|
||||
try:
|
||||
drv = self._drive_info
|
||||
drv.thread.join()
|
||||
except AttributeError:
|
||||
pass
|
||||
if self._thread:
|
||||
self._thread.join()
|
||||
|
||||
def _stop(self):
|
||||
self._move_steps = 0
|
||||
self.setFastPoll(False)
|
||||
self._stop_thread()
|
||||
self.read_status()
|
||||
def _run_drive(self, target):
|
||||
self.value = self._read_pos()
|
||||
self.status = self._status = BUSY, 'drive by steps'
|
||||
cnt = self.maxtry
|
||||
prev = 0
|
||||
tol = self.tolerance
|
||||
while True:
|
||||
dif = target - self.value
|
||||
if target > self._poslist[2] + tol: # 78th percentile
|
||||
steps = max(1, min(dif, (dif + tol) * self.prop) * self.steps_fwd)
|
||||
elif target < self._poslist[-3] - tol: # 22th percentile
|
||||
steps = min(-1, max(dif, (dif - tol) * self.prop) * self.steps_bwd)
|
||||
else:
|
||||
self._status = IDLE, 'in tolerance'
|
||||
self.read_status()
|
||||
return
|
||||
if cnt <= 0:
|
||||
self._status = ERROR, 'too many tries'
|
||||
self.read_status()
|
||||
return
|
||||
if abs(steps) > prev * 0.7:
|
||||
cnt -= 1
|
||||
prev = abs(steps)
|
||||
self._move_steps(steps)
|
||||
|
||||
def _thread_wrapper(self, func, *args):
|
||||
try:
|
||||
func(*args)
|
||||
except Stopped as e:
|
||||
self._status = IDLE, str(e)
|
||||
except Exception as e:
|
||||
self._status = ERROR, f'{type(e).__name__} - {e}'
|
||||
finally:
|
||||
self._hw.setAxisOutput(self.axis, enable=0, autoDisable=0)
|
||||
self.setFastPoll(False)
|
||||
self._thread = None
|
||||
|
||||
def _start_thread(self, *args):
|
||||
thread = threading.Thread(target=self._thread_wrapper, args=args)
|
||||
self._thread = thread
|
||||
thread.start()
|
||||
|
||||
def write_target(self, target):
|
||||
# self.check_limits(value)
|
||||
# self._try_count = 0
|
||||
self._move_steps = 0
|
||||
self._start_motor(target)
|
||||
# self._history = [self.value]
|
||||
self.setFastPoll(True, 0.25)
|
||||
self._stop_thread()
|
||||
self._hw.setTargetRange(self.axis, self.tolerance / self._scale)
|
||||
if self.step_mode:
|
||||
self.status = BUSY, 'changed target'
|
||||
self._start_thread(self._run_drive, target)
|
||||
else:
|
||||
self.setFastPoll(True, 0.25)
|
||||
self._hw.setTargetPosition(self.axis, target / self._scale)
|
||||
self._hw.setAxisOutput(self.axis, enable=0, autoDisable=0)
|
||||
self._hw.startAutoMove(self.axis, enable=1, relative=0)
|
||||
self._moving_since = time.time()
|
||||
self.status = self._get_status()
|
||||
return target
|
||||
|
||||
@Command()
|
||||
def stop(self):
|
||||
self._stopped.set()
|
||||
self._stop_thread()
|
||||
self._status = IDLE, 'stopped'
|
||||
self.read_status()
|
||||
|
||||
@Command(IntRange())
|
||||
def move(self, value):
|
||||
"""relative move by number of steps"""
|
||||
self.check_value(self.value, value)
|
||||
self._start_thread(self._run_move, value)
|
||||
|
||||
def _move_steps(self, steps):
|
||||
steps = round(steps)
|
||||
if not steps:
|
||||
return
|
||||
previous = self._read_pos()
|
||||
self._hw.setAxisOutput(self.axis, enable=1, autoDisable=0)
|
||||
# wait for output is really on
|
||||
for i in range(100):
|
||||
self._wait(0.001)
|
||||
self._get_status()
|
||||
if self._output:
|
||||
break
|
||||
else:
|
||||
raise ValueError('can not switch on output')
|
||||
for i in range(abs(steps)):
|
||||
if not self._thread:
|
||||
raise Stopped('stopped')
|
||||
self._hw.startSingleStep(self.axis, steps < 0)
|
||||
self._wait(1 / self.frequency)
|
||||
self._get_status()
|
||||
if not self._output and i:
|
||||
steps = i
|
||||
self._step_size = 0
|
||||
break
|
||||
self._wait(self.delay)
|
||||
self.value = pos = self._read_pos()
|
||||
if self._output:
|
||||
self._step_size = (pos - previous) / steps
|
||||
|
||||
def _run_move(self, steps):
|
||||
self.setFastPoll(True, self.fast_interval)
|
||||
self._move_steps(steps)
|
||||
|
||||
@Command(IntRange(0))
|
||||
def calib_steps(self, delta):
|
||||
"""relative move by number of steps"""
|
||||
self._stop_thread()
|
||||
self._start_thread(self._run_calib, delta)
|
||||
|
||||
def _run_calib(self, steps):
|
||||
self.target = self.value = self._read_pos()
|
||||
maxfwd = 0
|
||||
maxbwd = 0
|
||||
cntfwd = 0
|
||||
cntbwd = 0
|
||||
for i in range(10):
|
||||
if self.value < self.target:
|
||||
self._move_steps(steps)
|
||||
self._move_steps(steps)
|
||||
while self.value < self.target:
|
||||
self._move_steps(steps)
|
||||
if self._step_size:
|
||||
maxfwd = max(maxfwd, self._step_size)
|
||||
cntfwd += 1
|
||||
else:
|
||||
self._move_steps(-steps)
|
||||
self._move_steps(-steps)
|
||||
while self.value > self.target:
|
||||
self._move_steps(-steps)
|
||||
if self._step_size:
|
||||
maxbwd = max(maxbwd, self._step_size)
|
||||
cntbwd += 1
|
||||
if cntfwd >= 3 and cntbwd >= 3:
|
||||
self.steps_fwd = 1 / maxfwd
|
||||
self.steps_bwd = 1 / maxbwd
|
||||
self._run_drive()
|
||||
self._status = IDLE, 'calib step size done'
|
||||
break
|
||||
else:
|
||||
self._status = WARN, 'calib step size failed'
|
||||
|
||||
@nopoll
|
||||
def read_info(self):
|
||||
"""read info from controller"""
|
||||
cap = self._hw.measureCapacitance(self.axis) * 1e9
|
||||
axistype = ['linear', 'gonio', 'rotator'][self._hw.getActuatorType(self.axis)]
|
||||
return '%s %s %.3gnF' % (self._hw.getActuatorName(self.axis), axistype, cap)
|
||||
|
||||
@Command()
|
||||
def stop(self):
|
||||
self._idle_status = IDLE, 'stopped' if self.isBusy() else ''
|
||||
self._stop()
|
||||
self.status = self._idle_status
|
||||
|
||||
@Command(IntRange())
|
||||
def move(self, value):
|
||||
"""relative move by number of steps"""
|
||||
self._direction = 1 if value > 0 else -1
|
||||
self.check_value(self.value)
|
||||
if DIRECTION_NAME[self._direction] in self._error_state:
|
||||
raise BadValueError('can not move (%s)' % self._error_state)
|
||||
self._move_steps = value
|
||||
self._idle_status = IDLE, ''
|
||||
self.read_status()
|
||||
self.setFastPoll(True, 1/self.frequency)
|
||||
|
||||
@Command(IntRange())
|
||||
def automove(self, flag):
|
||||
"""switch automove and output"""
|
||||
self._hw.setAxisOutput(self.axis, enable=flag, autoDisable=0)
|
||||
self._hw.startAutoMove(self.axis, enable=flag, relative=0)
|
||||
Loading…
x
Reference in New Issue
Block a user