265 lines
10 KiB
Python
265 lines
10 KiB
Python
# *****************************************************************************
|
|
# 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:
|
|
# Markus Zolliker <markus.zolliker@psi.ch>
|
|
# *****************************************************************************
|
|
|
|
import sys
|
|
import time
|
|
from frappy.core import Drivable, Parameter, Command, Property, ERROR, WARN, BUSY, IDLE, Done, nopoll
|
|
from frappy.features import HasTargetLimits, HasSimpleOffset
|
|
from frappy.datatypes import IntRange, FloatRange, StringType, BoolType
|
|
from frappy.errors import ConfigError, BadValueError
|
|
sys.path.append('/home/l_samenv/Documents/anc350/Linux64/userlib/lib')
|
|
from PyANC350v4 import Positioner
|
|
|
|
|
|
DIRECTION_NAME = {1: 'forward', -1: 'backward'}
|
|
|
|
|
|
class FreezeStatus:
|
|
"""freeze status for some time
|
|
|
|
hardware quite often does not treat status correctly: on a target change it
|
|
may take some time to return the 'busy' status correctly.
|
|
|
|
in classes with this mixin, within :meth:`write_target` call
|
|
|
|
self.freeze_status(0.5, BUSY, 'changed target')
|
|
|
|
a wrapper around read_status will take care that the status will be the given value,
|
|
for at least the given delay. This does NOT cover the case when self.status is set
|
|
directly from an other method.
|
|
"""
|
|
__freeze_status_until = 0
|
|
|
|
def __init_subclass__(cls):
|
|
def wrapped(self, inner=cls.read_status):
|
|
if time.time() < self.__freeze_status_until:
|
|
return Done
|
|
return inner(self)
|
|
|
|
cls.read_status = wrapped
|
|
super().__init_subclass__()
|
|
|
|
def freeze_status(self, delay, code=BUSY, text='changed target'):
|
|
"""freezze status to the given value for the given delay"""
|
|
self.__freeze_status_until = time.time() + delay
|
|
self.status = code, text
|
|
|
|
|
|
class Axis(HasTargetLimits, FreezeStatus, Drivable):
|
|
axis = Property('axis number', IntRange(0, 2), 0)
|
|
value = Parameter('axis position', FloatRange(unit='deg'))
|
|
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, default=1, initwrite=True)
|
|
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())
|
|
|
|
_hw = Positioner()
|
|
_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}
|
|
_direction = 1 # move direction
|
|
_idle_status = IDLE, ''
|
|
_error_state = '' # empty string: no error
|
|
_history = None
|
|
_check_sensor = False
|
|
_try_count = 0
|
|
|
|
def __init__(self, name, logger, opts, srv):
|
|
unit = opts.pop('unit', 'deg')
|
|
opts['value.unit'] = unit
|
|
try:
|
|
self._scale = self.SCALES[unit] * opts.get('gear', 1)
|
|
except KeyError as e:
|
|
raise ConfigError('unsupported unit: %s' % unit)
|
|
super().__init__(name, logger, opts, srv)
|
|
|
|
def write_gear(self, value):
|
|
self._scale = self.SCALES[self.parameters['value'].datatype.unit] * self.gear
|
|
return value
|
|
|
|
def startModule(self, start_events):
|
|
super().startModule(start_events)
|
|
start_events.queue(self.read_info)
|
|
|
|
def check_value(self, value):
|
|
"""check if value allows moving in current direction"""
|
|
if self._direction > 0:
|
|
if value > self.target_limits[1]:
|
|
raise BadValueError('above upper limit')
|
|
elif value < self.target_limits[0]:
|
|
raise BadValueError('below lower limit')
|
|
|
|
def read_value(self):
|
|
pos = self._hw.getPosition(self.axis) * self._scale
|
|
if self.isBusy():
|
|
try:
|
|
self.check_value(pos)
|
|
except BadValueError as e:
|
|
self._stop()
|
|
self._idle_status = ERROR, str(e)
|
|
return pos
|
|
|
|
def read_frequency(self):
|
|
return self._hw.getFrequency(self.axis)
|
|
|
|
def write_frequency(self, value):
|
|
self._hw.setFrequency(self.axis, value)
|
|
return self._hw.getFrequency(self.axis)
|
|
|
|
def read_amplitude(self):
|
|
return self._hw.getAmplitude(self.axis)
|
|
|
|
def write_amplitude(self, value):
|
|
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)
|
|
return value
|
|
|
|
def read_status(self):
|
|
statusbits = self._hw.getAxisStatus(self.axis)
|
|
sensor, self.output, moving, attarget, eot_fwd, eot_bwd, sensor_error = statusbits
|
|
self.statusbits = ''.join((k for k, v in zip('SOMTFBE', statusbits) if v))
|
|
if self._move_steps:
|
|
if not (eot_fwd or eot_bwd):
|
|
return BUSY, 'moving by steps'
|
|
if not sensor:
|
|
self._error_state = 'no sensor connected'
|
|
elif sensor_error:
|
|
self._error_state = 'sensor error'
|
|
elif eot_fwd:
|
|
self._error_state = 'end of travel forward'
|
|
elif eot_bwd:
|
|
self._error_state = 'end of travel backward'
|
|
else:
|
|
if self._error_state and not DIRECTION_NAME[self._direction] in self._error_state:
|
|
self._error_state = ''
|
|
status_text = 'moving' if self._try_count == 0 else 'moving (retry %d)' % self._try_count
|
|
if moving and self._history is not None: # history None: moving by steps
|
|
self._history.append(self.value)
|
|
if len(self._history) < 5:
|
|
return BUSY, status_text
|
|
beg = self._history.pop(0)
|
|
if abs(beg - self.target) < self.tolerance:
|
|
# reset normal tolerance
|
|
self._stop()
|
|
self._idle_status = IDLE, 'in tolerance'
|
|
return self._idle_status
|
|
# self._hw.setTargetRange(self.axis, self.tolerance / self._scale)
|
|
if (self.value - beg) * self._direction > 0:
|
|
return BUSY, status_text
|
|
self._try_count += 1
|
|
if self._try_count < 10:
|
|
self.log.warn('no progress retry %d', self._try_count)
|
|
return BUSY, status_text
|
|
self._idle_status = WARN, 'no progress'
|
|
if self._error_state:
|
|
self._try_count += 1
|
|
if self._try_count < 10 and self._history is not None:
|
|
self.log.warn('end of travel retry %d', self._try_count)
|
|
self.write_target(self.target)
|
|
return Done
|
|
self._idle_status = WARN, self._error_state
|
|
if self.status[0] != IDLE:
|
|
self._stop()
|
|
return self._idle_status
|
|
|
|
def write_target(self, value):
|
|
if value == self.read_value():
|
|
return value
|
|
self.check_limits(value)
|
|
self._try_count = 0
|
|
self._direction = 1 if value > self.value else -1
|
|
# if self._error_state and DIRECTION_NAME[-self._direction] not in self._error_state:
|
|
# raise BadValueError('can not move (%s)' % self._error_state)
|
|
self._move_steps = 0
|
|
self.write_output(1)
|
|
# try first with 50 % of tolerance
|
|
self._hw.setTargetRange(self.axis, self.tolerance * 0.5 / self._scale)
|
|
for itry in range(5):
|
|
try:
|
|
self._hw.setTargetPosition(self.axis, value / self._scale)
|
|
self._hw.startAutoMove(self.axis, enable=1, relative=0)
|
|
except Exception as e:
|
|
if itry == 4:
|
|
raise
|
|
self.log.warn('%r', e)
|
|
self._history = [self.value]
|
|
self._idle_status = IDLE, ''
|
|
self.freeze_status(1, BUSY, 'changed target')
|
|
self.setFastPoll(True, 1)
|
|
return value
|
|
|
|
def doPoll(self):
|
|
if self._move_steps == 0:
|
|
super().doPoll()
|
|
return
|
|
self._hw.startSingleStep(self.axis, self._direction < 0)
|
|
self._move_steps -= self._direction
|
|
if self._move_steps % int(self.frequency) == 0: # poll value and status every second
|
|
super().doPoll()
|
|
|
|
@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)
|
|
|
|
def _stop(self):
|
|
self._move_steps = 0
|
|
self._history = None
|
|
for _ in range(5):
|
|
try:
|
|
self._hw.startAutoMove(self.axis, enable=0, relative=0)
|
|
break
|
|
except Exception as e:
|
|
if itry == 4:
|
|
raise
|
|
self.log.warn('%r', e)
|
|
self._hw.setTargetRange(self.axis, self.tolerance / self._scale)
|
|
self.setFastPoll(False)
|
|
|
|
@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)
|
|
self._history = None
|
|
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)
|