diff --git a/cfg/dpm.cfg b/cfg/dpm.cfg new file mode 100644 index 0000000..f15228d --- /dev/null +++ b/cfg/dpm.cfg @@ -0,0 +1,15 @@ +[NODE] +description = DPM driver for pressure cell +id = dpm.psi.ch + +[INTERFACE] +uri = tcp://5000 + +[force] +description = DPM driver to read out the transducer value, write and read the offset and scale factor +class = secop_psi.dpm.DPM3 +# uri = ldmse-d910-ts.psi.ch:3001 +uri = serial:///dev/ttyUSB1 +digits = 2 +scale_factor = 0.0156 + diff --git a/cfg/trinamic.cfg b/cfg/trinamic.cfg index 29e269f..3f9f108 100644 --- a/cfg/trinamic.cfg +++ b/cfg/trinamic.cfg @@ -8,7 +8,8 @@ uri = tcp://5000 [drv_iodev] description = class = secop.bytesio.BytesIO -uri = serial:///dev/ttyUSB0?baudrate=57600 +# uri = serial:///dev/ttyUSB0?baudrate=57600 +uri = serial:///dev/ttyUSB0?baudrate=9600 [drv] description = trinamic motor test diff --git a/cfg/uniax.cfg b/cfg/uniax.cfg new file mode 100644 index 0000000..4e9916f --- /dev/null +++ b/cfg/uniax.cfg @@ -0,0 +1,32 @@ +[NODE] +id = uniax.psi.ch +description = uniax pressure stick with motor and transducer + +[INTERFACE] +uri = tcp://5000 + +[drv_iodev] +description = +class = secop.bytesio.BytesIO +uri = serial:///dev/ttyUSB0?baudrate=57600 +# uri = serial:///dev/ttyUSB0?baudrate=9600 + +[drv] +description = trinamic motor test +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 +free_wheeling=0.001 +pull_up=1 + +[force] +description = DPM driver to read out the transducer value, write and read the offset and scale factor +class = secop_psi.dpm.DPM3 +uri = serial:///dev/ttyUSB1?baudrate=9600 +digits = 2 +scale_factor = 0.0156 diff --git a/secop_psi/dpm.py b/secop_psi/dpm.py new file mode 100644 index 0000000..407773b --- /dev/null +++ b/secop_psi/dpm.py @@ -0,0 +1,145 @@ +# -*- 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: +# ... +# +# ***************************************************************************** + +from secop.core import Readable, Parameter, FloatRange, BoolType, StringIO, HasIodev, IntRange, Done + + +class DPM3IO(StringIO): + end_of_line = '\r' + timeout = 3 + identification = [('*1R135', '01')] + + +def hex2float(hexvalue, digits): + value = int(hexvalue, 16) + if value >= 0x800000: + value -= 0x1000000 + return value / (10 ** digits) + + +def float2hex(value, digits): + intvalue = int(round(value * 10 ** digits,0)) + if intvalue < 0: + intvalue += 0x1000000 + return '%06X' % intvalue + + +class DPM3(HasIodev, Readable): + OFFSET = 0x8f + SCALE = 0x8c + + MAGNITUDE = {'1': 1, '2': 10, '3': 100, '4': 1e3, '5': 1e4, '6': 1e5, + '9':-1, 'A':-10, 'B':-100, 'C':-1e3, 'D':-1e4, 'E':-1e5} + + iodevClass = DPM3IO + + digits = Parameter('number of digits for value', IntRange(0, 5), initwrite=True, readonly=False) + value = Parameter(unit='N') + + offset = Parameter('', FloatRange(-1e5, 1e5), readonly=False, poll=True) + + #Note: we have tro treat the units properly. + """ We got an output of 150 for 10N. The maximal force we want to deal with is 100N, + thus a maximl output of 1500. 10=150/f + """ + scale_factor = Parameter('', FloatRange(-1e5, 1e5, unit='input_units/N'), readonly=False, poll=True) + + def query(self, adr, value=None): + if value is not None: + if adr == self.SCALE: + absval = abs(value) + for nibble, mag in self.MAGNITUDE.items(): + if 10000 <= round(value * mag, 0) <= 99999: + break + else: + # not suitable range found + if absval >= 99999.5: # overrange + raise ValueError('%s is out of range' % value) + # underrange: take lowest + nibble = '9' if value < 0 else '1' + mag = self.MAGNITUDE[nibble] + hex_val = nibble + '%05X' % int(round(value * mag, 0)) + if hex_val[1:] == '00000': + raise ValueError('scale factor can not be 0', value) + else: + hex_val = float2hex(value, self.digits) + cmd = '*1F3%02X%s\r' % (adr, hex_val) + else: + cmd = "" + cmd = cmd + '*1G3%02X' % adr + hexvalue = self._iodev.communicate(cmd) + print(cmd, hexvalue) + if adr == self.SCALE: + mag = self.MAGNITUDE[hexvalue[0:1]] + value = int(hexvalue[1:], 16) + return value/mag + else: + return hex2float(hexvalue, self.digits) + + def write_digits(self, value): + # value defines the number of digits + back_value=self._iodev.communicate('*1F135%02X\r*1G135' % (value + 1)) + self.digits = int(back_value,16) - 1 + # realculate proper scale and offset + self.write_scale_factor(self.scale_factor) + self.write_offset(self.offset) + return Done + + def read_digits(self): + back_value = self._iodev.communicate('*1G135') + return int(back_value,16) - 1 + + def read_value(self): + value = self._iodev.communicate('*1B1') + return float(value) + + def read_offset(self): + reply = self.query(self.OFFSET) + return reply + + def write_offset(self, value): + return self.query(self.OFFSET, value) + + def read_scale_factor(self): + reply = self.query(self.SCALE) + return float(reply) / 10 ** self.digits + + def write_scale_factor(self, value): + reply = self.query(self.SCALE, value * 10 ** self.digits) + return float(reply) / 10 ** self.digits + + + + + + + + + + + + + + + + + diff --git a/secop_psi/trinamic.py b/secop_psi/trinamic.py index 0c5a132..1cc13b0 100644 --- a/secop_psi/trinamic.py +++ b/secop_psi/trinamic.py @@ -25,6 +25,7 @@ import time import os import struct +import json from secop.core import BoolType, Command, EnumType, FloatRange, IntRange, \ HasIodev, Parameter, Property, Drivable, TupleOf, Done from secop.bytesio import BytesIO @@ -69,7 +70,9 @@ class AxisPar(Parameter): return value * self.scale def read(self, motor): - return round(self.from_raw(motor, motor.comm(self.GET, self.adr)), 3) + 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) @@ -136,12 +139,13 @@ class Motor(HasIodev, Drivable): default=(8, 0)) value = Parameter('motor position', FloatRange(unit='deg')) 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) + # 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) @@ -165,34 +169,73 @@ class Motor(HasIodev, Drivable): _targettime = None _prevconn = None iodevClass = BytesIO - _commcnt = 0 - _pollcnt = 0 - _lastpoll = 0 _calcTimeout = True + _poserror = None + save_filename = None def earlyInit(self): - self.resetValues = dict(self.writeDict) + self.writeDict.update(self.loadParams()) + + 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): - 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 + 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) * 180 + diff = round((diff - 180) / 360) * 360 if diff: self.comm(SET_AXIS_PAR, 209, AxisZeroPar.to_raw(self, encoder + diff)) + self._poserror = 'reset' else: - msg = 'loaded pos (%g) does not match encoder reading' % loaded_pos - self.status = self.Status.ERROR, msg - self.log.error(msg + ' (%g)' % encoder) + 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: @@ -203,7 +246,6 @@ class Motor(HasIodev, Drivable): 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): @@ -225,47 +267,37 @@ class Motor(HasIodev, Drivable): 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) + + 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(): + 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 + 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: + if self._poserror: + 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' + # 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 @@ -283,10 +315,20 @@ class Motor(HasIodev, Drivable): 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 + 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: + 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) @@ -298,19 +340,19 @@ class Motor(HasIodev, Drivable): 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 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)) @@ -321,13 +363,18 @@ class Motor(HasIodev, Drivable): 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 reset(self): + if self._poserror: + self._poserror = 'reset' + self.write_target(self.encoder) + @Command def stop(self): self.comm(MOTOR_STOP)