add missing files from secop_psi
- all of them have to be checked! Change-Id: I89d55ca683d0b2710222f14c2c3cd42f8fbf3a1f
This commit is contained in:
254
frappy_psi/phytron.py
Normal file
254
frappy_psi/phytron.py
Normal file
@ -0,0 +1,254 @@
|
||||
# -*- 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:
|
||||
# Markus Zolliker <markus.zolliker@psi.ch>
|
||||
#
|
||||
# *****************************************************************************
|
||||
|
||||
"""driver for phytron motors"""
|
||||
|
||||
from frappy.core import Done, Command, EnumType, FloatRange, IntRange, \
|
||||
HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, \
|
||||
StringIO, StringType, TupleOf
|
||||
from frappy.errors import CommunicationFailedError, HardwareError, BadValueError
|
||||
from frappy.lib import clamp
|
||||
|
||||
|
||||
class PhytronIO(StringIO):
|
||||
end_of_line = '\x03' # ETX
|
||||
timeout = 0.5
|
||||
identification = [('0IVR', 'MCC Minilog .*')]
|
||||
|
||||
def communicate(self, command):
|
||||
ntry = 5
|
||||
warn = None
|
||||
for itry in range(ntry):
|
||||
try:
|
||||
_, _, reply = super().communicate('\x02' + command).partition('\x02')
|
||||
if reply[0] == '\x06': # ACK
|
||||
break
|
||||
raise CommunicationFailedError('missing ACK %r (cmd: %r)'
|
||||
% (reply, command))
|
||||
except Exception as e:
|
||||
if itry < ntry - 1:
|
||||
warn = e
|
||||
else:
|
||||
raise
|
||||
if warn:
|
||||
self.log.warning('needed %d retries after %r', itry, warn)
|
||||
return reply[1:]
|
||||
|
||||
|
||||
class Motor(PersistentMixin, HasIO, Drivable):
|
||||
axis = Property('motor axis X or Y', StringType(), default='X')
|
||||
address = Property('address', IntRange(0, 15), default=0)
|
||||
|
||||
encoder_mode = Parameter('how to treat the encoder', EnumType('encoder', NO=0, READ=1, CHECK=2),
|
||||
default=1, readonly=False)
|
||||
value = Parameter('angle', FloatRange(unit='deg'))
|
||||
target = Parameter('target angle', FloatRange(unit='deg'), readonly=False)
|
||||
speed = Parameter('', FloatRange(0, 20, unit='deg/s'), readonly=False)
|
||||
accel = Parameter('', FloatRange(2, 250, unit='deg/s/s'), readonly=False)
|
||||
encoder_tolerance = Parameter('', FloatRange(unit='deg'), readonly=False, default=0.01)
|
||||
offset = PersistentParam('', FloatRange(unit='deg'), readonly=False, default=0)
|
||||
sign = PersistentParam('', IntRange(-1,1), readonly=False, default=1)
|
||||
encoder = Parameter('encoder reading', FloatRange(unit='deg'))
|
||||
backlash = Parameter("""backlash compensation\n
|
||||
offset for always approaching from the same side""",
|
||||
FloatRange(unit='deg'), readonly=False, default=0)
|
||||
abslimits = Parameter('abs limits (raw values)', default=(0, 0),
|
||||
datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg')))
|
||||
userlimits = PersistentParam('user limits', readonly=False, default=(0, 0), initwrite=True,
|
||||
datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg')))
|
||||
|
||||
ioClass = PhytronIO
|
||||
fast_poll = 0.1
|
||||
_backlash_pending = False
|
||||
_mismatch_count = 0
|
||||
_rawlimits = None
|
||||
_step_size = None # degree / step
|
||||
|
||||
def earlyInit(self):
|
||||
super().earlyInit()
|
||||
if self.abslimits == (0, 0):
|
||||
self.abslimits = -9e99, 9e99
|
||||
if self.userlimits == (0, 0):
|
||||
self._rawlimits = self.abslimits
|
||||
self.read_userlimits()
|
||||
self.loadParameters()
|
||||
|
||||
def get(self, cmd):
|
||||
return self.communicate('%x%s%s' % (self.address, self.axis, cmd))
|
||||
|
||||
def set(self, cmd, value):
|
||||
# make sure e format is not used, max 8 characters
|
||||
strvalue = '%.6g' % value
|
||||
if 'e' in strvalue:
|
||||
if abs(value) <= 1: # very small number
|
||||
strvalue = '%.7f' % value
|
||||
elif abs(value) < 99999999: # big number
|
||||
strvalue = '%.0f' % value
|
||||
else:
|
||||
raise ValueError('number (%g) must not have more than 8 digits' % value)
|
||||
self.communicate('%x%s%s%s' % (self.address, self.axis, cmd, strvalue))
|
||||
|
||||
def set_get(self, cmd, value, query):
|
||||
self.set(cmd, value)
|
||||
return self.get(query)
|
||||
|
||||
def read_value(self):
|
||||
prev_enc = self.encoder
|
||||
pos = float(self.get('P20R')) * self.sign - self.offset
|
||||
if self.encoder_mode != 'NO':
|
||||
enc = self.read_encoder()
|
||||
else:
|
||||
enc = pos
|
||||
status = self.communicate('%xSE' % self.address)
|
||||
status = status[0:4] if self.axis == 'X' else status[4:8]
|
||||
self.log.debug('run %s enc %s end %s', status[1], status[2], status[3])
|
||||
status = self.get('=H')
|
||||
if status == 'N':
|
||||
if self.encoder_mode == 'CHECK':
|
||||
e1, e2 = sorted((prev_enc, enc))
|
||||
if e1 - self.encoder_tolerance <= pos <= e2 + self.encoder_tolerance:
|
||||
self.status = self.Status.BUSY, 'driving'
|
||||
else:
|
||||
self.log.error('encoder lag: %g not within %g..%g',
|
||||
pos, e1, e2)
|
||||
self.get('S') # stop
|
||||
self.status = self.Status.ERROR, 'encoder lag error'
|
||||
self.setFastPoll(False)
|
||||
else:
|
||||
self.status = self.Status.BUSY, 'driving'
|
||||
else:
|
||||
if self._backlash_pending:
|
||||
# drive to real target
|
||||
self.set('A', self.sign * (self.target + self.offset))
|
||||
self._backlash_pending = False
|
||||
return pos
|
||||
if (self.encoder_mode == 'CHECK' and
|
||||
abs(enc - pos) > self.encoder_tolerance):
|
||||
if self._mismatch_count < 2:
|
||||
self._mismatch_count += 1
|
||||
else:
|
||||
self.log.error('encoder mismatch: abs(%g - %g) < %g',
|
||||
enc, pos, self.encoder_tolerance)
|
||||
self.status = self.Status.ERROR, 'encoder does not match pos'
|
||||
else:
|
||||
self._mismatch_count = 0
|
||||
self.status = self.Status.IDLE, ''
|
||||
self.setFastPoll(False)
|
||||
return pos
|
||||
|
||||
def read_encoder(self):
|
||||
if self.encoder_mode == 'NO':
|
||||
return self.value
|
||||
return float(self.get('P22R')) * self.sign - self.offset
|
||||
|
||||
def write_sign(self, value):
|
||||
self.sign = value
|
||||
self.saveParameters()
|
||||
return Done
|
||||
|
||||
def get_step_size(self):
|
||||
self._step_size = float(self.get('P03R'))
|
||||
|
||||
def read_speed(self):
|
||||
if self._step_size is None:
|
||||
# avoid repeatedly reading step size, as this is polled and will not change
|
||||
self.get_step_size()
|
||||
return float(self.get('P14R')) * self._step_size
|
||||
|
||||
def write_speed(self, value):
|
||||
self.get_step_size() # read step size anyway, it does not harm
|
||||
return float(self.set_get('P14S', round(value / self._step_size), 'P14R')) * self._step_size
|
||||
|
||||
def read_accel(self):
|
||||
if not self._step_size:
|
||||
self.get_step_size()
|
||||
return float(self.get('P15R')) * self._step_size
|
||||
|
||||
def write_accel(self, value):
|
||||
self.get_step_size()
|
||||
return float(self.set_get('P15S', round(value / self._step_size), 'P15R')) * self._step_size
|
||||
|
||||
def _check_limits(self, *values):
|
||||
for name, (mn, mx) in ('user', self._rawlimits), ('abs', self.abslimits):
|
||||
mn -= self.offset
|
||||
mx -= self.offset
|
||||
for v in values:
|
||||
if not mn <= v <= mx:
|
||||
raise BadValueError('%s limits violation: %g <= %g <= %g' % (name, mn, v, mx))
|
||||
v += self.offset
|
||||
|
||||
def write_target(self, value):
|
||||
if self.status[0] == self.Status.ERROR:
|
||||
raise HardwareError('need reset')
|
||||
self.status = self.Status.BUSY, 'changed target'
|
||||
self._check_limits(value, value + self.backlash)
|
||||
if self.backlash:
|
||||
# drive first to target + backlash
|
||||
# we do not optimize when already driving from the right side
|
||||
self._backlash_pending = True
|
||||
self.set('A', self.sign * (value + self.offset + self.backlash))
|
||||
else:
|
||||
self.set('A', self.sign * (value + self.offset))
|
||||
self.setFastPoll(True, self.fast_poll)
|
||||
return value
|
||||
|
||||
def read_userlimits(self):
|
||||
return self._rawlimits[0] - self.offset, self._rawlimits[1] - self.offset
|
||||
|
||||
def write_userlimits(self, value):
|
||||
self._rawlimits = [clamp(self.abslimits[0], v + self.offset, self.abslimits[1]) for v in value]
|
||||
value = self.read_userlimits()
|
||||
self.saveParameters()
|
||||
return value
|
||||
|
||||
def write_offset(self, value):
|
||||
self.offset = value
|
||||
self.read_userlimits()
|
||||
self.saveParameters()
|
||||
return Done
|
||||
|
||||
def stop(self):
|
||||
self.get('S')
|
||||
|
||||
@Command
|
||||
def reset(self):
|
||||
"""Reset error, set position to encoder"""
|
||||
self.read_value()
|
||||
if self.status[0] == self.Status.ERROR:
|
||||
enc = self.encoder + self.offset
|
||||
pos = self.value + self.offset
|
||||
if abs(enc - pos) > self.encoder_tolerance:
|
||||
if enc < 0:
|
||||
# assume we have a rotation (not a linear motor)
|
||||
while enc < 0:
|
||||
self.offset += 360
|
||||
enc += 360
|
||||
self.set('P22S', enc * self.sign)
|
||||
self.saveParameters()
|
||||
self.set('P20S', enc * self.sign) # set pos to encoder
|
||||
self.read_value()
|
||||
# self.status = self.Status.IDLE, ''
|
||||
|
||||
# TODO:
|
||||
# '=E' electronics status
|
||||
# '=I+' / '=I-': limit switches
|
||||
# use P37 to determine if restarted
|
||||
Reference in New Issue
Block a user