From 49ad153605e1eaced7378f7cdf6e3b6a7355f4ed Mon Sep 17 00:00:00 2001 From: Markus Zolliker Date: Tue, 27 Apr 2021 16:42:29 +0200 Subject: [PATCH] trinamic driver and bytesio module Change-Id: Id634e7514fecab6fd6bc3edf81e25ad41c2bb12f --- secop/bytesio.py | 90 +++++++++++ secop/lib/asynconn.py | 13 +- secop/stringio.py | 118 +++++++------- secop_psi/trinamic.py | 347 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 509 insertions(+), 59 deletions(-) create mode 100644 secop/bytesio.py create mode 100644 secop_psi/trinamic.py diff --git a/secop/bytesio.py b/secop/bytesio.py new file mode 100644 index 0000000..dc96d4f --- /dev/null +++ b/secop/bytesio.py @@ -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 +# ***************************************************************************** +"""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 diff --git a/secop/lib/asynconn.py b/secop/lib/asynconn.py index 348f971..e19feb4 100644 --- a/secop/lib/asynconn.py +++ b/secop/lib/asynconn.py @@ -79,8 +79,8 @@ class AsynConn: self.disconnect() @classmethod - def register_scheme(cls, scheme): - cls.SCHEME_MAP[scheme] = cls + def __init_subclass__(cls): + cls.SCHEME_MAP[cls.scheme] = cls def disconnect(self): raise NotImplementedError @@ -154,6 +154,8 @@ class AsynConn: class AsynTcp(AsynConn): + scheme = 'tcp' + def __init__(self, uri, *args, **kwargs): super().__init__(uri, *args, **kwargs) self.uri = uri @@ -202,9 +204,6 @@ class AsynTcp(AsynConn): raise ConnectionClosed() # marks end of connection -AsynTcp.register_scheme('tcp') - - class AsynSerial(AsynConn): """a serial connection using pyserial @@ -221,6 +220,7 @@ class AsynSerial(AsynConn): and others (see documentation of serial.Serial) """ + scheme = 'serial' PARITY_NAMES = {name[0]: name for name in ['NONE', 'ODD', 'EVEN', 'MASK', 'SPACE']} def __init__(self, uri, *args, **kwargs): @@ -282,6 +282,3 @@ class AsynSerial(AsynConn): return self.connection.read(n) data = self.connection.read(1) return data + self.connection.read(self.connection.in_waiting) - - -AsynSerial.register_scheme('serial') diff --git a/secop/stringio.py b/secop/stringio.py index 5554990..a860047 100644 --- a/secop/stringio.py +++ b/secop/stringio.py @@ -20,7 +20,8 @@ # ***************************************************************************** """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 @@ -37,66 +38,23 @@ from secop.modules import Attached, Command, \ from secop.poller import REGULAR -class StringIO(Communicator): - """line oriented communicator - - self healing is assured by polling the parameter 'is_connected' - """ +class BaseIO(Communicator): + """base of StringIO and BytesIO""" 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) wait_before = Parameter('wait time before sending', datatype=FloatRange(), default=0) is_connected = Parameter('connection state', datatype=BoolType(), readonly=False, poll=REGULAR) pollinterval = Parameter('reconnect interval', datatype=FloatRange(0), readonly=False, default=10) _reconnectCallbacks = None + _conn = None + _last_error = None def earlyInit(self): - self._conn = None 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): - 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)) + raise NotImplementedError def closeConnection(self): """close connection @@ -158,6 +116,62 @@ class StringIO(Communicator): if removeme: 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): """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, wait_before is respected for end_of_lines within a command. """ + command = command.encode(self.encoding) if not self.is_connected: self.read_is_connected() # try to reconnect if not self._conn: @@ -173,9 +188,9 @@ class StringIO(Communicator): with self._lock: # read garbage and wait before send if self.wait_before and self._eol_write: - cmds = command.encode(self.encoding).split(self._eol_write) + cmds = command.split(self._eol_write) else: - cmds = [command.encode(self.encoding)] + cmds = [command] garbage = None try: for cmd in cmds: @@ -186,6 +201,7 @@ class StringIO(Communicator): if garbage: self.log.debug('garbage: %r', garbage) self._conn.send(cmd + self._eol_write) + self.log.debug('send: %s', cmd + self._eol_write) reply = self._conn.readline(self.timeout) except ConnectionClosed: self.closeConnection() diff --git a/secop_psi/trinamic.py b/secop_psi/trinamic.py new file mode 100644 index 0000000..0c5a132 --- /dev/null +++ b/secop_psi/trinamic.py @@ -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 +# +# ***************************************************************************** + +"""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)