# -*- 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 # # ***************************************************************************** """driver for phytron motors""" from secop.core import Done, Command, EnumType, FloatRange, IntRange, \ HasIO, Parameter, Property, Drivable, PersistentMixin, PersistentParam, \ StringIO, StringType, TupleOf from secop.errors import CommunicationFailedError, HardwareError, BadValueError from secop.lib import clamp class PhytronIO(StringIO): end_of_line = '\x03' # ETX timeout = 0.2 identification = [('0IVR', 'MCC Minilog .*')] def communicate(self, command, expect_response=True): for ntry in range(5, 0, -1): try: head, _, reply = super().communicate('\x02' + command).partition('\x02') if reply[0] == '\x06': # ACK if len(reply) == 1 and expect_response: raise CommunicationFailedError('empty response') break raise CommunicationFailedError('missing ACK %r' % reply) except Exception as e: if ntry == 1: raise self.log.warning('%s - retry', e) return reply[1:] class Motor(PersistentMixin, HasIO, Drivable): axis = Property('motor axis X or Y', StringType(), default='X') address = Property('address', IntRange(0, 15), default=0) speed_factor = Property('steps / degree', FloatRange(0, None), default=2000) encoder_mode = Parameter('how to treat the encoder', EnumType('encoder', NO=0, READ=1, CHECK=2), default=1, readonly=False) value = Parameter('angle', FloatRange(unit='deg')) target = Parameter('target angle', FloatRange(unit='deg'), readonly=False) speed = Parameter('', FloatRange(0, 20, unit='deg/s'), readonly=False) accel = Parameter('', FloatRange(2, 250, unit='deg/s/s'), readonly=False) encoder_tolerance = Parameter('', FloatRange(unit='deg'), readonly=False, default=0.01) offset = PersistentParam('', FloatRange(unit='deg'), readonly=False, default=0) sign = PersistentParam('', IntRange(-1,1), readonly=False, default=1) encoder = Parameter('encoder reading', FloatRange(unit='deg')) sameside_offset = Parameter('offset when always approaching from the same side', FloatRange(unit='deg'), readonly=False, default=0) abslimits = Parameter('abs limits (raw values)', default=(0, 0), datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg'))) userlimits = PersistentParam('user limits', readonly=False, default=(0, 0), initwrite=True, datatype=TupleOf(FloatRange(unit='deg'), FloatRange(unit='deg'))) ioClass = PhytronIO fast_poll = 0.1 _sameside_pending = False _mismatch_count = 0 _rawlimits = None def earlyInit(self): super().earlyInit() if self.abslimits == (0, 0): self.abslimits = -9e99, 9e99 if self.userlimits == (0, 0): self._rawlimits = self.abslimits self.read_userlimits() self.loadParameters() def get(self, cmd): return self.communicate('\x02%x%s%s' % (self.address, self.axis, cmd)) def set(self, cmd, value): self.communicate('\x02%x%s%s%g' % (self.address, self.axis, cmd, value), False) def set_get(self, cmd, value, query): self.set(cmd, value) return self.get(query) def read_value(self): prev_enc = self.encoder pos = float(self.get('P20R')) * self.sign - self.offset if self.encoder_mode != 'NO': enc = self.read_encoder() else: enc = pos status = self.communicate('\x02%xSE' % self.address) status = status[0:4] if self.axis == 'X' else status[4:8] self.log.debug('run %s enc %s end %s', status[1], status[2], status[3]) status = self.get('=H') if status == 'N': if self.encoder_mode == 'CHECK': e1, e2 = sorted((prev_enc, enc)) if e1 - self.encoder_tolerance <= pos <= e2 + self.encoder_tolerance: self.status = self.Status.BUSY, 'driving' else: self.log.error('encoder lag: %g not within %g..%g', pos, e1, e2) self.get('S') # stop self.status = self.Status.ERROR, 'encoder lag error' self.setFastPoll(False) else: self.status = self.Status.BUSY, 'driving' else: if self._sameside_pending: # drive to real target self.set('A', self.sign * (self.target + self.offset)) self._sameside_pending = False return pos if (self.encoder_mode == 'CHECK' and abs(enc - pos) > self.encoder_tolerance): if self._mismatch_count < 2: self._mismatch_count += 1 else: self.log.error('encoder mismatch: abs(%g - %g) < %g', enc, pos, self.encoder_tolerance) self.status = self.Status.ERROR, 'encoder does not match pos' else: self._mismatch_count = 0 self.status = self.Status.IDLE, '' self.setFastPoll(False) return pos def read_encoder(self): if self.encoder_mode == 'NO': return self.value return float(self.get('P22R')) * self.sign - self.offset def read_speed(self): return float(self.get('P14R')) / self.speed_factor def write_speed(self, value): inv_factor = float(self.get('P03R')) if abs(inv_factor * self.speed_factor - 1) > 0.001: raise HardwareError('speed factor does not match %g' % (1.0 / inv_factor)) return float(self.set_get('P14S', int(value * self.speed_factor), 'P14R')) / self.speed_factor def read_accel(self): return float(self.get('P15R')) / self.speed_factor def write_accel(self, value): if abs(float(self.get('P03R')) * self.speed_factor - 1) > 0.001: raise HardwareError('speed factor does not match') return float(self.set_get('P15S', int(value * self.speed_factor), 'P15R')) / self.speed_factor def _check_limits(self, *values): for name, (mn, mx) in ('user', self._rawlimits), ('abs', self.abslimits): mn -= self.offset mx -= self.offset for v in values: if not (mn <= v <= mx): raise BadValueError('%s limits violation: %g <= %g <= %g' % (name, mn, v, mx)) v += self.offset def write_target(self, value): if self.status[0] == self.Status.ERROR: raise HardwareError('need reset') self.status = self.Status.BUSY, 'changed target' self._check_limits(value, value + self.sameside_offset) if self.sameside_offset: # drive first to target + sameside_offset # we do not optimize when already driving from the right side self._sameside_pending = True self.set('A', self.sign * (value + self.offset + self.sameside_offset)) else: self.set('A', self.sign * (value + self.offset)) self.setFastPoll(True, self.fast_poll) return value def read_userlimits(self): return self._rawlimits[0] - self.offset, self._rawlimits[1] - self.offset def write_userlimits(self, value): self._rawlimits = [clamp(self.abslimits[0], v + self.offset, self.abslimits[1]) for v in value] value = self.read_userlimits() self.saveParameters() return value def write_offset(self, value): self.offset = value self.read_userlimits() self.saveParameters() return Done def stop(self): self.get('S') @Command def reset(self): """reset error, set position to encoder""" self.read_value() if self.status[0] == self.Status.ERROR: enc = self.encoder + self.offset pos = self.value + self.offset if abs(enc - pos) > self.encoder_tolerance: if enc < 0: # assume we have a rotation (not a linear motor) while enc < 0: self.offset += 360 enc += 360 self.set('P22S', enc * self.sign) self.saveParameters() self.set('P20S', enc * self.sign) # set pos to encoder self.read_value() # self.status = self.Status.IDLE, '' # TODO: # '=E' electronics status # '=I+' / '=I-': limit switches # use P37 to determine if restarted