trinamic driver and bytesio module
Change-Id: Id634e7514fecab6fd6bc3edf81e25ad41c2bb12f
This commit is contained in:
parent
c6157049d7
commit
49ad153605
90
secop/bytesio.py
Normal file
90
secop/bytesio.py
Normal file
@ -0,0 +1,90 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
# -*- 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>
|
||||||
|
# *****************************************************************************
|
||||||
|
"""byte oriented stream communication"""
|
||||||
|
|
||||||
|
import time
|
||||||
|
import re
|
||||||
|
|
||||||
|
from secop.lib.asynconn import AsynConn, ConnectionClosed
|
||||||
|
from secop.modules import Property, Command
|
||||||
|
from secop.stringio import BaseIO
|
||||||
|
from secop.datatypes import BLOBType, IntRange, ArrayOf, TupleOf, StringType
|
||||||
|
from secop.errors import CommunicationFailedError, CommunicationSilentError
|
||||||
|
|
||||||
|
|
||||||
|
HEX_CODE = re.compile(r'[0-9a-fA-F][0-9a-fA-F]$')
|
||||||
|
|
||||||
|
|
||||||
|
class BytesIO(BaseIO):
|
||||||
|
identification = Property(
|
||||||
|
"""identification
|
||||||
|
|
||||||
|
a list of tuples with commands and expected responses, to be sent on connect.
|
||||||
|
commands and responses are whitespace separated items
|
||||||
|
an item is either:
|
||||||
|
- a two digit hexadecimal number (byte value)
|
||||||
|
- a character
|
||||||
|
- ?? indicating ignored bytes in responses
|
||||||
|
""", datatype=ArrayOf(TupleOf(StringType(), StringType())), default=[], export=False)
|
||||||
|
|
||||||
|
def connectStart(self):
|
||||||
|
if not self.is_connected:
|
||||||
|
uri = self.uri
|
||||||
|
self._conn = AsynConn(uri, b'')
|
||||||
|
self.is_connected = True
|
||||||
|
for command, match in self.identification:
|
||||||
|
cmdbytes = bytes([int(c, 16) if HEX_CODE.match(c) else ord(c) for c in command.split()])
|
||||||
|
replypat = [int(c, 16) if HEX_CODE.match(c.replace('??', '-1')) else ord(c) for c in command.split()]
|
||||||
|
reply = self.communicate(cmdbytes, len(replypat))
|
||||||
|
if any(b != c and c != -1 for b, c in zip(reply, replypat)):
|
||||||
|
self.closeConnection()
|
||||||
|
raise CommunicationFailedError('bad response: %r does not match %r' % (command, match))
|
||||||
|
|
||||||
|
@Command((BLOBType(), IntRange(0)), result=BLOBType())
|
||||||
|
def communicate(self, command, nbytes):
|
||||||
|
"""send a command and receive nbytes as reply"""
|
||||||
|
if not self.is_connected:
|
||||||
|
self.read_is_connected() # try to reconnect
|
||||||
|
if not self._conn:
|
||||||
|
raise CommunicationSilentError('can not connect to %r' % self.uri)
|
||||||
|
try:
|
||||||
|
with self._lock:
|
||||||
|
# read garbage and wait before send
|
||||||
|
try:
|
||||||
|
if self.wait_before:
|
||||||
|
time.sleep(self.wait_before)
|
||||||
|
garbage = self._conn.flush_recv()
|
||||||
|
if garbage:
|
||||||
|
self.log.debug('garbage: %r', garbage)
|
||||||
|
self._conn.send(command)
|
||||||
|
self.log.debug('send: %r', command)
|
||||||
|
reply = self._conn.readbytes(nbytes, self.timeout)
|
||||||
|
except ConnectionClosed:
|
||||||
|
self.closeConnection()
|
||||||
|
raise CommunicationFailedError('disconnected')
|
||||||
|
self.log.debug('recv: %r', reply)
|
||||||
|
return reply
|
||||||
|
except Exception as e:
|
||||||
|
if str(e) == self._last_error:
|
||||||
|
raise CommunicationSilentError(str(e))
|
||||||
|
self._last_error = str(e)
|
||||||
|
self.log.error(self._last_error)
|
||||||
|
raise
|
@ -79,8 +79,8 @@ class AsynConn:
|
|||||||
self.disconnect()
|
self.disconnect()
|
||||||
|
|
||||||
@classmethod
|
@classmethod
|
||||||
def register_scheme(cls, scheme):
|
def __init_subclass__(cls):
|
||||||
cls.SCHEME_MAP[scheme] = cls
|
cls.SCHEME_MAP[cls.scheme] = cls
|
||||||
|
|
||||||
def disconnect(self):
|
def disconnect(self):
|
||||||
raise NotImplementedError
|
raise NotImplementedError
|
||||||
@ -154,6 +154,8 @@ class AsynConn:
|
|||||||
|
|
||||||
|
|
||||||
class AsynTcp(AsynConn):
|
class AsynTcp(AsynConn):
|
||||||
|
scheme = 'tcp'
|
||||||
|
|
||||||
def __init__(self, uri, *args, **kwargs):
|
def __init__(self, uri, *args, **kwargs):
|
||||||
super().__init__(uri, *args, **kwargs)
|
super().__init__(uri, *args, **kwargs)
|
||||||
self.uri = uri
|
self.uri = uri
|
||||||
@ -202,9 +204,6 @@ class AsynTcp(AsynConn):
|
|||||||
raise ConnectionClosed() # marks end of connection
|
raise ConnectionClosed() # marks end of connection
|
||||||
|
|
||||||
|
|
||||||
AsynTcp.register_scheme('tcp')
|
|
||||||
|
|
||||||
|
|
||||||
class AsynSerial(AsynConn):
|
class AsynSerial(AsynConn):
|
||||||
"""a serial connection using pyserial
|
"""a serial connection using pyserial
|
||||||
|
|
||||||
@ -221,6 +220,7 @@ class AsynSerial(AsynConn):
|
|||||||
|
|
||||||
and others (see documentation of serial.Serial)
|
and others (see documentation of serial.Serial)
|
||||||
"""
|
"""
|
||||||
|
scheme = 'serial'
|
||||||
PARITY_NAMES = {name[0]: name for name in ['NONE', 'ODD', 'EVEN', 'MASK', 'SPACE']}
|
PARITY_NAMES = {name[0]: name for name in ['NONE', 'ODD', 'EVEN', 'MASK', 'SPACE']}
|
||||||
|
|
||||||
def __init__(self, uri, *args, **kwargs):
|
def __init__(self, uri, *args, **kwargs):
|
||||||
@ -282,6 +282,3 @@ class AsynSerial(AsynConn):
|
|||||||
return self.connection.read(n)
|
return self.connection.read(n)
|
||||||
data = self.connection.read(1)
|
data = self.connection.read(1)
|
||||||
return data + self.connection.read(self.connection.in_waiting)
|
return data + self.connection.read(self.connection.in_waiting)
|
||||||
|
|
||||||
|
|
||||||
AsynSerial.register_scheme('serial')
|
|
||||||
|
@ -20,7 +20,8 @@
|
|||||||
# *****************************************************************************
|
# *****************************************************************************
|
||||||
"""Line oriented stream communication
|
"""Line oriented stream communication
|
||||||
|
|
||||||
implements TCP/IP and is be used as a base for SerialIO
|
StringIO: string oriented IO. May be used for TCP/IP as well for serial IO or
|
||||||
|
other future extensions of AsynConn
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import re
|
import re
|
||||||
@ -37,66 +38,23 @@ from secop.modules import Attached, Command, \
|
|||||||
from secop.poller import REGULAR
|
from secop.poller import REGULAR
|
||||||
|
|
||||||
|
|
||||||
class StringIO(Communicator):
|
class BaseIO(Communicator):
|
||||||
"""line oriented communicator
|
"""base of StringIO and BytesIO"""
|
||||||
|
|
||||||
self healing is assured by polling the parameter 'is_connected'
|
|
||||||
"""
|
|
||||||
uri = Property('hostname:portnumber', datatype=StringType())
|
uri = Property('hostname:portnumber', datatype=StringType())
|
||||||
end_of_line = Property('end_of_line character', datatype=ValueType(),
|
|
||||||
default='\n', settable=True)
|
|
||||||
encoding = Property('used encoding', datatype=StringType(),
|
|
||||||
default='ascii', settable=True)
|
|
||||||
identification = Property('''
|
|
||||||
identification
|
|
||||||
|
|
||||||
a list of tuples with commands and expected responses as regexp,
|
|
||||||
to be sent on connect''',
|
|
||||||
datatype=ArrayOf(TupleOf(StringType(), StringType())), default=[], export=False)
|
|
||||||
|
|
||||||
timeout = Parameter('timeout', datatype=FloatRange(0), default=2)
|
timeout = Parameter('timeout', datatype=FloatRange(0), default=2)
|
||||||
wait_before = Parameter('wait time before sending', datatype=FloatRange(), default=0)
|
wait_before = Parameter('wait time before sending', datatype=FloatRange(), default=0)
|
||||||
is_connected = Parameter('connection state', datatype=BoolType(), readonly=False, poll=REGULAR)
|
is_connected = Parameter('connection state', datatype=BoolType(), readonly=False, poll=REGULAR)
|
||||||
pollinterval = Parameter('reconnect interval', datatype=FloatRange(0), readonly=False, default=10)
|
pollinterval = Parameter('reconnect interval', datatype=FloatRange(0), readonly=False, default=10)
|
||||||
|
|
||||||
_reconnectCallbacks = None
|
_reconnectCallbacks = None
|
||||||
|
_conn = None
|
||||||
|
_last_error = None
|
||||||
|
|
||||||
def earlyInit(self):
|
def earlyInit(self):
|
||||||
self._conn = None
|
|
||||||
self._lock = threading.RLock()
|
self._lock = threading.RLock()
|
||||||
eol = self.end_of_line
|
|
||||||
if isinstance(eol, (tuple, list)):
|
|
||||||
if len(eol) not in (1, 2):
|
|
||||||
raise ValueError('invalid end_of_line: %s' % eol)
|
|
||||||
else:
|
|
||||||
eol = [eol]
|
|
||||||
# eol for read and write might be distinct
|
|
||||||
self._eol_read = self._convert_eol(eol[0])
|
|
||||||
if not self._eol_read:
|
|
||||||
raise ValueError('end_of_line for read must not be empty')
|
|
||||||
self._eol_write = self._convert_eol(eol[-1])
|
|
||||||
self._last_error = None
|
|
||||||
|
|
||||||
def _convert_eol(self, value):
|
|
||||||
if isinstance(value, str):
|
|
||||||
return value.encode(self.encoding)
|
|
||||||
if isinstance(value, int):
|
|
||||||
return bytes([value])
|
|
||||||
if isinstance(value, bytes):
|
|
||||||
return value
|
|
||||||
raise ValueError('invalid end_of_line: %s' % repr(value))
|
|
||||||
|
|
||||||
def connectStart(self):
|
def connectStart(self):
|
||||||
if not self.is_connected:
|
raise NotImplementedError
|
||||||
uri = self.uri
|
|
||||||
self._conn = AsynConn(uri, self._eol_read)
|
|
||||||
self.is_connected = True
|
|
||||||
for command, regexp in self.identification:
|
|
||||||
reply = self.communicate(command)
|
|
||||||
if not re.match(regexp, reply):
|
|
||||||
self.closeConnection()
|
|
||||||
raise CommunicationFailedError('bad response: %s does not match %s' %
|
|
||||||
(reply, regexp))
|
|
||||||
|
|
||||||
def closeConnection(self):
|
def closeConnection(self):
|
||||||
"""close connection
|
"""close connection
|
||||||
@ -158,6 +116,62 @@ class StringIO(Communicator):
|
|||||||
if removeme:
|
if removeme:
|
||||||
self._reconnectCallbacks.pop(key)
|
self._reconnectCallbacks.pop(key)
|
||||||
|
|
||||||
|
def communicate(self, command):
|
||||||
|
return NotImplementedError
|
||||||
|
|
||||||
|
|
||||||
|
class StringIO(BaseIO):
|
||||||
|
"""line oriented communicator
|
||||||
|
|
||||||
|
self healing is assured by polling the parameter 'is_connected'
|
||||||
|
"""
|
||||||
|
end_of_line = Property('end_of_line character', datatype=ValueType(),
|
||||||
|
default='\n', settable=True)
|
||||||
|
encoding = Property('used encoding', datatype=StringType(),
|
||||||
|
default='ascii', settable=True)
|
||||||
|
identification = Property('''
|
||||||
|
identification
|
||||||
|
|
||||||
|
a list of tuples with commands and expected responses as regexp,
|
||||||
|
to be sent on connect''',
|
||||||
|
datatype=ArrayOf(TupleOf(StringType(), StringType())), default=[], export=False)
|
||||||
|
|
||||||
|
def _convert_eol(self, value):
|
||||||
|
if isinstance(value, str):
|
||||||
|
return value.encode(self.encoding)
|
||||||
|
if isinstance(value, int):
|
||||||
|
return bytes([value])
|
||||||
|
if isinstance(value, bytes):
|
||||||
|
return value
|
||||||
|
raise ValueError('invalid end_of_line: %s' % repr(value))
|
||||||
|
|
||||||
|
def earlyInit(self):
|
||||||
|
super().earlyInit()
|
||||||
|
eol = self.end_of_line
|
||||||
|
if isinstance(eol, (tuple, list)):
|
||||||
|
if len(eol) not in (1, 2):
|
||||||
|
raise ValueError('invalid end_of_line: %s' % eol)
|
||||||
|
else:
|
||||||
|
eol = [eol]
|
||||||
|
# eol for read and write might be distinct
|
||||||
|
self._eol_read = self._convert_eol(eol[0])
|
||||||
|
if not self._eol_read:
|
||||||
|
raise ValueError('end_of_line for read must not be empty')
|
||||||
|
self._eol_write = self._convert_eol(eol[-1])
|
||||||
|
|
||||||
|
def connectStart(self):
|
||||||
|
if not self.is_connected:
|
||||||
|
uri = self.uri
|
||||||
|
self._conn = AsynConn(uri, self._eol_read)
|
||||||
|
self.is_connected = True
|
||||||
|
for command, regexp in self.identification:
|
||||||
|
reply = self.communicate(command)
|
||||||
|
if not re.match(regexp, reply):
|
||||||
|
self.closeConnection()
|
||||||
|
raise CommunicationFailedError('bad response: %s does not match %s' %
|
||||||
|
(reply, regexp))
|
||||||
|
|
||||||
|
@Command(StringType(), result=StringType())
|
||||||
def communicate(self, command):
|
def communicate(self, command):
|
||||||
"""send a command and receive a reply
|
"""send a command and receive a reply
|
||||||
|
|
||||||
@ -165,6 +179,7 @@ class StringIO(Communicator):
|
|||||||
for commands without reply, the command must be joined with a query command,
|
for commands without reply, the command must be joined with a query command,
|
||||||
wait_before is respected for end_of_lines within a command.
|
wait_before is respected for end_of_lines within a command.
|
||||||
"""
|
"""
|
||||||
|
command = command.encode(self.encoding)
|
||||||
if not self.is_connected:
|
if not self.is_connected:
|
||||||
self.read_is_connected() # try to reconnect
|
self.read_is_connected() # try to reconnect
|
||||||
if not self._conn:
|
if not self._conn:
|
||||||
@ -173,9 +188,9 @@ class StringIO(Communicator):
|
|||||||
with self._lock:
|
with self._lock:
|
||||||
# read garbage and wait before send
|
# read garbage and wait before send
|
||||||
if self.wait_before and self._eol_write:
|
if self.wait_before and self._eol_write:
|
||||||
cmds = command.encode(self.encoding).split(self._eol_write)
|
cmds = command.split(self._eol_write)
|
||||||
else:
|
else:
|
||||||
cmds = [command.encode(self.encoding)]
|
cmds = [command]
|
||||||
garbage = None
|
garbage = None
|
||||||
try:
|
try:
|
||||||
for cmd in cmds:
|
for cmd in cmds:
|
||||||
@ -186,6 +201,7 @@ class StringIO(Communicator):
|
|||||||
if garbage:
|
if garbage:
|
||||||
self.log.debug('garbage: %r', garbage)
|
self.log.debug('garbage: %r', garbage)
|
||||||
self._conn.send(cmd + self._eol_write)
|
self._conn.send(cmd + self._eol_write)
|
||||||
|
self.log.debug('send: %s', cmd + self._eol_write)
|
||||||
reply = self._conn.readline(self.timeout)
|
reply = self._conn.readline(self.timeout)
|
||||||
except ConnectionClosed:
|
except ConnectionClosed:
|
||||||
self.closeConnection()
|
self.closeConnection()
|
||||||
|
347
secop_psi/trinamic.py
Normal file
347
secop_psi/trinamic.py
Normal file
@ -0,0 +1,347 @@
|
|||||||
|
# -*- 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>
|
||||||
|
#
|
||||||
|
# *****************************************************************************
|
||||||
|
|
||||||
|
"""drivers for trinamic PD-1161 motors"""
|
||||||
|
|
||||||
|
import time
|
||||||
|
import os
|
||||||
|
import struct
|
||||||
|
from secop.core import BoolType, Command, EnumType, FloatRange, IntRange, \
|
||||||
|
HasIodev, Parameter, Property, Drivable, TupleOf, Done
|
||||||
|
from secop.bytesio import BytesIO
|
||||||
|
from secop.lib import getGeneralConfig
|
||||||
|
from secop.errors import CommunicationFailedError, HardwareError
|
||||||
|
|
||||||
|
MOTOR_STOP = 3
|
||||||
|
MOVE = 4
|
||||||
|
SET_AXIS_PAR = 5
|
||||||
|
GET_AXIS_PAR = 6
|
||||||
|
SAVE_AXIS_PAR = 7
|
||||||
|
LOAD_AXIS_PAR = 8
|
||||||
|
SET_GLOB_PAR = 9
|
||||||
|
GET_GLOB_PAR = 10
|
||||||
|
SAVE_GLOB_PAR = 11
|
||||||
|
LOAD_GLOB_PAR = 12
|
||||||
|
SET_IO = 14
|
||||||
|
GET_IO = 15
|
||||||
|
|
||||||
|
MOT_FACT = object()
|
||||||
|
|
||||||
|
CONFIG = getGeneralConfig()
|
||||||
|
|
||||||
|
|
||||||
|
class AxisPar(Parameter):
|
||||||
|
SET = SET_AXIS_PAR
|
||||||
|
GET = GET_AXIS_PAR
|
||||||
|
|
||||||
|
def __init__(self, adr, min=None, max=None, unit=None, scale=1, readonly=False, poll=True):
|
||||||
|
if unit:
|
||||||
|
datatype = FloatRange(min, max, unit=unit)
|
||||||
|
else:
|
||||||
|
datatype = IntRange(min, max)
|
||||||
|
super().__init__('_', datatype, readonly=readonly, poll=poll)
|
||||||
|
self.adr = adr
|
||||||
|
self.scale = scale
|
||||||
|
|
||||||
|
def to_raw(self, motor, value):
|
||||||
|
return round(value / self.scale)
|
||||||
|
|
||||||
|
def from_raw(self, motor, value):
|
||||||
|
return value * self.scale
|
||||||
|
|
||||||
|
def read(self, motor):
|
||||||
|
return round(self.from_raw(motor, motor.comm(self.GET, self.adr)), 3)
|
||||||
|
|
||||||
|
def write(self, motor, value):
|
||||||
|
rvalue = self.to_raw(motor, value)
|
||||||
|
motor.comm(self.SET, self.adr, rvalue)
|
||||||
|
reply = motor.comm(self.GET, self.adr)
|
||||||
|
if rvalue != reply:
|
||||||
|
raise HardwareError('%s: reply does not match: %r != %r' % (self.name, rvalue, reply))
|
||||||
|
return round(self.from_raw(motor,reply), 3)
|
||||||
|
|
||||||
|
def __set_name__(self, owner, name):
|
||||||
|
super().__set_name__(owner, name)
|
||||||
|
setattr(owner, 'read_' + name, lambda self, pobj=self: pobj.read(self))
|
||||||
|
|
||||||
|
if not self.readonly:
|
||||||
|
|
||||||
|
setattr(owner, 'write_' + name, lambda self, value, pobj=self: pobj.write(self, value))
|
||||||
|
|
||||||
|
|
||||||
|
class IoPar(AxisPar):
|
||||||
|
SET = SET_IO
|
||||||
|
GET = GET_IO
|
||||||
|
|
||||||
|
def __init__(self, adr, bank, datatype=EnumType(CLOSED=0, OPEN=1), scale=1, readonly=False, poll=True):
|
||||||
|
Parameter.__init__(self, '_', datatype, readonly=readonly, poll=poll)
|
||||||
|
self.adr = adr + 1000 * bank
|
||||||
|
self.scale = scale
|
||||||
|
|
||||||
|
def read(self, motor):
|
||||||
|
return motor.comm(self.GET, self.adr)
|
||||||
|
|
||||||
|
def write(self, motor, value):
|
||||||
|
motor.comm(self.SET, self.adr, int(value))
|
||||||
|
return value
|
||||||
|
|
||||||
|
|
||||||
|
class AxisDiffPar(AxisPar):
|
||||||
|
@staticmethod
|
||||||
|
def to_raw(motor, value):
|
||||||
|
return round(value / motor.fact)
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def from_raw(motor, value):
|
||||||
|
return round(value * motor.fact, 3)
|
||||||
|
|
||||||
|
|
||||||
|
class AxisZeroPar(AxisPar):
|
||||||
|
@staticmethod
|
||||||
|
def to_raw(motor, value):
|
||||||
|
return round(value / motor.fact - motor.zero)
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def from_raw(motor, value):
|
||||||
|
return round(value * motor.fact + motor.zero, 3)
|
||||||
|
|
||||||
|
|
||||||
|
BAUDRATES = [9600, 0, 19200, 0, 38400, 57600, 0, 115200]
|
||||||
|
|
||||||
|
|
||||||
|
class Motor(HasIodev, Drivable):
|
||||||
|
address = Property('module address', IntRange(0, 255), default=1)
|
||||||
|
fact = Property('gear factor', FloatRange(unit='deg/step'), default=1.8/256)
|
||||||
|
limit_pin_mask = Property('input pin mask for lower/upper limit switch',
|
||||||
|
TupleOf(IntRange(0, 15), IntRange(0, 15)),
|
||||||
|
default=(8, 0))
|
||||||
|
value = Parameter('motor position', FloatRange(unit='deg'))
|
||||||
|
target = Parameter('_', FloatRange(unit='$'), default=0)
|
||||||
|
zero = Parameter('zero point', FloatRange(unit='$'), readonly=False, default=0)
|
||||||
|
tolerance = Parameter('positioning tolerance', FloatRange(unit='$'), readonly=False, default=0.9)
|
||||||
|
encoder = Parameter('encoder value', FloatRange(unit='$'), needscfg=False)
|
||||||
|
steppos = Parameter('motor steps position', FloatRange(unit='$'), needscfg=False)
|
||||||
|
at_upper_limit = Parameter('upper limit switch touched', BoolType(), needscfg=False)
|
||||||
|
at_lower_limit = Parameter('lower limit switch touched', BoolType(), needscfg=False)
|
||||||
|
pull_up = Parameter('activate pull up resistors', BoolType(), needscfg=False)
|
||||||
|
baudrate = Parameter('baud rate code', EnumType({'%d' % v: i for i, v in enumerate(BAUDRATES)}),
|
||||||
|
readonly=False, default=0, poll=True)
|
||||||
|
|
||||||
|
temperature = IoPar(9, 1, IntRange(), readonly=True)
|
||||||
|
operating_voltage = IoPar(8, 1, IntRange(), readonly=True)
|
||||||
|
|
||||||
|
enc_tolerance = AxisDiffPar(212, 0, 360., '$')
|
||||||
|
maxspeed = AxisPar(4, 0, 2047)
|
||||||
|
minspeed = AxisPar(130, 0, 2047)
|
||||||
|
maxcurrent = AxisPar(6, 0, 2.8, 'A', 2.8/250)
|
||||||
|
standby_current = AxisPar(7, 0, 2.8, 'A', 2.8/250)
|
||||||
|
acceleration = AxisPar(5, 0, 2047)
|
||||||
|
internal_target = AxisZeroPar(0, unit='$')
|
||||||
|
target_reached = AxisPar(8, readonly=True)
|
||||||
|
move_status = AxisPar(207, readonly=True)
|
||||||
|
error_bits = AxisPar(208, readonly=True)
|
||||||
|
free_wheeling = AxisPar(204, 0, 60, 'sec', scale=0.001)
|
||||||
|
|
||||||
|
fast_pollfactor = 0.001
|
||||||
|
_targettime = None
|
||||||
|
_prevconn = None
|
||||||
|
iodevClass = BytesIO
|
||||||
|
_commcnt = 0
|
||||||
|
_pollcnt = 0
|
||||||
|
_lastpoll = 0
|
||||||
|
_calcTimeout = True
|
||||||
|
|
||||||
|
def earlyInit(self):
|
||||||
|
self.resetValues = dict(self.writeDict)
|
||||||
|
|
||||||
|
def startModule(self, started_callback):
|
||||||
|
loaded_pos = None
|
||||||
|
try:
|
||||||
|
print(self.save_filename())
|
||||||
|
with open(self.save_filename()) as f:
|
||||||
|
loaded_pos = float(f.read())
|
||||||
|
except FileNotFoundError:
|
||||||
|
pass
|
||||||
|
super().startModule(started_callback)
|
||||||
|
encoder = self.read_encoder()
|
||||||
|
if loaded_pos is not None:
|
||||||
|
diff = loaded_pos - encoder + 180
|
||||||
|
if abs(diff % 360 - 180) < self.enc_tolerance:
|
||||||
|
diff = round(diff / 180) * 180
|
||||||
|
if diff:
|
||||||
|
self.comm(SET_AXIS_PAR, 209, AxisZeroPar.to_raw(self, encoder + diff))
|
||||||
|
else:
|
||||||
|
msg = 'loaded pos (%g) does not match encoder reading' % loaded_pos
|
||||||
|
self.status = self.Status.ERROR, msg
|
||||||
|
self.log.error(msg + ' (%g)' % encoder)
|
||||||
|
|
||||||
|
def comm(self, cmd, adr=0, value=0):
|
||||||
|
if self._calcTimeout:
|
||||||
|
self._calcTimeout = False
|
||||||
|
baudrate = getattr(self._iodev._conn.connection, 'baudrate', None)
|
||||||
|
if baudrate:
|
||||||
|
if baudrate not in BAUDRATES:
|
||||||
|
raise CommunicationFailedError('unsupported baud rate: %d' % baudrate)
|
||||||
|
self._iodev.timeout = 0.03 + 200 / baudrate
|
||||||
|
|
||||||
|
self._commcnt = self._commcnt + 1
|
||||||
|
bank = adr // 1000
|
||||||
|
iadr = adr % 1000
|
||||||
|
for itry in range(3):
|
||||||
|
byt = struct.pack('>BBBBi', self.address, cmd, iadr, bank, round(value))
|
||||||
|
try:
|
||||||
|
reply = self._iodev.communicate(byt + bytes([sum(byt) & 0xff]), 9)
|
||||||
|
except Exception as e:
|
||||||
|
print(repr(e), adr, bank)
|
||||||
|
continue
|
||||||
|
if sum(reply[:-1]) & 0xff == reply[-1]:
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
raise CommunicationFailedError('checksum error')
|
||||||
|
radr, modadr, status, rcmd, result = struct.unpack('>BBBBix', reply)
|
||||||
|
if status != 100:
|
||||||
|
self.log.warning('bad status from cmd %r %s: %d', cmd, adr, status)
|
||||||
|
if radr != 2 or modadr != self.address or cmd != rcmd:
|
||||||
|
raise CommunicationFailedError('bad reply %r to command %s %d' % (reply, cmd, adr))
|
||||||
|
return result
|
||||||
|
|
||||||
|
def read_value(self):
|
||||||
|
now = time.time()
|
||||||
|
if now > self._lastpoll + 1:
|
||||||
|
print(now - self._lastpoll, self._commcnt, self._pollcnt)
|
||||||
|
self._commcnt = 0
|
||||||
|
self._pollcnt = 0
|
||||||
|
self._lastpoll = now
|
||||||
|
self._pollcnt = self._pollcnt + 1
|
||||||
|
|
||||||
|
init = self.comm(GET_GLOB_PAR, 2255) # bank 2 adr 255
|
||||||
|
if not init or self._prevconn != self._iodev._conn:
|
||||||
|
# reset to initial parameters when either motor lost power or connection has changed
|
||||||
|
self.log.info('reset initial values')
|
||||||
|
for pname, value in self.resetValues.items():
|
||||||
|
getattr(self, 'write_' + pname)(value)
|
||||||
|
self.comm(SET_GLOB_PAR, 2255, 1)
|
||||||
|
self._prevconn = self._iodev._conn
|
||||||
|
rawenc = self.comm(GET_AXIS_PAR, 209)
|
||||||
|
rawpos = self.comm(GET_AXIS_PAR, 1)
|
||||||
|
self.encoder = AxisZeroPar.from_raw(self, rawenc)
|
||||||
|
self.save_pos()
|
||||||
|
self.steppos = AxisZeroPar.from_raw(self, rawpos)
|
||||||
|
if abs(rawenc - rawpos) > 128:
|
||||||
|
return self.encoder
|
||||||
|
return self.steppos
|
||||||
|
|
||||||
|
def save_filename(self):
|
||||||
|
savedir = os.path.join(CONFIG['logdir'], 'persistent')
|
||||||
|
os.makedirs(savedir, exist_ok=True)
|
||||||
|
return os.path.join(savedir, '%s.%s' % (type(self).__name__, self.name))
|
||||||
|
|
||||||
|
def save_pos(self):
|
||||||
|
with open(self.save_filename(), 'w') as f:
|
||||||
|
f.write('%g' % self.value)
|
||||||
|
|
||||||
|
def read_status(self):
|
||||||
|
if not self._targettime:
|
||||||
|
return self.status
|
||||||
|
self.get_limits()
|
||||||
|
if self.at_lower_limit or self.at_upper_limit:
|
||||||
|
self.stop()
|
||||||
|
return self.Status.ERROR, 'at limit'
|
||||||
|
target_reached = self.read_target_reached()
|
||||||
|
if target_reached or self.read_move_status():
|
||||||
|
self._targettime = None
|
||||||
|
if abs(self.target - self.value) < self.tolerance:
|
||||||
|
return self.Status.IDLE, ''
|
||||||
|
self.log.warning('out of tolerance (%d)', self.move_status)
|
||||||
|
return self.Status.WARN, 'out of tolerance (%d)' % self.move_status
|
||||||
|
if time.time() > self._targettime:
|
||||||
|
self.log.warning('move timeout')
|
||||||
|
return self.Status.WARN, 'move timeout'
|
||||||
|
return self.Status.BUSY, 'moving'
|
||||||
|
|
||||||
|
def write_target(self, target):
|
||||||
|
rawenc = self.comm(GET_AXIS_PAR, 209)
|
||||||
|
rawpos = self.comm(GET_AXIS_PAR, 1)
|
||||||
|
if abs(rawenc - rawpos) > 128:
|
||||||
|
# adjust missing steps
|
||||||
|
missing_steps = round((rawenc - rawpos) / 256.0)
|
||||||
|
self.log.warning('correct %s missing steps', missing_steps)
|
||||||
|
rawpos += missing_steps * 256
|
||||||
|
self.comm(SET_AXIS_PAR, 1, rawpos)
|
||||||
|
rawtarget = round((target - self.zero) / self.fact)
|
||||||
|
delay = (abs(rawtarget - rawpos) / 30.5 / self.maxspeed
|
||||||
|
+ self.maxspeed / self.acceleration / 15.25 + 0.5)
|
||||||
|
self._targettime = time.time() + delay
|
||||||
|
self.log.info('move to %.1f', target)
|
||||||
|
self.comm(MOVE, 0, rawtarget)
|
||||||
|
print('write_target')
|
||||||
|
self.target_reached = 0
|
||||||
|
self.status = self.Status.BUSY, 'changed target'
|
||||||
|
return target
|
||||||
|
|
||||||
|
def get_limits(self):
|
||||||
|
self.input_bits = self.comm(GET_IO, 255)
|
||||||
|
bits = (~ self.input_bits) & 0xf
|
||||||
|
self.at_lower_limit = bool(bits & self.limit_pin_mask[0])
|
||||||
|
self.at_upper_limit = bool(bits & self.limit_pin_mask[1])
|
||||||
|
|
||||||
|
def read_at_upper_limit(self):
|
||||||
|
self.get_limits()
|
||||||
|
return Done
|
||||||
|
|
||||||
|
def read_at_lower_limit(self):
|
||||||
|
self.get_limits()
|
||||||
|
return Done
|
||||||
|
|
||||||
|
def write_pull_up(self, value):
|
||||||
|
self.comm(SET_IO, 0, int(value))
|
||||||
|
return value
|
||||||
|
|
||||||
|
def write_baudrate(self, value):
|
||||||
|
self.comm(SET_GLOB_PAR, 65, int(value))
|
||||||
|
|
||||||
|
def read_baudrate(self):
|
||||||
|
reply = self.comm(GET_GLOB_PAR, 65)
|
||||||
|
print('BAUD', reply)
|
||||||
|
return reply
|
||||||
|
|
||||||
|
@Command(FloatRange())
|
||||||
|
def set_zero(self, value):
|
||||||
|
self.zero += value - self.read_value()
|
||||||
|
|
||||||
|
@Command
|
||||||
|
def stop(self):
|
||||||
|
self.comm(MOTOR_STOP)
|
||||||
|
self.status = self.Status.IDLE, 'stopped'
|
||||||
|
self._targettime = None
|
||||||
|
|
||||||
|
@Command(FloatRange())
|
||||||
|
def step(self, value):
|
||||||
|
self.comm(MOVE, 1, value / self.fact)
|
||||||
|
|
||||||
|
@Command(IntRange(), result=IntRange())
|
||||||
|
def get_axis_par(self, adr):
|
||||||
|
return self.comm(GET_AXIS_PAR, adr)
|
||||||
|
|
||||||
|
@Command(IntRange())
|
||||||
|
def set_axis_par(self, adr, value):
|
||||||
|
return self.comm(SET_AXIS_PAR, adr, value)
|
Loading…
x
Reference in New Issue
Block a user