diff --git a/cfg/trinamic.cfg b/cfg/trinamic.cfg index 3f9f108..5431be7 100644 --- a/cfg/trinamic.cfg +++ b/cfg/trinamic.cfg @@ -8,8 +8,8 @@ uri = tcp://5000 [drv_iodev] description = class = secop.bytesio.BytesIO -# uri = serial:///dev/ttyUSB0?baudrate=57600 -uri = serial:///dev/ttyUSB0?baudrate=9600 +uri = serial:///dev/ttyUSB0?baudrate=57600 +# uri = serial:///dev/ttyUSB0?baudrate=9600 [drv] description = trinamic motor test @@ -17,9 +17,9 @@ class = secop_psi.trinamic.Motor iodev = drv_iodev standby_current=0.1 maxcurrent=1.4 -acceleration=50 -maxspeed=200 -zero=-36 -enc_tolerance=3.6 +acceleration=150. +movelimit=360 +speed=40 +encoder_tolerance=3.6 free_wheeling=0.001 -pull_up=1 +# pull_up=1 diff --git a/secop/modules.py b/secop/modules.py index 920059c..0fd2cb5 100644 --- a/secop/modules.py +++ b/secop/modules.py @@ -24,13 +24,15 @@ import sys +import os +import json import time from secop.datatypes import ArrayOf, BoolType, EnumType, FloatRange, \ IntRange, StatusType, StringType, TextType, TupleOf, get_datatype from secop.errors import BadValueError, ConfigError, InternalError, \ 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.params import Accessible, Command, Parameter from secop.poller import BasicPoller, Poller @@ -240,6 +242,8 @@ class Module(HasAccessibles): self.name = name self.valueCallbacks = {} self.errorCallbacks = {} + self.persistentFile = None + self.persistentData = {} errors = [] # handle module properties @@ -385,7 +389,7 @@ class Module(HasAccessibles): if k in self.parameters or k in self.propertyDict: setattr(self, k, v) cfgdict.pop(k) - except (ValueError, TypeError): + except (ValueError, TypeError) as e: # self.log.exception(formatExtendedStack()) errors.append('module %s, parameter %s: %s' % (self.name, k, e)) @@ -395,6 +399,8 @@ class Module(HasAccessibles): if '$' in dt.unit: dt.setProperty('unit', dt.unit.replace('$', self.parameters['value'].datatype.unit)) + self.writeDict.update(self.loadParameters()) + # 6) check complete configuration of * properties if not errors: try: @@ -533,6 +539,7 @@ class Module(HasAccessibles): self.log.error(str(e)) except Exception: self.log.error(formatException()) + self.saveParameters() if started_callback: started_callback() @@ -545,6 +552,60 @@ class Module(HasAccessibles): """ 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): """basic readable module""" diff --git a/secop/params.py b/secop/params.py index 5b0de55..174480c 100644 --- a/secop/params.py +++ b/secop/params.py @@ -39,10 +39,6 @@ class Accessible(HasProperties): 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): # do not use self.propertyValues.update here, as no invalid values should be # assigned to properties, even not before checkProperties @@ -153,6 +149,9 @@ class Parameter(Accessible): handler = Property( '[internal] overload the standard read and write functions', ValueType(), export=False, default=None, settable=False) + persistent = Property( + '[internal] persistent setting', BoolType(), + export=False, default=False) initwrite = Property( '''[internal] write this parameter on initialization @@ -165,7 +164,7 @@ class Parameter(Accessible): readerror = None 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 not isinstance(datatype, DataType): if isinstance(datatype, type) and issubclass(datatype, DataType): @@ -178,6 +177,8 @@ class Parameter(Accessible): if 'default' in kwds: 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: self.description = inspect.cleandoc(description) @@ -313,7 +314,8 @@ class Command(Accessible): func = None 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): # normal case if argument is False and result: diff --git a/secop/persistent.py b/secop/persistent.py new file mode 100644 index 0000000..6d73ef2 --- /dev/null +++ b/secop/persistent.py @@ -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 +# +# ***************************************************************************** +"""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 + diff --git a/secop/server.py b/secop/server.py index c9be71b..8f1b8cb 100644 --- a/secop/server.py +++ b/secop/server.py @@ -260,7 +260,8 @@ class Server: self.modules[modname] = modobj except ConfigError as e: 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) except Exception: failure_traceback = traceback.format_exc() diff --git a/secop_psi/trinamic.py b/secop_psi/trinamic.py index 1cc13b0..e901f96 100644 --- a/secop_psi/trinamic.py +++ b/secop_psi/trinamic.py @@ -23,221 +23,116 @@ """drivers for trinamic PD-1161 motors""" import time -import os import struct -import json +from math import log10 + 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 +from secop.errors import CommunicationFailedError, HardwareError, BadValueError, IsBusyError + 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): - 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) - +# STORE_GLOB_PAR = 11 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): 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)) + + # 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')) + 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) - 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) - operating_voltage = IoPar(8, 1, IntRange(), readonly=True) + movelimit = Parameter('max. angle to drive in one go', FloatRange(unit='$'), + 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 + fast_pollfactor = 0.001 # poll as fast as possible when busy + _started = 0 _calcTimeout = True - _poserror = None - save_filename = None + _need_reset = False + _save_filename = None + # _try_count = 0 - def earlyInit(self): - self.writeDict.update(self.loadParams()) + def comm(self, cmd, adr, value=0, bank=0): + """set or get a parameter - def loadParams(self): - # the following should be moved to core functionality - savedir = os.path.join(CONFIG['logdir'], 'persistent') - os.makedirs(savedir, exist_ok=True) - self._save_filename = os.path.join(savedir, '%s.%s.json' % (self.DISPATCHER.equipment_id, self.name)) - 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): + :param adr: parameter number + :param cmd: SET command (in the GET case, 1 is added to this) + :param bank: the bank + :param value: if given, the parameter is written, else it is returned + :return: the returned value + """ if self._calcTimeout: self._calcTimeout = False baudrate = getattr(self._iodev._conn.connection, 'baudrate', None) @@ -246,19 +141,21 @@ class Motor(HasIodev, Drivable): raise CommunicationFailedError('unsupported baud rate: %d' % baudrate) self._iodev.timeout = 0.03 + 200 / baudrate - bank = adr // 1000 - iadr = adr % 1000 - for itry in range(3): - byt = struct.pack('>BBBBi', self.address, cmd, iadr, bank, round(value)) + exc = None + for itry in range(3,0,-1): + byt = struct.pack('>BBBBi', self.address, cmd, adr, bank, round(value)) try: 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: - print(repr(e), adr, bank) + if itry == 1: + raise + exc = e continue - if sum(reply[:-1]) & 0xff == reply[-1]: - break - else: - raise CommunicationFailedError('checksum error') + break + if exc: + self.log.warning('tried %d times after %s', itry, exc) 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) @@ -266,129 +163,249 @@ class Motor(HasIodev, Drivable): raise CommunicationFailedError('bad reply %r to command %s %d' % (reply, cmd, adr)) return result - def read_value(self): - rawenc = self.comm(GET_AXIS_PAR, 209) - rawpos = self.comm(GET_AXIS_PAR, 1) - self.encoder = AxisZeroPar.from_raw(self, rawenc) - self.steppos = AxisZeroPar.from_raw(self, rawpos) + def get(self, pname, **kwds): + """get parameter""" + pobj = self.parameters[pname] + value = self.comm(GET_AXIS_PAR, pobj.adr, **kwds) + # 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 - if initialized and self._prevconn == self._iodev._conn: # no power loss or connection interrupt - self.saveParams() - else: - self.log.info('set to previous saved values') - for pname, value in self.loadParams().items(): + def set(self, pname, value, check=True, **kwds): + """set parameter and check result""" + pobj = self.parameters[pname] + scale = pobj.scale + rawvalue = round(value / scale) + 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: getattr(self, 'write_' + pname)(value) except Exception as e: - self.log.warning('can not write %r to %r' % (value, pname)) - self.comm(SET_GLOB_PAR, 2255, 1) - self._prevconn = self._iodev._conn + self.log.warning('can not write %r to %r (%r)' % (value, pname, e)) + self.fix_encoder(encoder) + 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 self.encoder - return self.steppos + return encoder if abs(encoder - steppos) > self.tolerance else steppos def read_status(self): - if not self._targettime: - if self._poserror: - return self.Status.ERROR, 'encoder does not match internal pos' + oldpos = self.steppos + self.read_value() # make sure encoder and steppos are fresh + 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 - # 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' + now = time.time() + if oldpos != self.steppos or not (self.read_target_reached() or self.read_move_status() + or self.read_error_bits()): + return self.Status.BUSY, 'moving' + diff = self.target - self.encoder + if abs(diff) <= self.tolerance: + self._started = 0 + return self.Status.IDLE, '' + #if (abs(self.target - self.steppos) < self.tolerance and + # abs(self.encoder - self.steppos) < self.encoder_tolerance): + # self._try_count += 1 + # if self._try_count < 3: + # # occasionaly, two attempts are needed, as steppos and encoder might have been + # # off by 1-2 full steps before 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): - rawenc = self.comm(GET_AXIS_PAR, 209) - 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) + self.read_value() # make sure encoder and steppos are fresh if abs(target - self.encoder) > self.movelimit: - raise ValueError('can not move more than %s deg' % self.movelimit) - 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 + raise BadValueError('can not move more than %s deg' % self.movelimit) + diff = self.encoder - self.steppos + if self._need_reset: + raise HardwareError('need reset (%s)' % self.status[1]) + 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.comm(MOVE, 0, rawtarget) - print('write_target') - self.target_reached = 0 + self.comm(MOVE, 0, (target - self.zero) / ANGLE_SCALE) 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)) + def write_zero(self, value): + diff = value - self.zero + self.encoder += diff + self.steppos += diff + self.value += diff 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): 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() def reset(self): - if self._poserror: - self._poserror = 'reset' - self.write_target(self.encoder) + """set steppos to encoder value, if not within tolerance""" + if self._started: + 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): - self.comm(MOTOR_STOP) + self.comm(MOTOR_STOP, 0) self.status = self.Status.IDLE, 'stopped' - self._targettime = None + self._started = 0 - @Command(FloatRange()) - def step(self, value): - self.comm(MOVE, 1, value / self.fact) + @Command() + def step(self): + self.comm(MOVE, 1, FULL_STEP / ANGLE_SCALE) + + @Command() + def back(self): + self.comm(MOVE, 1, - FULL_STEP / ANGLE_SCALE) @Command(IntRange(), result=IntRange()) def get_axis_par(self, adr): return self.comm(GET_AXIS_PAR, adr) - @Command(IntRange()) + @Command((IntRange(), FloatRange()), result=IntRange()) def set_axis_par(self, adr, value): return self.comm(SET_AXIS_PAR, adr, value)