experimental persistent mixin

- still contains changes in mouduls/params ...
This commit is contained in:
zolliker 2021-06-04 12:19:04 +02:00
parent 1409959f53
commit fe5a2caac7
6 changed files with 483 additions and 308 deletions

View File

@ -8,8 +8,8 @@ uri = tcp://5000
[drv_iodev] [drv_iodev]
description = description =
class = secop.bytesio.BytesIO class = secop.bytesio.BytesIO
# uri = serial:///dev/ttyUSB0?baudrate=57600 uri = serial:///dev/ttyUSB0?baudrate=57600
uri = serial:///dev/ttyUSB0?baudrate=9600 # uri = serial:///dev/ttyUSB0?baudrate=9600
[drv] [drv]
description = trinamic motor test description = trinamic motor test
@ -17,9 +17,9 @@ class = secop_psi.trinamic.Motor
iodev = drv_iodev iodev = drv_iodev
standby_current=0.1 standby_current=0.1
maxcurrent=1.4 maxcurrent=1.4
acceleration=50 acceleration=150.
maxspeed=200 movelimit=360
zero=-36 speed=40
enc_tolerance=3.6 encoder_tolerance=3.6
free_wheeling=0.001 free_wheeling=0.001
pull_up=1 # pull_up=1

View File

@ -24,13 +24,15 @@
import sys import sys
import os
import json
import time import time
from secop.datatypes import ArrayOf, BoolType, EnumType, FloatRange, \ from secop.datatypes import ArrayOf, BoolType, EnumType, FloatRange, \
IntRange, StatusType, StringType, TextType, TupleOf, get_datatype IntRange, StatusType, StringType, TextType, TupleOf, get_datatype
from secop.errors import BadValueError, ConfigError, InternalError, \ from secop.errors import BadValueError, ConfigError, InternalError, \
ProgrammingError, SECoPError, SilentError, secop_error ProgrammingError, SECoPError, SilentError, secop_error
from secop.lib import formatException, mkthread from secop.lib import formatException, getGeneralConfig, mkthread
from secop.lib.enum import Enum from secop.lib.enum import Enum
from secop.params import Accessible, Command, Parameter from secop.params import Accessible, Command, Parameter
from secop.poller import BasicPoller, Poller from secop.poller import BasicPoller, Poller
@ -240,6 +242,8 @@ class Module(HasAccessibles):
self.name = name self.name = name
self.valueCallbacks = {} self.valueCallbacks = {}
self.errorCallbacks = {} self.errorCallbacks = {}
self.persistentFile = None
self.persistentData = {}
errors = [] errors = []
# handle module properties # handle module properties
@ -385,7 +389,7 @@ class Module(HasAccessibles):
if k in self.parameters or k in self.propertyDict: if k in self.parameters or k in self.propertyDict:
setattr(self, k, v) setattr(self, k, v)
cfgdict.pop(k) cfgdict.pop(k)
except (ValueError, TypeError): except (ValueError, TypeError) as e:
# self.log.exception(formatExtendedStack()) # self.log.exception(formatExtendedStack())
errors.append('module %s, parameter %s: %s' % (self.name, k, e)) errors.append('module %s, parameter %s: %s' % (self.name, k, e))
@ -395,6 +399,8 @@ class Module(HasAccessibles):
if '$' in dt.unit: if '$' in dt.unit:
dt.setProperty('unit', dt.unit.replace('$', self.parameters['value'].datatype.unit)) dt.setProperty('unit', dt.unit.replace('$', self.parameters['value'].datatype.unit))
self.writeDict.update(self.loadParameters())
# 6) check complete configuration of * properties # 6) check complete configuration of * properties
if not errors: if not errors:
try: try:
@ -533,6 +539,7 @@ class Module(HasAccessibles):
self.log.error(str(e)) self.log.error(str(e))
except Exception: except Exception:
self.log.error(formatException()) self.log.error(formatException())
self.saveParameters()
if started_callback: if started_callback:
started_callback() started_callback()
@ -545,6 +552,60 @@ class Module(HasAccessibles):
""" """
mkthread(self.writeInitParams, started_callback) mkthread(self.writeInitParams, started_callback)
def loadParameters(self):
"""load persistent parameters
:return: persistent parameters which have to be written
is called upon startup and may be called from a module
when a hardware powerdown is detected
"""
if any(pobj.persistent for pobj in self.parameters.values()):
persistentdir = os.path.join(getGeneralConfig()['logdir'], 'persistent')
self.persistentFile = os.path.join(persistentdir, '%s.%s.json' % (self.DISPATCHER.equipment_id, self.name))
else:
self.persistentFile = None
return {}
try:
with open(self.persistentFile, 'r') as f:
self.persistentData = json.load(f)
except FileNotFoundError:
self.persistentData = {}
writeDict = {}
for pname, pobj in self.parameters.items():
if pobj.persistent and pname in self.persistentData:
value = pobj.datatype.import_value(self.persistentData[pname])
try:
pobj.value = value
if not pobj.readonly:
writeDict[pname] = value
except Exception as e:
self.log.warning('can not restore %r to %r (%r)' % (pname, value, e))
return writeDict
def saveParameters(self):
"""save persistent parameters
to be called regularely explicitly by the module
"""
data = {k: v.export_value() for k, v in self.parameters.items() if v.persistent}
if data != self.persistentData:
self.persistentData = data
persistentdir = os.path.basename(self.persistentFile)
tmpfile = self.persistentFile + '.tmp'
if not os.path.isdir(persistentdir):
os.makedirs(persistentdir, exist_ok=True)
try:
with open(tmpfile, 'w') as f:
json.dump(self.persistentData, f, indent=2)
f.write('\n')
os.rename(tmpfile, self.persistentFile)
finally:
try:
os.remove(tmpfile)
except FileNotFoundError:
pass
class Readable(Module): class Readable(Module):
"""basic readable module""" """basic readable module"""

View File

@ -39,10 +39,6 @@ class Accessible(HasProperties):
kwds = None # is a dict if it might be used as Override kwds = None # is a dict if it might be used as Override
def __init__(self, **kwds):
super().__init__()
self.init(kwds)
def init(self, kwds): def init(self, kwds):
# do not use self.propertyValues.update here, as no invalid values should be # do not use self.propertyValues.update here, as no invalid values should be
# assigned to properties, even not before checkProperties # assigned to properties, even not before checkProperties
@ -153,6 +149,9 @@ class Parameter(Accessible):
handler = Property( handler = Property(
'[internal] overload the standard read and write functions', ValueType(), '[internal] overload the standard read and write functions', ValueType(),
export=False, default=None, settable=False) export=False, default=None, settable=False)
persistent = Property(
'[internal] persistent setting', BoolType(),
export=False, default=False)
initwrite = Property( initwrite = Property(
'''[internal] write this parameter on initialization '''[internal] write this parameter on initialization
@ -165,7 +164,7 @@ class Parameter(Accessible):
readerror = None readerror = None
def __init__(self, description=None, datatype=None, inherit=True, *, unit=None, constant=None, **kwds): def __init__(self, description=None, datatype=None, inherit=True, *, unit=None, constant=None, **kwds):
super().__init__(**kwds) super().__init__()
if datatype is not None: if datatype is not None:
if not isinstance(datatype, DataType): if not isinstance(datatype, DataType):
if isinstance(datatype, type) and issubclass(datatype, DataType): if isinstance(datatype, type) and issubclass(datatype, DataType):
@ -178,6 +177,8 @@ class Parameter(Accessible):
if 'default' in kwds: if 'default' in kwds:
self.default = datatype(kwds['default']) self.default = datatype(kwds['default'])
self.init(kwds) # datatype must be defined before we can treat dataset properties like fmtstr or unit
if description is not None: if description is not None:
self.description = inspect.cleandoc(description) self.description = inspect.cleandoc(description)
@ -313,7 +314,8 @@ class Command(Accessible):
func = None func = None
def __init__(self, argument=False, *, result=None, inherit=True, **kwds): def __init__(self, argument=False, *, result=None, inherit=True, **kwds):
super().__init__(**kwds) super().__init__()
self.init(kwds)
if result or kwds or isinstance(argument, DataType) or not callable(argument): if result or kwds or isinstance(argument, DataType) or not callable(argument):
# normal case # normal case
if argument is False and result: if argument is False and result:

94
secop/persistent.py Normal file
View File

@ -0,0 +1,94 @@
# -*- 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>
#
# *****************************************************************************
"""Define base classes for real Modules implemented in the server"""
import os
import json
from secop.errors import BadValueError, ConfigError, InternalError, \
ProgrammingError, SECoPError, SilentError, secop_error
class PersistentMixin:
def __init__(*args, **kwds):
super().__init__(*args, **kwds)
# self.persistentFile = None
# self.persistentData = {}
self.writeDict.update(self.loadParameters())
def writeInitParams(self, started_callback=None):
super().writeInitParams()
self.saveParameters()
if started_callback:
started_callback()
def loadParameters(self):
"""load persistent parameters
:return: persistent parameters which have to be written
is called upon startup and may be called from a module
when a hardware powerdown is detected
"""
persistentdir = os.path.join(getGeneralConfig()['logdir'], 'persistent')
self.persistentFile = os.path.join(persistentdir, '%s.%s.json' % (self.DISPATCHER.equipment_id, self.name))
try:
with open(self.persistentFile, 'r') as f:
self.persistentData = json.load(f)
except FileNotFoundError:
self.persistentData = {}
writeDict = {}
for pname, pobj in self.parameters.items():
if pobj.persistent and pname in self.persistentData:
value = pobj.datatype.import_value(self.persistentData[pname])
try:
pobj.value = value
if not pobj.readonly:
writeDict[pname] = value
except Exception as e:
self.log.warning('can not restore %r to %r (%r)' % (pname, value, e))
return writeDict
def saveParameters(self):
"""save persistent parameters
to be called regularely explicitly by the module
"""
data = {k: v.export_value() for k, v in self.parameters.items() if v.persistent}
if data != self.persistentData:
self.persistentData = data
persistentdir = os.path.basename(self.persistentFile)
tmpfile = self.persistentFile + '.tmp'
if not os.path.isdir(persistentdir):
os.makedirs(persistentdir, exist_ok=True)
try:
with open(tmpfile, 'w') as f:
json.dump(self.persistentData, f, indent=2)
f.write('\n')
os.rename(tmpfile, self.persistentFile)
finally:
try:
os.remove(tmpfile)
except FileNotFoundError:
pass

View File

@ -260,7 +260,8 @@ class Server:
self.modules[modname] = modobj self.modules[modname] = modobj
except ConfigError as e: except ConfigError as e:
errors.append('error creating module %s:' % modname) errors.append('error creating module %s:' % modname)
for errtxt in e.args[0] if isinstance(e.args, list) else [e.args[0]]: print('E', e.args)
for errtxt in e.args[0] if isinstance(e.args[0], list) else [e.args[0]]:
errors.append(' ' + errtxt) errors.append(' ' + errtxt)
except Exception: except Exception:
failure_traceback = traceback.format_exc() failure_traceback = traceback.format_exc()

View File

@ -23,221 +23,116 @@
"""drivers for trinamic PD-1161 motors""" """drivers for trinamic PD-1161 motors"""
import time import time
import os
import struct import struct
import json from math import log10
from secop.core import BoolType, Command, EnumType, FloatRange, IntRange, \ from secop.core import BoolType, Command, EnumType, FloatRange, IntRange, \
HasIodev, Parameter, Property, Drivable, TupleOf, Done HasIodev, Parameter, Property, Drivable, TupleOf, Done
from secop.bytesio import BytesIO from secop.bytesio import BytesIO
from secop.lib import getGeneralConfig from secop.errors import CommunicationFailedError, HardwareError, BadValueError, IsBusyError
from secop.errors import CommunicationFailedError, HardwareError
MOTOR_STOP = 3 MOTOR_STOP = 3
MOVE = 4 MOVE = 4
SET_AXIS_PAR = 5 SET_AXIS_PAR = 5
GET_AXIS_PAR = 6 GET_AXIS_PAR = 6
SAVE_AXIS_PAR = 7
LOAD_AXIS_PAR = 8
SET_GLOB_PAR = 9 SET_GLOB_PAR = 9
GET_GLOB_PAR = 10 GET_GLOB_PAR = 10
SAVE_GLOB_PAR = 11 # STORE_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):
result = round(self.from_raw(motor, motor.comm(self.GET, self.adr)), 3)
# print('read', self.adr, result)
return result
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] BAUDRATES = [9600, 0, 19200, 0, 38400, 57600, 0, 115200]
FULL_STEP = 1.8
ANGLE_SCALE = FULL_STEP/256
# assume factory settings for pulse and ramp divisors:
SPEED_SCALE = 1E6 / 2 ** 15 * ANGLE_SCALE
MAX_SPEED = 2047 * SPEED_SCALE
ACCEL_SCALE = 1E12 / 2 ** 31 * ANGLE_SCALE
MAX_ACCEL = 2047 * ACCEL_SCALE
CURRENT_SCALE = 2.8/250
ENCODER_RESOLUTION = 0.4 # 365 / 1024, rounded up
class HwParam(Parameter):
adr = Property('parameter address', IntRange(0, 255), export=False)
scale = Property('parameter address', FloatRange(), export=False)
def __init__(self, description, datatype, adr, scale=1, poll=True, persistent=False, **kwds):
"""hardware parameter"""
if isinstance(datatype, FloatRange) and not kwds.get('fmtstr'):
datatype.fmtstr = '%%.%df' % max(0, 1 - int(log10(scale)))
super().__init__(description, datatype, poll=poll, adr=adr, scale=scale,
persistent=persistent, **kwds)
def copy(self):
res = HwParam(self.description, self.datatype.copy(), self.adr)
res.name = self.name
res.init(self.propertyValues)
return res
class Motor(HasIodev, Drivable): class Motor(HasIodev, Drivable):
address = Property('module address', IntRange(0, 255), default=1) 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', # limit_pin_mask = Property('input pin mask for lower/upper limit switch',
TupleOf(IntRange(0, 15), IntRange(0, 15)), # TupleOf(IntRange(0, 15), IntRange(0, 15)),
default=(8, 0)) # default=(8, 0))
value = Parameter('motor position', FloatRange(unit='deg')) value = Parameter('motor position', FloatRange(unit='deg'))
zero = Parameter('zero point', FloatRange(unit='$'), readonly=False, default=0, persistent=True)
encoder = HwParam('encoder reading', FloatRange(unit='$'),
209, ANGLE_SCALE, readonly=True, initwrite=False, persistent=True)
steppos = HwParam('position from motor steps', FloatRange(unit='$'),
1, ANGLE_SCALE, readonly=True, initwrite=False)
target = Parameter('_', FloatRange(unit='$'), default=0) target = Parameter('_', FloatRange(unit='$'), default=0)
movelimit = Parameter('max. angle to drive in one go', FloatRange(unit='$'), default=400, readonly=False)
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) movelimit = Parameter('max. angle to drive in one go', FloatRange(unit='$'),
operating_voltage = IoPar(8, 1, IntRange(), readonly=True) readonly=False, default=360, group='more', persistent=True)
tolerance = Parameter('positioning tolerance', FloatRange(unit='$'),
readonly=False, default=0.9)
encoder_tolerance = HwParam('the allowed deviation between steppos and encoder\n\nmust be > tolerance',
FloatRange(0, 360., unit='$'),
212, ANGLE_SCALE, readonly=False, group='more', persistent=True)
speed = HwParam('max. speed', FloatRange(0, MAX_SPEED, unit='$/sec'),
4, SPEED_SCALE, readonly=False, group='more', persistent=True)
minspeed = HwParam('min. speed', FloatRange(0, MAX_SPEED, unit='$/sec'),
130, SPEED_SCALE, readonly=False, default=SPEED_SCALE, group='motorparam')
currentspeed = HwParam('current speed', FloatRange(-MAX_SPEED, MAX_SPEED, unit='$/sec'),
3, SPEED_SCALE, readonly=True, group='motorparam')
maxcurrent = HwParam('_', FloatRange(0, 2.8, unit='A'),
6, CURRENT_SCALE, readonly=False, group='motorparam', persistent=True)
standby_current = HwParam('_', FloatRange(0, 2.8, unit='A'),
7, CURRENT_SCALE, readonly=False, group='motorparam')
acceleration = HwParam('_', FloatRange(4.6 * ACCEL_SCALE, MAX_ACCEL, unit='deg/s^2'),
5, ACCEL_SCALE, readonly=False, group='motorparam', persistent=True)
target_reached = HwParam('_', BoolType(), 8, group='hwstatus')
move_status = HwParam('_', IntRange(0, 3),
207, readonly=True, persistent=False, group='hwstatus')
error_bits = HwParam('_', IntRange(0, 255),
208, readonly=True, persistent=False, group='hwstatus')
free_wheeling = HwParam('_', FloatRange(0, 60., unit='sec'),
204, 0.001, readonly=False, group='motorparam', persistent=True)
baudrate = Parameter('_', EnumType({'%d' % v: i for i, v in enumerate(BAUDRATES)}),
readonly=False, default=0, poll=True, visibility=3, group='more')
pollinterval = Parameter(group='more', persistent=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 iodevClass = BytesIO
fast_pollfactor = 0.001 # poll as fast as possible when busy
_started = 0
_calcTimeout = True _calcTimeout = True
_poserror = None _need_reset = False
save_filename = None _save_filename = None
# _try_count = 0
def earlyInit(self): def comm(self, cmd, adr, value=0, bank=0):
self.writeDict.update(self.loadParams()) """set or get a parameter
def loadParams(self): :param adr: parameter number
# the following should be moved to core functionality :param cmd: SET command (in the GET case, 1 is added to this)
savedir = os.path.join(CONFIG['logdir'], 'persistent') :param bank: the bank
os.makedirs(savedir, exist_ok=True) :param value: if given, the parameter is written, else it is returned
self._save_filename = os.path.join(savedir, '%s.%s.json' % (self.DISPATCHER.equipment_id, self.name)) :return: the returned value
try: """
with open(self._save_filename, 'r') as f:
save_dict = json.load(f)
except FileNotFoundError:
save_dict = {}
writeDict = {}
for pname, value in save_dict.items():
pobj = self.parameters[pname]
val = pobj.datatype.import_value(value)
if pobj.readonly:
try:
pobj.value = val
except Exception as e:
self.log.warning('can not restore %r to %r' % (pname, val))
else:
writeDict[pname] = val
return writeDict
def saveParams(self):
save_dict = {}
for pname, pobj in self.parameters.items():
if pname not in ('target', 'internal_target'):
save_dict[pname] = pobj.export_value()
tmpfile = self._save_filename + '.tmp'
try:
with open(tmpfile, 'w') as f:
json.dump(save_dict, f, indent=2)
f.write('\n')
os.rename(tmpfile, self._save_filename)
finally:
try:
os.remove(tmpfile)
except FileNotFoundError:
pass
def startModule(self, started_callback):
self._initialize = True
def cb(self=self, started_callback=started_callback, encoder=self.encoder, zero=self.zero):
encoder += self.zero - zero
diff = encoder - self.encoder + 180
if abs(diff % 360 - 180) < self.enc_tolerance:
diff = round((diff - 180) / 360) * 360
if diff:
self.comm(SET_AXIS_PAR, 209, AxisZeroPar.to_raw(self, encoder + diff))
self._poserror = 'reset'
else:
self.log.error('encoder: saved value (%g) does not match reading (%g)' % (encoder, self.encoder))
self._poserror = 'need_reset'
self.comm(SET_GLOB_PAR, 2255, 1)
self._initialize = False
self._prevconn = self._iodev._conn
started_callback()
super().startModule(cb)
def comm(self, cmd, adr=0, value=0):
if self._calcTimeout: if self._calcTimeout:
self._calcTimeout = False self._calcTimeout = False
baudrate = getattr(self._iodev._conn.connection, 'baudrate', None) baudrate = getattr(self._iodev._conn.connection, 'baudrate', None)
@ -246,19 +141,21 @@ class Motor(HasIodev, Drivable):
raise CommunicationFailedError('unsupported baud rate: %d' % baudrate) raise CommunicationFailedError('unsupported baud rate: %d' % baudrate)
self._iodev.timeout = 0.03 + 200 / baudrate self._iodev.timeout = 0.03 + 200 / baudrate
bank = adr // 1000 exc = None
iadr = adr % 1000 for itry in range(3,0,-1):
for itry in range(3): byt = struct.pack('>BBBBi', self.address, cmd, adr, bank, round(value))
byt = struct.pack('>BBBBi', self.address, cmd, iadr, bank, round(value))
try: try:
reply = self._iodev.communicate(byt + bytes([sum(byt) & 0xff]), 9) reply = self._iodev.communicate(byt + bytes([sum(byt) & 0xff]), 9)
if sum(reply[:-1]) & 0xff != reply[-1]:
raise CommunicationFailedError('checksum error')
except Exception as e: except Exception as e:
print(repr(e), adr, bank) if itry == 1:
raise
exc = e
continue continue
if sum(reply[:-1]) & 0xff == reply[-1]: break
break if exc:
else: self.log.warning('tried %d times after %s', itry, exc)
raise CommunicationFailedError('checksum error')
radr, modadr, status, rcmd, result = struct.unpack('>BBBBix', reply) radr, modadr, status, rcmd, result = struct.unpack('>BBBBix', reply)
if status != 100: if status != 100:
self.log.warning('bad status from cmd %r %s: %d', cmd, adr, status) self.log.warning('bad status from cmd %r %s: %d', cmd, adr, status)
@ -266,129 +163,249 @@ class Motor(HasIodev, Drivable):
raise CommunicationFailedError('bad reply %r to command %s %d' % (reply, cmd, adr)) raise CommunicationFailedError('bad reply %r to command %s %d' % (reply, cmd, adr))
return result return result
def read_value(self): def get(self, pname, **kwds):
rawenc = self.comm(GET_AXIS_PAR, 209) """get parameter"""
rawpos = self.comm(GET_AXIS_PAR, 1) pobj = self.parameters[pname]
self.encoder = AxisZeroPar.from_raw(self, rawenc) value = self.comm(GET_AXIS_PAR, pobj.adr, **kwds)
self.steppos = AxisZeroPar.from_raw(self, rawpos) # do not apply scale when 1 (datatype might not be float)
return value if pobj.scale == 1 else value * pobj.scale
initialized = self._initialize or self.comm(GET_GLOB_PAR, 2255) # bank 2 adr 255 def set(self, pname, value, check=True, **kwds):
if initialized and self._prevconn == self._iodev._conn: # no power loss or connection interrupt """set parameter and check result"""
self.saveParams() pobj = self.parameters[pname]
else: scale = pobj.scale
self.log.info('set to previous saved values') rawvalue = round(value / scale)
for pname, value in self.loadParams().items(): self.comm(SET_AXIS_PAR, pobj.adr, rawvalue, **kwds)
if check:
result = self.comm(GET_AXIS_PAR, pobj.adr, **kwds)
if result != rawvalue:
raise HardwareError('result does not match %d != %d' % (result, rawvalue))
value = result * scale
return value
def startModule(self, started_callback):
# get encoder value from motor. at this stage self.encoder contains the persistent value
encoder = self.get('encoder')
encoder += self.zero
self.fix_encoder(encoder)
super().startModule(started_callback)
def fix_encoder(self, encoder_from_hw):
"""fix encoder value
:param encoder_from_hw: the encoder value read from the HW
self.encoder is assumed to contain the last known (persistent) value
if encoder has not changed modulo 360, adjust by an integer multiple of 360
set status to error when encoder has changed be more than encoder_tolerance
"""
# calculate nearest, most probable value
adjusted_encoder = encoder_from_hw + round((self.encoder - encoder_from_hw) / 360.) * 360
if abs(self.encoder - adjusted_encoder) >= self.encoder_tolerance:
# encoder module0 360 has changed
self.log.error('saved encoder value (%.2f) does not match reading (%.2f %.2f)', self.encoder, encoder_from_hw, adjusted_encoder)
if adjusted_encoder != encoder_from_hw:
self.log.info('take next closest encoder value (%.2f)' % adjusted_encoder)
self._need_reset = True
self.status = self.Status.ERROR, 'saved encoder value does not match reading'
self.set('encoder', adjusted_encoder - self.zero, check=False)
def read_value(self):
encoder = self.read_encoder()
steppos = self.read_steppos()
initialized = self.comm(GET_GLOB_PAR, 255, bank=2)
if initialized: # no power loss
self.saveParameters()
if any((v==0 for v in self.persistentData.values())):
print('SAVED', self.persistentData)
else: # just powered up
# get persistent values
writeDict = self.loadParameters()
# self.encoder now contains the last known (persistent) value
self.log.info('set to previous saved values %r', writeDict)
for pname, value in writeDict.items():
try: try:
getattr(self, 'write_' + pname)(value) getattr(self, 'write_' + pname)(value)
except Exception as e: except Exception as e:
self.log.warning('can not write %r to %r' % (value, pname)) self.log.warning('can not write %r to %r (%r)' % (value, pname, e))
self.comm(SET_GLOB_PAR, 2255, 1) self.fix_encoder(encoder)
self._prevconn = self._iodev._conn value = self.encoder - self.zero
self.comm(SET_GLOB_PAR, 255, 1, bank=2) # set initialized flag
self._started = 0
self._need_reset = True
self.status = self.Status.ERROR, 'power loss'
# or should we just fix instead of error status?
# self.set('steppos', self.steppos - self.zero, check=False)
if abs(rawenc - rawpos) > 128: return encoder if abs(encoder - steppos) > self.tolerance else steppos
return self.encoder
return self.steppos
def read_status(self): def read_status(self):
if not self._targettime: oldpos = self.steppos
if self._poserror: self.read_value() # make sure encoder and steppos are fresh
return self.Status.ERROR, 'encoder does not match internal pos' if not self._started:
if abs(self.encoder - self.steppos) > self.encoder_tolerance:
self._need_reset = True
if self.status[0] != self.Status.ERROR:
self.log.error('encoder (%.2f) does not match internal pos (%.2f)', self.encoder, self.steppos)
return self.Status.ERROR, 'encoder does not match internal pos'
return self.status return self.status
# self.get_limits() now = time.time()
# if self.at_lower_limit or self.at_upper_limit: if oldpos != self.steppos or not (self.read_target_reached() or self.read_move_status()
# self.stop() or self.read_error_bits()):
# return self.Status.ERROR, 'at limit' return self.Status.BUSY, 'moving'
target_reached = self.read_target_reached() diff = self.target - self.encoder
if target_reached or self.read_move_status(): if abs(diff) <= self.tolerance:
self._targettime = None self._started = 0
if abs(self.target - self.value) < self.tolerance: return self.Status.IDLE, ''
return self.Status.IDLE, '' #if (abs(self.target - self.steppos) < self.tolerance and
self.log.warning('out of tolerance (%d)', self.move_status) # abs(self.encoder - self.steppos) < self.encoder_tolerance):
return self.Status.WARN, 'out of tolerance (%d)' % self.move_status # self._try_count += 1
if time.time() > self._targettime: # if self._try_count < 3:
self.log.warning('move timeout') # # occasionaly, two attempts are needed, as steppos and encoder might have been
return self.Status.WARN, 'move timeout' # # off by 1-2 full steps before moving
return self.Status.BUSY, 'moving' # self.fix_steppos(self.tolerance, self.target)
# self.log.warning('try move again')
# return self.Status.BUSY, 'try again'
self.log.error('out of tolerance by %.3g', diff)
self._started = 0
return self.Status.ERROR, 'out of tolerance'
def write_target(self, target): def write_target(self, target):
rawenc = self.comm(GET_AXIS_PAR, 209) self.read_value() # make sure encoder and steppos are fresh
rawpos = self.comm(GET_AXIS_PAR, 1)
if abs(rawenc - rawpos) > 128:
# adjust missing steps
missing_steps = (rawenc - rawpos) / 256.0
#if self._poserror == 'reset':
print(missing_steps, self._poserror)
if abs(missing_steps) < 10 or self._poserror == 'reset':
self.log.warning('correct missing steps (%.1f deg)', AxisDiffPar.from_raw(self, missing_steps))
self._poserror = None
self.status = self.Status.IDLE, ''
else:
self._poserror = 'need_reset'
raise ValueError('encoder does not match internal pos')
rawpos += round(missing_steps) * 256
self.comm(SET_AXIS_PAR, 1, rawpos)
if abs(target - self.encoder) > self.movelimit: if abs(target - self.encoder) > self.movelimit:
raise ValueError('can not move more than %s deg' % self.movelimit) raise BadValueError('can not move more than %s deg' % self.movelimit)
rawtarget = round((target - self.zero) / self.fact) diff = self.encoder - self.steppos
delay = (abs(rawtarget - rawpos) / 30.5 / self.maxspeed if self._need_reset:
+ self.maxspeed / self.acceleration / 15.25 + 0.5) raise HardwareError('need reset (%s)' % self.status[1])
self._targettime = time.time() + delay if abs(diff) > self.tolerance:
if abs(diff) > self.encoder_tolerance:
self._need_reset = True
self.status = self.Status.ERROR, 'encoder does not match internal pos'
raise HardwareError('need reset (encoder does not match internal pos)')
self.set('steppos', self.encoder - self.zero)
# self.fix_steppos(self.encoder_tolerance)
self._started = time.time()
# self._try_count = 0
self.log.info('move to %.1f', target) self.log.info('move to %.1f', target)
self.comm(MOVE, 0, rawtarget) self.comm(MOVE, 0, (target - self.zero) / ANGLE_SCALE)
print('write_target')
self.target_reached = 0
self.status = self.Status.BUSY, 'changed target' self.status = self.Status.BUSY, 'changed target'
return target return target
# def get_limits(self): def write_zero(self, value):
# self.input_bits = self.comm(GET_IO, 255) diff = value - self.zero
# bits = (~ self.input_bits) & 0xf self.encoder += diff
# self.at_lower_limit = bool(bits & self.limit_pin_mask[0]) self.steppos += diff
# self.at_upper_limit = bool(bits & self.limit_pin_mask[1]) self.value += diff
#
# 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 return value
def read_encoder(self):
return self.get('encoder') + self.zero
def read_steppos(self):
return self.get('steppos') + self.zero
def read_encoder_tolerance(self):
return self.get('encoder_tolerance')
def write_encoder_tolerance(self, value):
return self.set('encoder_tolerance', value)
def read_target_reached(self):
return self.get('target_reached')
def read_speed(self):
return self.get('speed')
def write_speed(self, value):
return self.set('speed', value)
def read_minspeed(self):
return self.get('minspeed')
def write_minspeed(self, value):
return self.set('minspeed', value)
def read_currentspeed(self):
return self.get('currentspeed')
def read_acceleration(self):
return self.get('acceleration')
def write_acceleration(self, value):
return self.set('acceleration', value)
def read_maxcurrent(self):
return self.get('maxcurrent')
def write_maxcurrent(self, value):
return self.set('maxcurrent', value)
def read_standby_current(self):
return self.get('standby_current')
def write_standby_current(self, value):
return self.set('standby_current', value)
def read_free_wheeling(self):
return self.get('free_wheeling')
def write_free_wheeling(self, value):
return self.set('free_wheeling', value)
def read_move_status(self):
return self.get('move_status')
def read_error_bits(self):
return self.get('error_bits')
@Command(FloatRange())
def set_zero(self, value):
self.write_zero(value - self.read_value())
def read_baudrate(self):
return self.comm(GET_GLOB_PAR, 65)
def write_baudrate(self, value): def write_baudrate(self, value):
self.comm(SET_GLOB_PAR, 65, int(value)) self.comm(SET_GLOB_PAR, 65, int(value))
def read_baudrate(self):
reply = self.comm(GET_GLOB_PAR, 65)
return reply
@Command(FloatRange())
def set_zero(self, value):
self.zero += value - self.read_value()
@Command() @Command()
def reset(self): def reset(self):
if self._poserror: """set steppos to encoder value, if not within tolerance"""
self._poserror = 'reset' if self._started:
self.write_target(self.encoder) raise IsBusyError('can not reset while moving')
tol = ENCODER_RESOLUTION
for itry in range(10):
diff = self.read_encoder() - self.read_steppos()
if abs(diff) <= tol:
self._need_reset = False
self.status = self.Status.IDLE, 'ok'
return
self.set('steppos', self.encoder - self.zero, check=False)
self.comm(MOVE, 0, (self.encoder - self.zero) / ANGLE_SCALE)
time.sleep(0.01)
if itry > 5:
tol = self.tolerance
self.status = self.Status.ERROR, 'reset failed'
return
@Command @Command()
def stop(self): def stop(self):
self.comm(MOTOR_STOP) self.comm(MOTOR_STOP, 0)
self.status = self.Status.IDLE, 'stopped' self.status = self.Status.IDLE, 'stopped'
self._targettime = None self._started = 0
@Command(FloatRange()) @Command()
def step(self, value): def step(self):
self.comm(MOVE, 1, value / self.fact) self.comm(MOVE, 1, FULL_STEP / ANGLE_SCALE)
@Command()
def back(self):
self.comm(MOVE, 1, - FULL_STEP / ANGLE_SCALE)
@Command(IntRange(), result=IntRange()) @Command(IntRange(), result=IntRange())
def get_axis_par(self, adr): def get_axis_par(self, adr):
return self.comm(GET_AXIS_PAR, adr) return self.comm(GET_AXIS_PAR, adr)
@Command(IntRange()) @Command((IntRange(), FloatRange()), result=IntRange())
def set_axis_par(self, adr, value): def set_axis_par(self, adr, value):
return self.comm(SET_AXIS_PAR, adr, value) return self.comm(SET_AXIS_PAR, adr, value)