class MirrorSample(BaseMirrorSample): _misalignments = {} parameters = { 'alignerrors': Param( 'Errors to simulate the sample misalignment', type=dictof(str, floatrange(0)), settable=False, userparam=False, default={ 'sample_y': 2, 'omega': 0.1, 'detarm': 0.2, 'chi': 1, }, ), } def doInit(self, mode): if mode == MASTER: self._misalign() def _applyParams(self, number, parameters): BaseMirrorSample._applyParams(self, number, parameters) self._misalign() def _misalign(self): for n, err in self.alignerrors.items(): self._misalignments[n] = random.uniform(-err, err)
class TriangleAngle(HasOffset, TriangleMaster): parameters = { 'index': Param('index of return', type=intrange(0, 1), settable=False, volatile=False, userparam=False), 'scale': Param('direction definition (-1, 1)', type=oneof(-1, 1), settable=False, mandatory=True), } parameter_overrides = { 'offset': Override(type=floatrange(-2, 2)), } def doRead(self, maxage=0): try: self.log.debug('index: %d' % self.index) res = self.offset + self.scale * self._read_controller(self.index) self.log.debug('pos: %f' % res) except IndexError: res = 0 return res
def doIsAllowed(self, target): self.log.debug('doIsAllowed') if isinstance(target, string_types): try: oneof('horizontal', '12mrad_b3_12.000', '12mrad_b2_12.254_eng', '12mrad_b2_12.88_big', '12mrad_b3_13.268', '12mrad_b3_789', '48mrad')(target) return True, '' except ValueError as e: return False, str(e) elif isinstance(target, number_types): try: floatrange(0, 48)(target) return True, '' except ValueError as e: return False, str(e) return False, 'Wrong value type'
class BeckhoffMotorCab1M0x(BeckhoffMotorCab1): parameter_overrides = { # see docu: speed <= 8mm/s 'vmax': Override(settable=True, type=floatrange(0, 800)), } @requires(level='admin') def doWriteVMax(self, value): self._HW_writeParameter('vMax', self._phys2speed(value))
def doInit(self, mode): MultiSwitcher.doInit(self, mode) self._sm_values = sorted( [x for x in self.mapping if isinstance(x, number_types)])[1:] named_vals = {k: v[0] for k, v in self.mapping.items()} self.valuetype = oneofdict_or(named_vals, floatrange(0, self._sm_values[-1]))
class ReadonlySwitcher(MappedReadable): """Same as the `Switcher`, but for read-only underlying devices.""" attached_devices = { 'readable': Attach('The continuous device which is read', Readable), } parameters = { 'precision': Param('Precision for comparison', type=floatrange(0), default=0), } parameter_overrides = { 'fallback': Override(userparam=False, type=none_or(str), mandatory=False), } hardware_access = False def _readRaw(self, maxage=0): return self._attached_readable.read(maxage) def _mapReadValue(self, pos): prec = self.precision for name, value in iteritems(self.mapping): if prec: if abs(pos - value) <= prec: return name elif pos == value: return name if self.fallback is not None: return self.fallback raise PositionError(self, 'unknown position of %s' % self._attached_readable) def doStatus(self, maxage=0): # if the underlying device is moving or in error state, # reflect its status move_status = self._attached_readable.status(maxage) if move_status[0] not in (status.OK, status.WARN): return move_status # otherwise, we have to check if we are at a known position, # and otherwise return an error status try: if self.read(maxage) == self.fallback: return status.NOTREACHED, 'unconfigured position of %s or '\ 'still moving' % self._attached_readable except PositionError as e: return status.NOTREACHED, str(e) return status.OK, '' def doReset(self): self._attached_readable.reset()
class ChopperDisc(HasLimits, HasPrecision, DeviceMixinBase): parameters = { 'phase': Param('Phase of chopper disc', type=floatrange(0, 360), settable=True, userparam=True, fmtstr='%.2f', unit='deg', category='status'), 'current': Param('motor current', type=float, settable=False, volatile=True, userparam=True, fmtstr='%.2f', unit='A'), 'mode': Param('Internal mode', type=int, settable=False, userparam=True), 'chopper': Param('chopper number inside controller', type=intrange(1, 6), settable=False, userparam=True), 'reference': Param('reference to Disc one', type=floatrange(-360, 360), settable=False, userparam=True), 'edge': Param('Chopper edge of neutron window', type=oneof('open', 'close'), settable=False, userparam=True), 'gear': Param('Chopper ratio', type=intrange(-6, 6), settable=False, userparam=True, default=0), 'pos': Param('Distance to the disc 1', type=floatrange(0), settable=False, userparam=True, default=0., fmtstr='%.2f', unit='m', category='status'), } parameter_overrides = { 'unit': Override(default='rpm', mandatory=False,), 'precision': Override(default=2), 'abslimits': Override(default=(0, 6000), mandatory=False), 'fmtstr': Override(default='%.f'), } def doPoll(self, n, maxage): self._pollParam('current') def _isStopped(self): return abs(self.read(0)) <= self.precision
class PivotPoint(ManualSwitch): parameters = { 'grid': Param('Distance between the possible points', type=floatrange(0), settable=False, userparam=False, default=125., unit='mm'), 'height': Param('Height above ground level', type=floatrange(0), settable=False, default=373, unit='mm'), } parameter_overrides = { 'requires': Override(default={'level': ADMIN}, settable=False), } def doStart(self, target): ManualSwitch.doStart(self, target)
class MUX(CARESSDevice, Device): parameters = { 'initsleep': Param('sleep time after initialize', type=floatrange(0), default=10, settable=False, userparam=False), } def doInit(self, mode): session.delay(self.initsleep) CARESSDevice.doInit(self, mode) session.delay(self.initsleep)
def doInit(self, mode): # Even if the slit could not be become closer then 0 and not more # opened the maxheight the instrument scientist want to scan over # the limits to find out the 'open' and 'closed' point for the neutrons self.valuetype = tupleof(floatrange(-1, self.maxheight + 1), float) # generate auto devices for name, idx, opmode in [('height', 0, CENTERED), ('center', 1, CENTERED)]: self.__dict__[name] = SingleSlitAxis('%s.%s' % (self.name, name), slit=self, unit=self.unit, lowlevel=True, index=idx, opmode=opmode) self._motors = [self._attached_slit_r, self._attached_slit_s]
class Disc(VirtualMotor): parameters = { 'phase': Param( 'Phase in respect to the first disc', type=floatrange(-180, 180), default=0, settable=True, ), 'gear': Param( 'Gear', type=float, default=1, settable=True, ), 'crc': Param( 'Counter-rotating mode', type=int, default=1, settable=True, ), 'slittype': Param( 'Slit type', type=int, default=1, settable=True, ), } parameter_overrides = { 'abslimits': Override(default=(-27000, 27000), mandatory=False), 'unit': Override( default='rpm', mandatory=False, ), 'jitter': Override(default=2), 'curvalue': Override(default=6000), }
class IPCModBusTaco(TacoDevice, IPCModBus): """IPC protocol communication over TACO RS-485 server.""" taco_class = RS485Client taco_errorcodes = { 4019: InvalidCommandError, 537133063: InvalidCommandError, } parameters = { 'bustimeout': Param('Communication timeout for this device', type=floatrange(0.1, 1200), default=0.5, settable=True), } def send(self, addr, cmd, param=0, length=0): return self._taco_guard(self._dev.SDARaw, addr, cmd, length, param) def get(self, addr, cmd, param=0, length=0): return self._taco_guard(self._dev.SRDRaw, addr, cmd, length, param) def ping(self, addr): return self._taco_guard(self._dev.Ping, addr) def doReadBustimeout(self): if self._dev and hasattr(self._dev, 'timeout'): return float(self._taco_guard(self._dev.timeout)) raise ProgrammingError(self, "TacoDevice has no 'timeout'!") def doUpdateBustimeout(self, value): if not self._sim_intercept and self._dev: try: self._taco_update_resource('timeout', str(value)) except (TACOError, Exception) as e: self.log.warning('ignoring %r', e)
class IsegNHQChannel(EpicsAnalogMoveableEss): """ Device class to handle a single channel of an Iseg NHQ power supply. Create two or more of these in a setup to handle additional channels. Only pvprefix and channel parameters need to be set in the setup. The ramp, trip and current parameters can be used to set and read additional device settings and readouts at runtime. """ parameters = { 'ramp': Param('Ramp speed when changing voltage (120 to 15300)', type=floatrange(120, 15300), unit='V/min', settable=True, volatile=True), 'trip': Param('Max current before tripping emergency off (0 for off)', type=float, unit='mA', settable=True, volatile=True), 'current': Param('Measured output current (mA)', type=float, unit='mA', settable=False, volatile=True), 'pvprefix': Param('Prefix to use for EPICS PVs', type=pvname, mandatory=True, settable=False), 'channel': Param('Which channel to use (eg: 1 or 2)', type=int, mandatory=True, settable=False), } parameter_overrides = { # This device uses its own PVs internally, see _get_record_fields(). 'readpv': Override(mandatory=False, userparam=False, settable=False), 'writepv': Override(mandatory=False, userparam=False, settable=False), 'targetpv': Override(mandatory=False, userparam=False, settable=False), # Always read device value using doRead[Param] 'abslimits': Override(volatile=True), # Defaults 'unit': Override(mandatory=False, default='V'), 'fmtstr': Override(mandatory=False, default='%.1f'), } # PVs used, channel is substituted in based on given parameter value. def _get_record_fields(self): pv_map = { 'readpv': 'Volt{}_rbv', 'writepv': 'SetVolt{}', 'targetpv': 'SetVolt{}_rbv', 'startpv': 'StartVolt{}', 'vmax': 'VMax', 'error': 'Error', 'setramp': 'RampSpeed{}', 'getramp': 'RampSpeed{}_rbv', 'getcurr': 'Curr{}_rbv', 'settrip': 'CurrTrip{}', 'gettrip': 'CurrTrip{}_rbv', 'status': 'Status{}_rbv', 'modstat': 'ModStatus{}_rbv' } return pv_map def _get_pv_parameters(self): return set(self._record_fields) def _get_pv_name(self, pvparam): return self.pvprefix + self._record_fields[pvparam].format( self.channel) def doInit(self, mode): self._started = False def doStatus(self, maxage=0): if self._mode == SIMULATION: return status.OK, 'simulation' # Reading the Status on this device has the side-effect of resetting # error conditions after what caused them has been resolved. # Status covers most states we're interested in stat = self._get_pv('status') # Kill switch is only available in Module Status modstat = int(self._get_pv('modstat')) if modstat & 0x10: stat = 'KIL' if self._started and stat == 'ON': stat = 'WAIT' elif self._started and stat != 'ON': self._started = False return { 'ON': (status.OK, 'Ready'), 'OFF': (status.UNKNOWN, 'Front panel switch off'), 'MAN': (status.UNKNOWN, 'Front panel set to manual'), 'ERR': (status.ERROR, 'VMax or IMax exceeded!'), 'INH': (status.ERROR, 'Inhibit signal activated!'), 'QUA': (status.WARN, 'Quality of output voltage not given'), 'L2H': (status.BUSY, 'Voltage is increasing'), 'H2L': (status.BUSY, 'Voltage is decreasing'), 'LAS': (status.WARN, 'Look at Status (only after G-command)'), 'TRP': (status.ERROR, 'Current trip has been activated!'), 'KIL': (status.ERROR, 'Kill switch enabled!'), 'WAIT': (status.BUSY, 'Start command issued, waiting for response'), }.get(stat, (status.UNKNOWN, 'Unknown Status: "%s"' % stat)) def doRead(self, maxage=0): return self._get_pv('readpv') def doStart(self, pos): # We want these to happen in order. If the first times out, an # exception is thrown and prevents the startpv from being triggered. self._started = True self._put_pv_blocking('writepv', pos) self._put_pv('startpv', 1) # Value doesn't actually matter def doReadTarget(self): self._get_pv('targetpv') def doReadAbslimits(self): return 0, self._get_pv('vmax') def doReadRamp(self): return self._get_pv('getramp') * 60 def doWriteRamp(self, value): self._put_pv('setramp', value / 60.0) def doReadTrip(self): return self._get_pv('gettrip') def doWriteTrip(self, value): self._put_pv('settrip', value) def doReadCurrent(self): # This one is read-only, for information return self._get_pv('getcurr')
class Sans1ColliMotorAllParams(Sans1ColliMotor): """ Device object for a digital output device via a Beckhoff modbus interface. Maximum Parameter Implementation. All Relevant Parameters are accessible and can be configured. """ _paridx = dict( refpos=2, vmax=3, v_max=3, vmin=4, v_min=4, vref=5, v_ref=5, acc=6, a_acc=6, ae=7, a_e=7, microsteps=8, backlash=9, fullsteps_u=10, fullsteps=10, imax=11, i_max=11, iv=12, i_v=12, iv0=12, i_v0=12, imin=12, i_min=12, encodersteps_u=20, features=30, t=40, temp=40, temperature=40, type=250, hw=251, fw=252, reset=255, ) parameters = { # provided by parent class: speed, unit, fmtstr, warnlimits, userlimits, # abslimits, precision and others 'power': Param( 'Power on/off for the motordriver and ' 'enable/disable for the logic', type=oneof('off', 'on'), default='off', settable=True), 'backlash': Param('Backlash correction in physical units', type=float, default=0.0, settable=True, mandatory=False, volatile=True), 'maxcurrent': Param('Max Motor current in A', type=floatrange(0.05, 5), settable=True, volatile=True), 'idlecurrent': Param('Idle Motor current in A', type=floatrange(0.05, 5), settable=True, volatile=True), 'temperature': Param('Temperature of the motor driver', type=float, settable=False, volatile=True), 'minspeed': Param('The minimum motor speed', unit='main/s', settable=True, volatile=True), 'refspeed': Param('The referencing speed', unit='main/s', settable=True, volatile=True), 'accel': Param('Acceleration/Decceleration', unit='main/s**2', settable=True, volatile=True), 'stopaccel': Param('Emergency Decceleration', unit='main/s**2', settable=True, volatile=True), # needed ? Ask the Sans1 people... 'hw_vmax': Param('Maximum Velocity in HW-units', type=intrange(1, 2047), settable=True, volatile=True), 'hw_vmin': Param('Minimum Velocity in HW-units', type=intrange(1, 2047), settable=True, volatile=True), 'hw_vref': Param('Referencing Velocity in HW-units', type=intrange(1, 2047), settable=True, volatile=True), 'hw_accel': Param('Acceleration in HW-units', type=intrange(16, 2047), settable=True, volatile=True), 'hw_accel_e': Param('Acceleration when hitting a limit switch in ' 'HW-units', type=intrange(16, 2047), settable=True, volatile=True), 'hw_backlash': Param( 'Backlash in HW-units', # don't use type=intrange(-32768, 32767), settable=True, volatile=True), 'hw_fullsteps': Param('Motor steps per turn in HW-units', type=intrange(1, 65535), settable=True, volatile=True), 'hw_enc_steps': Param('Encoder steps per turn in HW-units', type=intrange(1, 65535), settable=True, volatile=True), 'hw_features': Param('Value of features register (16 Bit, see docu)', type=intrange(0, 65535), settable=True, volatile=True), 'hw_type': Param('Value of features register (16 Bit, see docu)', type=int, settable=True, volatile=True), 'hw_revision': Param('Value of HW-revision register ' '(16 Bit, see docu)', type=int, settable=True, volatile=True), 'hw_firmware': Param('Value of HW-Firmware register ' '(16 Bit, see docu)', type=int, settable=True, volatile=True), 'hw_coderflt': Param('Input filter for Encoder signals', type=oneofdict({ 1: 'disabled', 0: 'enabled' }), default='enabled', settable=True, volatile=True), 'hw_feedback': Param('Feedback signal for positioning', type=oneofdict({ 0: 'encoder', 1: 'motor' }), default='motor', settable=True, volatile=True), 'hw_invposfb': Param('Turning direction of encoder', type=oneofdict({ 1: 'opposite', 0: 'concordant' }), default='concordant', settable=True, volatile=True), 'hw_ramptype': Param('Shape of accel/deccel ramp', type=oneofdict({ 1: 'exponential', 0: 'linear' }), default='linear', settable=True, volatile=True), 'hw_revin1': Param('Type of input 1', type=oneofdict({ 1: 'nc', 0: 'no' }), default='no', settable=True, volatile=True), 'hw_revin2': Param('Type of input 2', type=oneofdict({ 1: 'nc', 0: 'no' }), default='no', settable=True, volatile=True), 'hw_disin1rev': Param('Use Input 1 as reference input', type=oneofdict({ 1: 'off', 0: 'on' }), default='on', settable=True, volatile=True), 'hw_disin2rev': Param('Use Input 2 as reference input', type=oneofdict({ 1: 'off', 0: 'on' }), default='on', settable=True, volatile=True), 'hw_invrev': Param('Direction of reference drive', type=oneofdict({ 1: 'pos', 0: 'neg' }), default='neg', settable=True, volatile=True), } parameter_overrides = { 'microsteps': Override(mandatory=False, settable=True, volatile=True), 'refpos': Override(settable=True), } # more advanced stuff: setting/getting parameters # only to be used manually at the moment @usermethod @requires(level='user') def readParameter(self, index): self.log.debug('readParameter %d', index) try: index = int(self._paridx.get(index, index)) except ValueError: raise UsageError( self, 'Unknown parameter %r, try one of %s' % (index, ', '.join(self._paridx))) from None if self._readStatusWord() & (1 << 7): raise UsageError( self, 'Can not access Parameters while Motor is ' 'moving, please stop it first!') if self.power == 'on': self.power = 'off' # wait for inactive ACK/NACK self.log.debug('Wait for idle ACK/NACK bits') for _ in range(1000): if self._readStatusWord() & (3 << 14) == 0: break else: raise CommunicationError( self, 'HW still busy, can not read ' 'Parameter, please retry later.') self._writeUpperControlWord((index << 8) | 4) self.log.debug('Wait for ACK/NACK bits') for _ in range(1000): if self._readStatusWord() & (3 << 14) != 0: break else: raise CommunicationError( self, 'ReadPar command not recognized by ' 'HW, please retry later.') if self._readStatusWord() & (1 << 14): raise CommunicationError( self, 'Reading of Parameter %r failed, ' 'got a NACK' % index) return self._readReturn() @usermethod @requires(level='admin') def writeParameter(self, index, value, store2eeprom=False): self.log.debug('writeParameter %d:0x%04x', index, value) if store2eeprom: self.log.warning('writeParameter stores to eeprom !') try: index = int(self._paridx.get(index, index)) except ValueError: UsageError(self, 'Unknown parameter %r' % index) if self._readStatusWord() & (1 << 7): raise UsageError( self, 'Can not access Parameters while Motor is ' 'moving, please stop it first!') if self.power == 'on': self.power = 'off' # wait for inactive ACK/NACK self.log.debug('Wait for idle ACK/NACK bits') for _ in range(1000): if self._readStatusWord() & (3 << 14) == 0: break else: raise CommunicationError( self, 'HW still busy, can not write ' 'Parameter, please retry later.') self._writeDestination(value) if store2eeprom: # store to eeprom self._writeUpperControlWord((index << 8) | 3) else: # store to volatile memory self._writeUpperControlWord((index << 8) | 1) self.log.debug('Wait for ACK/NACK bits') for _ in range(1000): if self._readStatusWord() & (3 << 14) != 0: break else: raise CommunicationError( self, 'WritePar command not recognized ' 'by HW, please retry later.') if self._readStatusWord() & (1 << 14): raise CommunicationError( self, 'Writing of Parameter %r failed, ' 'got a NACK' % index) return self._readReturn() # # Parameter access methods # def doWritePower(self, value): if self._readStatusWord() & (1 << 7): raise UsageError( self, 'Never switch off Power while Motor is ' 'moving !') value = ['off', 'on'].index(value) # docu: bit0 = enable/disable self._writeControlBit(0, value) def doReadPower(self): # docu: bit0 = enable/disable return ['off', 'on'][self._readControlBit(0)] # Parameter 1 : CurrentPosition def doSetPosition(self, value): self.writeParameter(1, self._phys2steps(value)) # Parameter 2 : Refpos def doReadRefpos(self): return self._steps2phys(self.readParameter(2)) def doWriteRefpos(self, value): self.writeParameter(2, self._phys2steps(value), store2eeprom=True) # Parameter 3 : hw_vmax -> speed def doReadHw_Vmax(self): return self.readParameter(3) def doReadSpeed(self): return self._speed2phys(self.hw_vmax) # units per second def doWriteHw_Vmax(self, value): self.writeParameter(3, value) def doWriteSpeed(self, speed): self.hw_vmax = self._phys2speed(speed) # Parameter 4 : hw_vmin -> minspeed def doReadHw_Vmin(self): return self.readParameter(4) def doReadMinspeed(self): return self._speed2phys(self.hw_vmin) # units per second def doWriteHw_Vmin(self, value): self.writeParameter(4, value) def doWriteMinspeed(self, speed): self.hw_vmin = self._phys2speed(speed) # Parameter 5 : hw_vref -> refspeed def doReadHw_Vref(self): return self.readParameter(5) # µSteps per second def doReadRefspeed(self): return self._speed2phys(self.hw_vref) # units per second def doWriteHw_Vref(self, value): self.writeParameter(5, value) def doWriteRefspeed(self, speed): self.hw_vref = self._phys2speed(speed) # Parameter 6 : hw_accel -> accel def doReadHw_Accel(self): return self.readParameter(6) # µSteps per second def doReadAccel(self): return self._speed2phys(self.hw_accel) # units per second def doWriteHw_Accel(self, value): self.writeParameter(6, value) def doWriteAccel(self, accel): self.hw_accel = self._phys2speed(accel) # Parameter 7 : hw_accel_e -> stopaccel def doReadHw_Accel_E(self): return self.readParameter(7) # µSteps per second def doReadStopaccel(self): return self._speed2phys(self.hw_accel_e) # units per second def doWriteHw_Accel_E(self, value): self.writeParameter(7, value) def doWriteStopaccel(self, accel): self.hw_accel_e = self._phys2speed(accel) # Parameter 8 : microsteps def doWriteMicrosteps(self, value): for i in range(7): if value == 2**i: self.writeParameter(8, i) break else: raise InvalidValueError( self, 'This should never happen! value should be one of: ' '1, 2, 4, 8, 16, 32, 64 !') def doReadMicrosteps(self): return 2**self.readParameter(8) # Parameter 9 : hw_backlash -> backlash def doReadHw_Backlash(self): return self.readParameter(9) # µSteps per second def doReadBacklash(self): return self._steps2phys(self.hw_backlash) def doWriteHw_Backlash(self, value): self.writeParameter(9, value) def doWriteBacklash(self, value): self.hw_backlash = self._phys2steps(value) # Parameter 10 : Fullsteps per turn def doReadHw_Fullsteps(self): return self.readParameter(10) def doWriteHw_Fullsteps(self, value): self.writeParameter(10, value) # Parameter 11 : MaxCurrent def doReadMaxcurrent(self): return self.readParameter(11) * 0.05 def doWriteMaxcurrent(self, value): self.writeParameter(11, int(0.5 + value / 0.05)) # Parameter 12 : IdleCurrent def doReadIdlecurrent(self): return self.readParameter(12) * 0.05 def doWriteIdlecurrent(self, value): self.writeParameter(12, int(0.5 + value / 0.05)) # Parameter 20 : Encodersteps per turn def doReadHw_Enc_Steps(self): return self.readParameter(20) def doWriteHw_Enc_Steps(self, value): self.writeParameter(20, value) # Parameter 30 : Features def doReadHw_Features(self): value = self.readParameter(30) self.log.debug('Feature0: Inputfilter for encodersignals: %d', value & 1) self.log.debug( 'Feature1: Positionsrueckfuehrung (0=Encoder, ' '1=Zaehler): %d', (value >> 1) & 1) self.log.debug( 'Feature2: Zaehlrichtung encoder (0=mitlaufend, ' '1=gegenlaufend): %d', (value >> 2) & 1) self.log.debug('Feature3: Bremsrampe (0=linear, 1=exponentiell): %d', (value >> 3) & 1) self.log.debug('Feature4: Eingang1 (0=Schliesser, 1=oeffner): %d', (value >> 4) & 1) self.log.debug('Feature5: Eingang2 (0=Schliesser, 1=oeffner): %d', (value >> 5) & 1) self.log.debug('Feature6: Eingang1 (0=referenz, 1=normal): %d', (value >> 6) & 1) self.log.debug('Feature7: Eingang2 (0=referenz, 1=normal): %d', (value >> 7) & 1) self.log.debug( 'Feature8: Richtung der Referenzfahrt (0=negativ, ' '1=positiv): %d', (value >> 8) & 1) return value def doWriteHw_Features(self, value): self.writeParameter(30, value) # bitwise access def doReadHw_Coderflt(self): return (self.hw_features >> 0) & 1 def doWriteHw_Coderflt(self, value): if value in [0, 1]: self.hw_features = (self.hw_features & ~(1 << 0)) | (value << 0) else: raise InvalidValueError(self, 'hw_disencfltr can only be 0 or 1') def doReadHw_Feedback(self): return (self.hw_features >> 1) & 1 def doWriteHw_Feedback(self, value): if value in [0, 1]: self.hw_features = (self.hw_features & ~(1 << 1)) | (value << 1) else: raise InvalidValueError(self, 'hw_feedback can only be 0 or 1') def doReadHw_Invposfb(self): return (self.hw_features >> 2) & 1 def doWriteHw_Invposfb(self, value): if value in [0, 1]: self.hw_features = (self.hw_features & ~(1 << 2)) | (value << 2) else: raise InvalidValueError(self, 'hw_invposfb can only be 0 or 1') def doReadHw_Ramptype(self): return (self.hw_features >> 3) & 1 def doWriteHw_Ramptype(self, value): if value in [0, 1]: self.hw_features = (self.hw_features & ~(1 << 3)) | (value << 3) else: raise InvalidValueError(self, 'hw_ramptype can only be 0 or 1') def doReadHw_Revin1(self): return (self.hw_features >> 4) & 1 def doWriteHw_Revin1(self, value): if value in [0, 1]: self.hw_features = (self.hw_features & ~(1 << 4)) | (value << 4) else: raise InvalidValueError(self, 'hw_revin1 can only be 0 or 1') def doReadHw_Revin2(self): return (self.hw_features >> 5) & 1 def doWriteHw_Revin2(self, value): if value in [0, 1]: self.hw_features = (self.hw_features & ~(1 << 5)) | (value << 5) else: raise InvalidValueError(self, 'hw_revin2 can only be 0 or 1') def doReadHw_Disin1Rev(self): return (self.hw_features >> 6) & 1 def doWriteHw_Disin1Rev(self, value): if value in [0, 1]: self.hw_features = (self.hw_features & ~(1 << 6)) | (value << 6) else: raise InvalidValueError(self, 'hw_disin1rev can only be 0 or 1') def doReadHw_Disin2Rev(self): return (self.hw_features >> 7) & 1 def doWriteHw_Disin2Rev(self, value): if value in [0, 1]: self.hw_features = (self.hw_features & ~(1 << 7)) | (value << 7) else: raise InvalidValueError(self, 'hw_disin2rev can only be 0 or 1') def doReadHw_Invrev(self): return (self.hw_features >> 8) & 1 def doWriteHw_Invrev(self, value): if value in [0, 1]: self.hw_features = (self.hw_features & ~(1 << 8)) | (value << 8) else: raise InvalidValueError(self, 'hw_invrev can only be 0 or 1') # Parameter 40 : Temperature def doReadTemperature(self): return self.readParameter(40) # Parameter 250 : Klemmentyp def doReadHw_Type(self): return self.readParameter(250) # Parameter 251 : Hardwarestand def doReadHw_Revision(self): return self.readParameter(251) # Parameter 252 : Firmwarestand def doReadHw_Firmware(self): return self.readParameter(252) # Parameter 255 : Factory Reset @usermethod def FactoryReset(self, password): """resets the motorcontroller to factory default values for the right password see docu""" # 0x544B4531 self.writeParameter(255, password)
class EpicsDevice(DeviceMixinBase): """ Basic EPICS device. """ hardware_access = True valuetype = anytype parameters = { 'epicstimeout': Param('Timeout for getting EPICS PVs', type=none_or(floatrange(0.1, 60)), default=1.0), } # A set of all parameters that indicate PV names. Since PVs are very # limited, an EpicsDevice is expected to use many different PVs a lot # of times. pv_parameters = set() # This will store PV objects for each PV param. _pvs = {} _pvctrls = {} def doPreinit(self, mode): # Don't create PVs in simulation mode self._pvs = {} self._pvctrls = {} if mode != SIMULATION: # in case we get started in a thread, make sure to use the global # CA context in that thread if epics.ca.current_context() is None: epics.ca.use_initial_context() # When there are standard names for PVs (see motor record), the PV # names may be derived from some prefix. To make this more flexible, # the pv_parameters are obtained via a method that can be overridden # in subclasses. pv_parameters = self._get_pv_parameters() for pvparam in pv_parameters: # Retrieve the actual PV-name from (potentially overridden) method pvname = self._get_pv_name(pvparam) if not pvname: raise ConfigurationError(self, 'PV for parameter %s was ' 'not found!' % pvparam) pv = self._pvs[pvparam] = epics.pv.PV( pvname, connection_timeout=self.epicstimeout) pv.connect() if not pv.wait_for_connection(timeout=self.epicstimeout): raise CommunicationError(self, 'could not connect to PV %r' % pvname) self._pvctrls[pvparam] = pv.get_ctrlvars() or {} else: for pvparam in self._get_pv_parameters(): self._pvs[pvparam] = HardwareStub(self) self._pvctrls[pvparam] = {} def _get_pv_parameters(self): # The default implementation of this method simply returns the # pv_parameters set return self.pv_parameters def _get_pv_name(self, pvparam): # In the default case, the name of a PV-parameter is stored in ai # parameter. This method can be overridden in subclasses in case the # name can be derived using some other information. return getattr(self, pvparam) def doStatus(self, maxage=0): # Return the status and the affected pvs in case the status is not OK mapped_status, affected_pvs = self._get_mapped_epics_status() status_message = 'Affected PVs: ' + ', '.join( affected_pvs) if mapped_status != status.OK else '' return mapped_status, status_message def _get_mapped_epics_status(self): # Checks the status and severity of all the associated PVs. # Returns the worst status (error prone first) and # a list of all associated pvs having that error if epics.ca.current_context() is None: epics.ca.use_initial_context() status_map = {} for name in self._pvs: epics_status = self._get_pvctrl(name, 'status', update=True) epics_severity = self._get_pvctrl(name, 'severity') mapped_status = STAT_TO_STATUS.get(epics_status, None) if mapped_status is None: mapped_status = SEVERITY_TO_STATUS.get( epics_severity, status.UNKNOWN) status_map.setdefault(mapped_status, []).append( self._get_pv_name(name)) return max(status_map.items()) def _setMode(self, mode): super(EpicsDevice, self)._setMode(mode) # remove the PVs on entering simulation mode, to prevent # accidental access to the hardware if mode == SIMULATION: for key in self._pvs: self._pvs[key] = HardwareStub(self) def _get_pv(self, pvparam, as_string=False): # since NICOS devices can be accessed from any thread, we have to # ensure that the same context is set on every thread if epics.ca.current_context() is None: epics.ca.use_initial_context() result = self._pvs[pvparam].get(timeout=self.epicstimeout, as_string=as_string) if result is None: # timeout raise CommunicationError(self, 'timed out getting PV %r from EPICS' % self._get_pv_name(pvparam)) return result def _get_pvctrl(self, pvparam, ctrl, default=None, update=False): if update: if epics.ca.current_context() is None: epics.ca.use_initial_context() self._pvctrls[pvparam] = self._pvs[pvparam].get_ctrlvars() result = self._pvctrls[pvparam] if result is None: return default return result.get(ctrl, default) def _put_pv(self, pvparam, value, wait=False): if epics.ca.current_context() is None: epics.ca.use_initial_context() self._pvs[pvparam].put(value, wait=wait, timeout=self.epicstimeout) def _put_pv_blocking(self, pvparam, value, update_rate=0.1, timeout=60): if epics.ca.current_context() is None: epics.ca.use_initial_context() pv = self._pvs[pvparam] pv.put(value, use_complete=True) start = currenttime() while not pv.put_complete: if currenttime() - start > timeout: raise CommunicationError('Timeout in setting %s' % pv.pvname) session.delay(update_rate)
class Motor(HasTimeout, NicosMotor): """This class supports IPC 6-fold, 3-fold and single motor cards. It can be used with the `nicos.devices.generic.Axis` class. """ parameters = { 'addr': Param( 'Bus address of the motor', # 0 for the profibus card adaptor type=oneof(*([0] + list(range(32, 256)))), mandatory=True), 'unit': Param('Motor unit', type=str, default='steps'), 'zerosteps': Param('Motor steps for physical zero', type=float, unit='steps', settable=True), 'slope': Param('Motor slope', type=float, settable=True, unit='steps/main', default=1.0), # those parameters come from the card 'firmware': Param('Firmware version', type=int), 'steps': Param('Last position in steps', settable=True, type=intrange(0, 999999), prefercache=False), 'speed': Param('Motor speed (0..255)', settable=True, type=intrange(0, 255)), 'accel': Param('Motor acceleration (0..255)', settable=True, type=intrange(0, 255)), 'confbyte': Param('Configuration byte', type=intrange(-1, 255), settable=True), 'ramptype': Param('Ramp type', settable=True, type=intrange(1, 4)), 'microstep': Param('Microstepping mode', unit='steps', settable=True, type=oneof(1, 2, 4, 8, 16)), 'min': Param('Lower motorlimit', settable=True, type=intrange(0, 999999), unit='steps'), 'max': Param('Upper motorlimit', settable=True, type=intrange(0, 999999), unit='steps'), 'startdelay': Param('Start delay', type=floatrange(0, 25), unit='s', settable=True, volatile=True), 'stopdelay': Param('Stop delay', type=floatrange(0, 25), unit='s', settable=True, volatile=True), 'divider': Param('Speed divider', settable=True, type=intrange(-1, 7)), # volatile parameters to read/switch card features 'inhibit': Param('Inhibit input', default='off', volatile=True, type=oneofdict({ 0: 'off', 1: 'on' })), 'relay': Param('Relay switch', type=oneofdict({ 0: 'off', 1: 'on' }), settable=True, default='off', volatile=True), 'power': Param('Internal power stage switch', default='on', type=oneofdict({ 0: 'off', 1: 'on' }), settable=True, volatile=True), } parameter_overrides = { 'timeout': Override(mandatory=False, default=360), } attached_devices = { 'bus': Attach('The communication bus', IPCModBus), } errorstates = () def doInit(self, mode): if mode != SIMULATION: self._attached_bus.ping(self.addr) if self._hwtype == 'single': if self.confbyte != self.doReadConfbyte(): self.doWriteConfbyte(self.confbyte) self.log.warning( 'Confbyte mismatch between setup and ' 'card, overriding card value to 0x%02x', self.confbyte) # make sure that the card has the right "last steps" if self.steps != self.doReadSteps(): self.doWriteSteps(self.steps) self.log.warning( 'Resetting stepper position to last known ' 'good value %d', self.steps) self._type = 'stepper motor, ' + self._hwtype else: self._type = 'simulated stepper' def doVersion(self): try: version = self._attached_bus.get(self.addr, 137) except InvalidCommandError: return [] else: return [('IPC motor card', str(version))] def _tosteps(self, value): return int(float(value) * self.slope + self.zerosteps + 0.5) def _fromsteps(self, value): return float(value - self.zerosteps) / self.slope @lazy_property def _hwtype(self): """Returns 'single', 'triple', or 'sixfold', used for features that only one of the card types supports. """ if self._mode == SIMULATION: return 'single' # can't determine value in simulation mode! if self.doReadDivider() == -1: try: self._attached_bus.get(self.addr, 142) return 'sixfold' except InvalidCommandError: return 'single' return 'triple' def doReadUserlimits(self): if self.slope < 0: return (self._fromsteps(self.max), self._fromsteps(self.min)) else: return (self._fromsteps(self.min), self._fromsteps(self.max)) def doWriteUserlimits(self, limits): NicosMotor.doWriteUserlimits(self, limits) if self.slope < 0: self.min = self._tosteps(limits[1]) self.max = self._tosteps(limits[0]) else: self.min = self._tosteps(limits[0]) self.max = self._tosteps(limits[1]) def doReadSpeed(self): return self._attached_bus.get(self.addr, 128) def doWriteSpeed(self, value): self._attached_bus.send(self.addr, 41, value, 3) def doReadAccel(self): return self._attached_bus.get(self.addr, 129) def doWriteAccel(self, value): if self._hwtype != 'single' and value > 31: raise ValueError( self, 'acceleration value %d too big for ' 'non-single cards' % value) self._attached_bus.send(self.addr, 42, value, 3) self.log.info('parameter change not permanent, use _store() ' 'method to write to EEPROM') def doReadRamptype(self): try: return self._attached_bus.get(self.addr, 136) except InvalidCommandError: return 1 def doWriteRamptype(self, value): try: self._attached_bus.send(self.addr, 50, value, 3) except InvalidCommandError as err: raise InvalidValueError(self, 'ramp type not supported by card') from err self.log.info('parameter change not permanent, use _store() ' 'method to write to EEPROM') def doReadDivider(self): if self._mode == SIMULATION: return -1 # can't determine value in simulation mode! try: return self._attached_bus.get(self.addr, 144) except InvalidCommandError: return -1 def doWriteDivider(self, value): try: self._attached_bus.send(self.addr, 60, value, 3) except InvalidCommandError as err: raise InvalidValueError(self, 'divider not supported by card') from err self.log.info('parameter change not permanent, use _store() ' 'method to write to EEPROM') def doReadMicrostep(self): try: # microstepping cards return [1, 2, 4, 8, 16][self._attached_bus.get(self.addr, 141)] except InvalidCommandError: # simple cards only support full or half steps return [1, 2][(self._attached_bus.get(self.addr, 134) & 4) >> 2] def doWriteMicrostep(self, value): if self._hwtype == 'single': if value == 1: self._attached_bus.send(self.addr, 36) elif value == 2: self._attached_bus.send(self.addr, 37) else: raise InvalidValueError( self, 'microsteps > 2 not supported by card') else: self._attached_bus.send(self.addr, 57, [1, 2, 4, 8, 16].index(value), 3) self.log.info('parameter change not permanent, use _store() ' 'method to write to EEPROM') def doReadMax(self): return self._attached_bus.get(self.addr, 131) def doWriteMax(self, value): self._attached_bus.send(self.addr, 44, value, 6) def doReadMin(self): return self._attached_bus.get(self.addr, 132) def doWriteMin(self, value): self._attached_bus.send(self.addr, 45, value, 6) def doReadStepsize(self): return bool(self._attached_bus.get(self.addr, 134) & 4) def doReadConfbyte(self): try: return self._attached_bus.get(self.addr, 135) except InvalidCommandError: return -1 def doWriteConfbyte(self, value): if self._hwtype == 'single': self._attached_bus.send(self.addr, 49, value, 3) else: raise InvalidValueError(self, 'confbyte not supported by card') self.log.info('parameter change not permanent, use _store() ' 'method to write to EEPROM') def doReadStartdelay(self): if self.firmware > 40: try: return self._attached_bus.get(self.addr, 139) / 10.0 except InvalidCommandError: return 0.0 else: return 0.0 def doWriteStartdelay(self, value): if self._hwtype == 'single': self._attached_bus.send(self.addr, 55, int(value * 10), 3) else: raise InvalidValueError(self, 'startdelay not supported by card') self.log.info('parameter change not permanent, use _store() ' 'method to write to EEPROM') def doReadStopdelay(self): if self.firmware > 44: try: return self._attached_bus.get(self.addr, 143) / 10.0 except InvalidCommandError: return 0.0 else: return 0.0 def doWriteStopdelay(self, value): if self._hwtype == 'single': self._attached_bus.send(self.addr, 58, int(value * 10), 3) else: raise InvalidValueError(self, 'stopdelay not supported by card') self.log.info('parameter change not permanent, use _store() ' 'method to write to EEPROM') def doReadFirmware(self): return self._attached_bus.get(self.addr, 137) def doReadSteps(self): return self._attached_bus.get(self.addr, 130) def doWriteSteps(self, value): self.log.debug('setting new steps value: %s', value) self._attached_bus.send(self.addr, 43, value, 6) def doWritePrecision(self, precision): minprec = abs(2. / self.slope) if precision < minprec: self.log.warning('Precision needs to be at least %.3f, adjusting.', minprec) return minprec def doStart(self, target): bus = self._attached_bus target = self._tosteps(target) self.log.debug('target is %d steps', target) self._hw_wait() pos = self._tosteps(self.read(0)) self.log.debug('pos is %d steps', pos) diff = target - pos if diff == 0: return elif diff < 0: bus.send(self.addr, 35) else: bus.send(self.addr, 34) bus.send(self.addr, 46, abs(diff), 6) session.delay(0.1) # moved here from doWait. def doReset(self): bus = self._attached_bus if self.status(0)[0] != status.OK: # busy or error bus.send(self.addr, 33) # stop try: # this might take a while, ignore errors self._hw_wait() except Exception: pass # remember current state actpos = bus.get(self.addr, 130) speed = bus.get(self.addr, 128) accel = bus.get(self.addr, 129) minstep = bus.get(self.addr, 132) maxstep = bus.get(self.addr, 131) bus.send(self.addr, 31) # reset card session.delay(0.2) if self._hwtype != 'single': # triple and sixfold cards need a LONG time for resetting session.delay(2) # update state bus.send(self.addr, 41, speed, 3) bus.send(self.addr, 42, accel, 3) bus.send(self.addr, 45, minstep, 6) bus.send(self.addr, 44, maxstep, 6) bus.send(self.addr, 43, actpos, 6) def doStop(self): if self._hwtype == 'single': self._attached_bus.send(self.addr, 52) else: self._attached_bus.send(self.addr, 33) session.delay(0.2) def doRead(self, maxage=0): value = self._attached_bus.get(self.addr, 130) self._params['steps'] = value # save last valid position in cache if self._cache: self._cache.put(self, 'steps', value) self.log.debug('value is %d', value) return self._fromsteps(value) def doReadRelay(self): return 'on' if self._attached_bus.get(self.addr, 134) & 8 else 'off' def doWriteRelay(self, value): if value in [0, 'off']: self._attached_bus.send(self.addr, 39) elif value in [1, 'on']: self._attached_bus.send(self.addr, 38) def doReadInhibit(self): return (self._attached_bus.get(self.addr, 134) & 16) == 16 def doReadPower(self): return 'on' if self._attached_bus.get(self.addr, 134) & 16384 else 'off' def doWritePower(self, value): if value in [0, 'off']: self._attached_bus.send(self.addr, 53) elif value in [1, 'on']: self._attached_bus.send(self.addr, 54) def doReadPrecision(self): # precision: 1 step return 2. / abs(self.slope) def doStatus(self, maxage=0): state = self._attached_bus.get(self.addr, 134) st = status.OK msg = '' # msg += (state & 0x2) and ', backward' or ', forward' # msg += (state & 0x4) and ', halfsteps' or ', fullsteps' if state & 0x10: msg += ', inhibit active' if state & 0x80: msg += ', reference switch active' if state & 0x100: msg += ', software limit - reached' if state & 0x200: msg += ', software limit + reached' if state & 0x4000 == 0: msg += ', external power stage enabled' if state & 0x20: if self._hwtype == 'sixfold' and self.firmware < 63: msg += ', limit switch + active' else: msg += ', limit switch - active' if state & 0x40: if self._hwtype == 'sixfold' and self.firmware < 63: msg += ', limit switch - active' else: msg += ', limit switch + active' if self._hwtype == 'single': msg += (state & 0x8) and ', relais on' or ', relais off' if state & 0x8: # on single cards, if relay is ON, card is supposedly BUSY st = status.BUSY if state & 0x8000: st = status.BUSY msg += ', waiting for start/stopdelay' # check error states last if state & (0x20 | 0x40) == 0x60: st = status.ERROR msg = msg.replace( 'limit switch - active, limit switch + active', 'EMERGENCY STOP pressed or both limit switches ' 'broken') if state & 0x400: st = status.ERROR msg += ', device overheated' if state & 0x800: st = status.ERROR msg += ', motor undervoltage' if state & 0x1000: st = status.ERROR msg += ', motor not connected or leads broken' if state & 0x2000: st = status.ERROR msg += ', hardware failure or device not reset after power-on' # if it's moving, it's not in error state! (except if the emergency # stop is active) if state & 0x1 and (state & (0x20 | 0x40) != 0x60): st = status.BUSY msg = ', moving' + msg self.log.debug('status is %d:%s', st, msg[2:]) return st, msg[2:] def doSetPosition(self, target): """Adjust the current stepper position of the IPC-stepper card to match the given position. This is in contrast to the normal behaviour which just adjusts the zerosteps, but IPC cards have a limited range, so it is crucial to stay within that. So we 'set' the position of the card instead of adjusting our zerosteps. """ self.log.debug('setPosition: %s', target) value = self._tosteps(target) self.doWriteSteps(value) self._params['steps'] = value # save last valid position in cache if self._cache: self._cache.put(self, 'steps', value) @usermethod def _store(self): self._attached_bus.send(self.addr, 40) self.log.info('parameters stored to EEPROM') @usermethod def _printconfig(self): byte = self.confbyte c = '' if byte & 1: c += 'limit switch 1: high = active\n' else: c += 'limit switch 1: low = active\n' if byte & 2: c += 'limit switch 2: high = active\n' else: c += 'limit switch 2: low = active\n' if byte & 4: c += 'inhibit entry: high = active\n' else: c += 'inhibit entry: low = active\n' if byte & 8: c += 'reference switch: high = active\n' else: c += 'reference switch: low = active\n' if byte & 16: c += 'use external powerstage\n' else: c += 'use internal powerstage\n' if byte & 32: c += 'leads testing disabled\n' else: c += 'leads testing enabled\n' if byte & 64: c += 'reversed limit switches\n' else: c += 'normal limit switch order\n' if byte & 128: c += 'freq-range: 8-300Hz\n' else: c += 'freq-range: 90-3000Hz\n' self.log.info(c)
class ChopperParams(Moveable): """Setting chopper parameters in terms of (frequency, opening angle).""" valuetype = tupleof(floatrange(0, 100), floatrange(0, 90)) hardware_access = False attached_devices = { 'freq1': Attach('The frequency of the first chopper', HasPrecision), 'freq2': Attach('The frequency of the second chopper', HasPrecision), 'phase1': Attach('The phase of the first chopper', HasPrecision), 'phase2': Attach('The phase of the second chopper', HasPrecision), } parameter_overrides = { 'fmtstr': Override(default='(%.1f, %.1f)'), 'unit': Override(mandatory=False, default=''), } def doIsAllowed(self, pos): freq = pos[0] # forbidden ranges 0-5 and 25-32 Hz (0 must be allowed and means OFF) if 0 < freq <= 5 or 25 <= freq <= 32: return False, 'moving to "forbidden" frequency ranges ' \ '0-5 Hz and 25-32 Hz is not allowed' return True, '' def doStart(self, pos): if pos[0] == 0: if self._attached_freq1.read(0) == 0: return for dev in (self._attached_phase1, self._attached_phase2, self._attached_freq1, self._attached_freq2): dev.start(0) return (freq, opening) = pos self._attached_freq1.start(freq) self._attached_freq2.start(freq) # calculate phases of the two choppers (they should be around 180deg) p0 = 90.0 - opening # p0: phase difference for given opening angle p1 = 180.0 - p0/2.0 p2 = 180.0 + p0/2.0 self._attached_phase1.start(p1) self._attached_phase2.start(p2) # doStatus provided by adevs is enough def doRead(self, maxage=0): freq = self._attached_freq1.read(maxage) p1 = self._attached_phase1.read(maxage) p2 = self._attached_phase2.read(maxage) if freq == p1 == p2 == 0: return (0.0, 0.0) opening = 90.0 - (p2 - p1) return (freq, opening) def doIsAtTarget(self, pos, target): # take precision into account tfreq, topen = target rfreq, ropen = pos return abs(tfreq - rfreq) < self._attached_freq1.precision and \ abs(topen - ropen) < self._attached_phase1.precision
class PowderSample(Sample): """Powder sample with the mur parameter used at SINQ.""" parameters = { 'mur' : Param('Sample muR', type=floatrange(.0,1.),settable=True,category='sample'), }
class PyTangoDevice(HasCommunication): """ Basic PyTango device. The PyTangoDevice uses an internal PyTango.DeviceProxy but wraps command execution and attribute operations with logging and exception mapping. """ hardware_access = True parameters = { 'tangodevice': Param('Tango device name', type=tangodev, mandatory=True, preinit=True), 'tangotimeout': Param('TANGO network timeout for this process', unit='s', type=floatrange(0.0, 1200), default=3, settable=True, preinit=True), } parameter_overrides = { 'unit': Override(mandatory=False), } tango_status_mapping = { PyTango.DevState.ON: status.OK, PyTango.DevState.ALARM: status.WARN, PyTango.DevState.OFF: status.DISABLED, PyTango.DevState.FAULT: status.ERROR, PyTango.DevState.MOVING: status.BUSY, } # Since each DeviceProxy leaks a few Python objects, we can't just # drop them when the device fails to initialize, and create another one. # It is also not required since they reconnect automatically. proxy_cache = {} def doPreinit(self, mode): # Wrap PyTango client creation (so even for the ctor, logging and # exception mapping is enabled). self._createPyTangoDevice = self._applyGuardToFunc( self._createPyTangoDevice, 'constructor') self._dev = None # Don't create PyTango device in simulation mode if mode != SIMULATION: self._dev = self._createPyTangoDevice(self.tangodevice) else: self._dev = HardwareStub(self) def doStatus(self, maxage=0): # Map Tango state to NICOS status nicosState = self.tango_status_mapping.get(self._dev.State(), status.UNKNOWN) return (nicosState, self._dev.Status()) def _hw_wait(self): """Wait until hardware status is not BUSY.""" while PyTangoDevice.doStatus(self, 0)[0] == status.BUSY: session.delay(self._base_loop_delay) def doVersion(self): return [(self.tangodevice, self._dev.version)] def doReset(self): self._dev.Reset() while self._dev.State() == PyTango.DevState.INIT: session.delay(self._base_loop_delay) def _setMode(self, mode): super(PyTangoDevice, self)._setMode(mode) # remove the Tango device on entering simulation mode, to prevent # accidental access to the hardware if mode == SIMULATION: self._dev = HardwareStub(self) def _getProperty(self, name, dev=None): """ Utility function for getting a property by name easily. """ if dev is None: dev = self._dev # return value is: [name, value, name, value, ...] props = dev.GetProperties() propnames = props[::2] return props[2*propnames.index(name) + 1] \ if name in propnames else None def doReadUnit(self): """For devices with a unit attribute.""" attrInfo = self._dev.attribute_query('value') # prefer configured unit if nothing is set on the Tango device if attrInfo.unit == 'No unit': return self._config.get('unit', '') return attrInfo.unit def _createPyTangoDevice(self, address): # pylint: disable=E0202 """ Creates the PyTango DeviceProxy and wraps command execution and attribute operations with logging and exception mapping. """ check_tango_host_connection(self.tangodevice, self.tangotimeout) proxy_key = (self._name, address) if proxy_key not in PyTangoDevice.proxy_cache: PyTangoDevice.proxy_cache[proxy_key] = PyTango.DeviceProxy(address) device = PyTangoDevice.proxy_cache[proxy_key] device.set_timeout_millis(int(self.tangotimeout * 1000)) # detect not running and not exported devices early, because that # otherwise would lead to attribute errors later try: device.State except AttributeError: raise NicosError(self, 'connection to Tango server failed, ' 'is the server running?') return self._applyGuardsToPyTangoDevice(device) def _applyGuardsToPyTangoDevice(self, dev): """ Wraps command execution and attribute operations of the given device with logging and exception mapping. """ dev.command_inout = self._applyGuardToFunc(dev.command_inout) dev.write_attribute = self._applyGuardToFunc(dev.write_attribute, 'attr_write') dev.read_attribute = self._applyGuardToFunc(dev.read_attribute, 'attr_read') dev.attribute_query = self._applyGuardToFunc(dev.attribute_query, 'attr_query') return dev def _applyGuardToFunc(self, func, category='cmd'): """ Wrap given function with logging and exception mapping. """ def wrap(*args, **kwds): info = category + ' ' + args[0] if args else category # handle different types for better debug output if category == 'cmd': self.log.debug('[Tango] command: %s%r', args[0], args[1:]) elif category == 'attr_read': self.log.debug('[Tango] read attribute: %s', args[0]) elif category == 'attr_write': self.log.debug('[Tango] write attribute: %s => %r', args[0], args[1:]) elif category == 'attr_query': self.log.debug('[Tango] query attribute properties: %s', args[0]) elif category == 'constructor': self.log.debug('[Tango] device creation: %s', args[0]) try: result = func(*args, **kwds) return self._com_return(result, info) except Exception as err: self._com_raise(err, info) elif category == 'internal': self.log.debug('[Tango integration] internal: %s%r', func.__name__, args) else: self.log.debug('[Tango] call: %s%r', func.__name__, args) return self._com_retry(info, func, *args, **kwds) # hide the wrapping wrap.__name__ = func.__name__ return wrap def _com_return(self, result, info): # explicit check for loglevel to avoid expensive reprs if self.loglevel == 'debug': if isinstance(result, PyTango.DeviceAttribute): the_repr = repr(result.value)[:300] else: # This line explicitly logs '=> None' for commands which # does not return a value. This indicates that the command # execution ended. the_repr = repr(result)[:300] self.log.debug('\t=> %s', the_repr) return result def _tango_exc_desc(self, err): exc = str(err) if err.args: exc = err.args[0] # Can be str or DevError if isinstance(exc, PyTango.DevError): return describe_dev_error(exc) return exc def _tango_exc_reason(self, err): if err.args and isinstance(err.args[0], PyTango.DevError): return err.args[0].reason.strip() return '' def _com_warn(self, retries, name, err, info): if self._tango_exc_reason(err) in FATAL_REASONS: self._com_raise(err, info) if retries == self.comtries - 1: self.log.warning('%s failed, retrying up to %d times: %s', info, retries, self._tango_exc_desc(err)) def _com_raise(self, err, info): reason = self._tango_exc_reason(err) exclass = REASON_MAPPING.get( reason, EXC_MAPPING.get(type(err), NicosError)) fulldesc = self._tango_exc_desc(err) self.log.debug('[Tango] error: %s', fulldesc) raise exclass(self, fulldesc)
class TacoDevice(HasCommunication): """Mixin class for TACO devices. Use it in concrete device classes like this:: class Counter(TacoDevice, Measurable): taco_class = IO.Counter # more overwritten methods i.e., put TacoDevice first in the base class list. TacoDevice provides the following methods already: * `.doVersion` (returns TACO device version) * `.doPreinit` (creates the TACO device from the `tacodevice` parameter) * `.doRead` (reads the TACO device) * `.doStatus` (returns status.OK for ON and DEVICE_NORMAL, ERROR otherwise) * `.doReset` (resets the TACO device) * `.doReadUnit` (reads the unit parameter from the TACO device if not configured in setup) You can however override them and provide your own specialized implementation. TacoDevice subclasses will automatically log all calls to TACO if their loglevel is DEBUG. TacoDevice also has the following class attributes, which can be overridden in derived classes: * `taco_class` -- the Python class to use for the TACO client * `taco_resetok` -- a boolean value indicating if the device can be reset during connection if it is in error state * `taco_errorcodes` -- a dictionary mapping TACO error codes to NICOS exception classes The following utility methods are provided: .. automethod:: _taco_guard .. automethod:: _taco_update_resource .. automethod:: _create_client """ parameters = { 'tacodevice': Param('TACO device name', type=tacodev, mandatory=True, preinit=True), 'tacotimeout': Param('TACO network timeout for this process', unit='s', type=floatrange(0.0, 1200), default=3, settable=True, preinit=True), } parameter_overrides = { # the unit isn't mandatory -- TACO usually knows it already 'unit': Override(mandatory=False), } _TACO_STATUS_MAPPING = { # OK states TACOStates.ON: status.OK, TACOStates.DEVICE_NORMAL: (status.OK, 'idle'), TACOStates.POSITIVE_ENDSTOP: (status.OK, 'limit switch +'), TACOStates.NEGATIVE_ENDSTOP: (status.OK, 'limit switch -'), TACOStates.STOPPED: (status.OK, 'idle or paused'), TACOStates.PRESELECTION_REACHED: status.OK, TACOStates.DISABLED: status.OK, # BUSY states # explicit ramp string as there seem to be some inconsistencies TACOStates.RAMP: (status.BUSY, 'ramping'), TACOStates.MOVING: status.BUSY, TACOStates.STOPPING: status.BUSY, TACOStates.INIT: (status.BUSY, 'initializing taco device / hardware'), TACOStates.RESETTING: status.BUSY, TACOStates.STOP_REQUESTED: status.BUSY, TACOStates.COUNTING: status.BUSY, TACOStates.STARTED: status.BUSY, # NOTREACHED states TACOStates.UNDEFINED: status.NOTREACHED, # WARN states TACOStates.ALARM: status.WARN, # ERROR states TACOStates.FAULT: status.ERROR, TACOStates.BLOCKED: status.ERROR, TACOStates.TRIPPED: status.ERROR, TACOStates.OVERFLOW: status.ERROR, TACOStates.OFF: status.ERROR, TACOStates.DEVICE_OFF: status.ERROR, TACOStates.ON_NOT_REACHED: status.ERROR, } # the TACO client class to instantiate taco_class = None # whether to call deviceReset() if the initial switch-on fails taco_resetok = True # additional TACO error codes mapping to Nicos exception classes taco_errorcodes = {} # TACO device instance _dev = None def doPreinit(self, mode): if self.loglevel == 'debug': self._taco_guard = self._taco_guard_log if self.taco_class is None: raise ProgrammingError('missing taco_class attribute in class ' + self.__class__.__name__) if mode != SIMULATION: self._dev = self._create_client() else: self._dev = HardwareStub(self) def doShutdown(self): if self._dev: self._dev.disconnectClient() del self._dev def _setMode(self, mode): super()._setMode(mode) # remove the TACO device on entering simulation mode, to prevent # accidental access to the hardware if mode == SIMULATION: # keep the device instance around to avoid destruction (which can # mess with the TACO connections in the main process if simulation # has been forked off) self._orig_dev = self._dev self._dev = HardwareStub(self) def doVersion(self): return [(self.tacodevice, self._taco_guard(self._dev.deviceVersion))] def doRead(self, maxage=0): return self._taco_guard(self._dev.read) def doStatus(self, maxage=0): for i in range(self.comtries or 1): if i: session.delay(self.comdelay) tacoState = self._taco_guard(self._dev.deviceState) if tacoState != TACOStates.FAULT: break state = self._TACO_STATUS_MAPPING.get(tacoState, status.ERROR) if isinstance(state, tuple): return state statusStr = self._taco_guard(self._dev.deviceStatus) return (state, statusStr) def doReset(self): self._taco_reset(self._dev) def doReadUnit(self): # explicitly configured unit has precendence if 'unit' in self._config: return self._config['unit'] if hasattr(self._dev, 'unit'): return self._taco_guard(self._dev.unit) return self.parameters['unit'].default def doWriteUnit(self, value): if hasattr(self._dev, 'setUnit'): self._taco_guard(self._dev.setUnit, value) if 'unit' in self._config: if self._config['unit'] != value: self.log.warning( 'configured unit %r in configuration differs ' 'from current unit %r', self._config['unit'], value) def doUpdateTacotimeout(self, value): if not self._sim_intercept and self._dev: if value != 3.0: self.log.warning( '%r: client network timeout changed to: ' '%.2f s', self.tacodevice, value) self._taco_guard(self._dev.setClientNetworkTimeout, value) def doUpdateLoglevel(self, value): super().doUpdateLoglevel(value) self._taco_guard = value == 'debug' and self._taco_guard_log or \ self._taco_guard_nolog # internal utilities def _create_client(self, devname=None, class_=None, resetok=None, timeout=None): """Create a new TACO client to the device given by *devname*, using the Python class *class_*. Initialize the device in a consistent state, handling eventual errors. If no arguments are given, the values of *devname*, *class_*, *resetok* and *timeout* are taken from the class attributes *taco_class* and *taco_resetok* as well as the device parameters *tacodevice* and *tacotimeout*. This is done during `.doPreinit`, so that you usually don't have to call this method in TacoDevice subclasses. You can use this method to create additional TACO clients in a device implementation that uses more than one TACO device. """ if devname is None: devname = self.tacodevice if class_ is None: class_ = self.taco_class if resetok is None: resetok = self.taco_resetok if timeout is None: timeout = self.tacotimeout self.log.debug('creating %s TACO device', class_.__name__) try: dev = class_(devname) self._check_server_running(dev) except TACOError as err: self._raise_taco( err, 'Could not connect to device %r; make sure ' 'the device server is running' % devname) try: if timeout != 0: if timeout != 3.0: self.log.warning( 'client network timeout changed to: ' '%.2f s', timeout) dev.setClientNetworkTimeout(timeout) except TACOError as err: self.log.warning( 'Setting TACO network timeout failed: ' '[TACO %d] %s', err.errcode, err) try: if dev.isDeviceOff(): dev.deviceOn() except TACOError as err: self.log.warning( 'Switching TACO device %r on failed: ' '[TACO %d] %s', devname, err.errcode, err) try: if dev.deviceState() == TACOStates.FAULT: if resetok: dev.deviceReset() dev.deviceOn() except TACOError as err: self._raise_taco( err, 'Switching device %r on after ' 'reset failed' % devname) return dev def _check_server_running(self, dev): dev.deviceVersion() def _taco_guard_log(self, function, *args): """Like _taco_guard(), but log the call.""" self.log.debug('TACO call: %s%r', function.__name__, args) if not self._dev: raise NicosError(self, 'TACO Device not initialised') self._com_lock.acquire() try: ret = function(*args) except TACOError as err: # for performance reasons, starting the loop and querying # self.comtries only triggers in the error case if self.comtries > 1 or err == DevErr_RPCTimedOut: tries = 2 if err == DevErr_RPCTimedOut and self.comtries == 1 \ else self.comtries - 1 self.log.warning('TACO %s failed, retrying up to %d times', function.__name__, tries, exc=1) while True: session.delay(self.comdelay) tries -= 1 try: if self.taco_resetok and \ self._dev.deviceState() == TACOStates.FAULT: self._dev.deviceInit() session.delay(self.comdelay) ret = function(*args) self.log.debug('TACO return: %r', ret) return ret except TACOError as err: if tries == 0: break # and fall through to _raise_taco self.log.warning('TACO %s failed again', function.__name__, exc=True) self.log.debug('TACO exception: %r', err) self._raise_taco(err) else: self.log.debug('TACO return: %r', ret) return ret finally: self._com_lock.release() def _taco_guard_nolog(self, function, *args): """Try running the TACO function, and raise a NicosError on exception. A more specific NicosError subclass is chosen if appropriate. For example, database-related errors are converted to `.CommunicationError`. A TacoDevice subclass can add custom error code to exception class mappings by using the `.taco_errorcodes` class attribute. If the `comtries` parameter is > 1, the call is retried accordingly. """ if not self._dev: raise NicosError(self, 'TACO device not initialised') self._com_lock.acquire() try: return function(*args) except TACOError as err: # for performance reasons, starting the loop and querying # self.comtries only triggers in the error case if self.comtries > 1 or err == DevErr_RPCTimedOut: tries = 2 if err == DevErr_RPCTimedOut and self.comtries == 1 \ else self.comtries - 1 self.log.warning('TACO %s failed, retrying up to %d times', function.__name__, tries) while True: session.delay(self.comdelay) tries -= 1 try: return function(*args) except TACOError as err: if tries == 0: break # and fall through to _raise_taco self._raise_taco(err, '%s%r' % (function.__name__, args)) finally: self._com_lock.release() _taco_guard = _taco_guard_nolog def _taco_update_resource(self, resname, value): """Update the TACO resource *resname* to *value* (both must be strings), switching the device off and on. """ if not self._dev: raise NicosError(self, 'TACO device not initialised') self._com_lock.acquire() try: self.log.debug('TACO resource update: %s %s', resname, value) self._dev.deviceOff() self._dev.deviceUpdateResource(resname, value) self._dev.deviceOn() self.log.debug('TACO resource update successful') except TACOError as err: self._raise_taco(err, 'While updating %s resource' % resname) finally: self._com_lock.release() def _raise_taco(self, err, addmsg=None): """Raise a suitable NicosError for a given TACOError instance.""" tb = sys.exc_info()[2] code = err.errcode cls = NicosError if code in self.taco_errorcodes: cls = self.taco_errorcodes[code] elif code < 50: # error numbers 0-50: RPC call errors cls = CommunicationError elif 400 <= code < 500: # error number 400-499: database system error messages cls = CommunicationError elif code == DevErr_RangeError: cls = LimitError elif code in (DevErr_InvalidValue, DevErr_ExecutionDenied): cls = InvalidValueError elif code in (DevErr_IOError, DevErr_InternalError, DevErr_RuntimeError, DevErr_SystemError): cls = CommunicationError msg = '[TACO %d] %s' % (err.errcode, err) if addmsg is not None: msg = addmsg + ': ' + msg exc = cls(self, msg, tacoerr=err.errcode) raise exc.with_traceback(tb) def _taco_reset(self, client, resetcall='deviceReset'): try: hostname = client.getServerHost() servername = client.getServerProcessName() personalname = client.getServerPersonalName() self.log.info( 'Resetting TACO device; if this does not help try ' 'restarting the %s named %s on host %s.', servername, personalname, hostname) except AttributeError: # older version without these API calls self.log.info('Resetting TACO device; if this does not help try ' 'restarting the server.') try: if resetcall == 'deviceReset': self._taco_guard(client.deviceReset) else: self._taco_guard(client.deviceInit) except Exception as err: self.log.warning('%s failed with %s', resetcall, err) if self._taco_guard(client.isDeviceOff): self._taco_guard(client.deviceOn)
class PumaCoupledAxis(HasPrecision, HasLimits, Moveable): """PUMA multianalyzer coupled 'att' axis. This axis moves two axes and the movement of one axis is allowed only in a limited range in correlation to the current position of the other axis. At the moment the method is written especially for the analyser 2 theta movement, if the multianalyser is on PUMA and does not use a threaded movement. """ hardware_access = False attached_devices = { 'tt': Attach('tth', Moveable), 'th': Attach('th', Moveable), } parameter_overrides = { # 'timeout': Override(settable=False, default=600), 'precision': Override(settable=False, default=.1), 'abslimits': Override(mandatory=False, default=(-60, 0)), } parameters = { 'difflimit': Param( 'increment of the allowed angle movement of one ' 'axis without collision of the other axis', type=floatrange(0, 60), settable=False, default=3.), 'dragerror': Param( 'Maximum deviation between both axes when read out ' 'during a positioning step', unit='main', default=1, settable=True), '_status': Param('read only status', type=bool, settable=False, internal=True, default=False), } @property def tt(self): """Return the attached 'tt' device.""" return self._attached_tt @property def th(self): """Return the attached 'th' device.""" return self._attached_th def doInit(self, mode): """Init device.""" # maximum angle difference allowed for the two axes before movement # or initialization try: self.__setDiffLimit() except PositionError: pass self._setROParam('_status', False) def doIsAllowed(self, target): """Check whether position given by target is allowed.""" tt_allowed = self.tt.isAllowed(target) th_allowed = self.th.isAllowed(-target) if tt_allowed[0] and th_allowed[0]: if self._checkZero(self.tt.read(0), self.th.read(0)): return True, '' return False, '%s and %s are not close enough' % (self.tt, self.th) return False, '; '.join([th_allowed[1], tt_allowed[1]]) def doStart(self, position): """Move coupled axis (tt/th). The tt axis should without moving the coupled axis th (tt + th == 0) """ if self.doStatus(0)[0] == status.BUSY: self.log.error('device busy') target = (position, -position) if not self._checkReachedPosition(target): try: self._setROParam('_status', True) self.__setDiffLimit() tt = self.tt.read(0) th = self.th.read(0) if abs(tt - target[0]) > self.difflimit or \ abs(th - target[1]) > self.difflimit: delta = abs(tt - position) mod = math.fmod(delta, self.difflimit) steps = int(delta / self.difflimit) self.log.debug('delta/self.difflimit, mod: %s, %s', steps, mod) if tt > position: self.log.debug('case tt > position') delta = -self.difflimit elif tt < position: self.log.debug('case tt < position') delta = self.difflimit for i in range(1, steps + 1): d = i * delta self.log.debug('step: %d, move tt: %.2f, th: %.2f:', i, tt + d, th - d) self.__setDiffLimit() self.th.move(th - d) self.tt.move(tt + d) self._hw_wait() else: steps = 1 if not self._checkReachedPosition(target): self.log.debug('step: %d, move tt: %.2f, th: %.2f:', steps, position, -position) self.__setDiffLimit() self.th.move(-position) self.tt.move(position) self._hw_wait() if not self._checkReachedPosition(target): PositionError( self, "couldn't reach requested position " '%7.3f' % position) finally: self.log.debug('Clear status flag') self._setROParam('_status', False) self.poll() def _hw_wait(self): loops = 0 final_exc = None devlist = [self.tt, self.th] while devlist: loops += 1 for dev in devlist[:]: try: done = dev.doStatus(0)[0] except Exception: dev.log.exception('while waiting') final_exc = filterExceptions(sys.exc_info(), final_exc) # remove this device from the waiters - we might still # have its subdevices in the list so that _hw_wait() # should not return until everything is either OK or # ERROR devlist.remove(dev) if done == status.BUSY: # we found one busy dev, normally go to next iteration # until this one is done (saves going through the whole # list of devices and doing unnecessary HW communication) if loops % 10: break # every 10 loops, go through everything to get an accurate # display in the action line continue devlist.remove(dev) if devlist: session.delay(self._base_loop_delay) if final_exc: reraise(*final_exc) def doReset(self): """Reset individual axes, set angle between axes within one degree.""" multiReset([self.tt, self.th]) tt, th = self.tt.read(0), self.th.read(0) if not self._checkZero(tt, th): if (tt + th) <= self.difflimit: self.tt.maw(-th) return raise PositionError( self, '%s and %s are not close enough' % (self.tt, self.th)) def doRead(self, maxage=0): """Read back the value of the 2theta axis.""" tt, _ = self.tt.read(maxage), self.th.read(maxage) return tt def doStatus(self, maxage=0): """Return status of device in dependence of the individual axes.""" if self._status: return status.BUSY, 'moving' return multiStatus(self._adevs, maxage) def __setDiffLimit(self): """Set limits of device in dependence of allowed set of difflimit.""" if self._checkZero(self.tt.read(0), self.th.read(0)): for ax in [self.tt, self.th]: p = ax.read(0) self.log.debug('%s, %s', ax, p) limit = self.difflimit absMin = p - (limit + 2. * ax.precision - 0.0001) absMax = p + (limit + 2. * ax.precision - 0.0001) self.log.debug('user limits for %s: %r', ax.name, (absMin, absMax)) # ax.userlimits = (absMin, absMax) self.log.debug('user limits for %s: %r', ax.name, ax.userlimits) else: raise PositionError( self, 'cannot set new limits; coupled axes %s ' 'and %s are not close enough difference > %f ' 'deg.' % (self.tt, self.th, self.difflimit)) def _checkReachedPosition(self, pos): """Check if requested positions are reached for individual axes.""" if pos is None: return False # This may appear in the test and fresh setup systems if None in (self.tt.target, self.th.target): return False return self.tt.isAtTarget(pos[0]) and self.th.isAtTarget(pos[1]) def _checkZero(self, tt, th): """Check if the two axes are within the allowed limit.""" return abs(tt + th) <= self.dragerror
class McStasImage(ImageChannelMixin, PassiveChannel): """Image channel based on McStas simulation. This channel should be used together with `McStasTimerChannel` which provides the preselection [s] for calculating the number of simulated neutron counts: Ncounts = preselection [s] * ratio [cts/s] Note: Please configure **ratio** to match the average simulated neutron counts per second on your system. """ _mythread = None _process = None _started = None parameters = { 'size': Param( 'Detector size in pixels (x, y)', settable=False, type=tupleof(intrange(1, 8192), intrange(1, 8192)), default=(1, 1), ), 'mcstasprog': Param('Name of the McStas simulation executable', type=str, settable=False), 'mcstasdir': Param('Directory where McStas stores results', type=str, default='%(session.experiment.dataroot)s' '/singlecount', settable=False), 'mcstasfile': Param('Name of the McStas data file', type=str, settable=False), 'mcsiminfo': Param('Name for the McStas Siminfo file', settable=False, type=str, default='mccode.sim'), 'ratio': Param( 'Simulated neutrons per second for this machine. Please' ' tune this parameter according to your hardware for ' ' realistic count times', settable=False, type=floatrange(1e3), default=1e6), 'ci': Param('Constant ci multiplied with simulated intensity I', settable=False, type=floatrange(1.)), # preselection time, usually set by McStasTimer 'preselection': Param('Preset value for this channel', type=float, settable=True, default=1.), } def doInit(self, mode): self.arraydesc = ArrayDesc(self.name, self.size, '<u4') self._workdir = os.getcwd() self._start_time = None def doReadArray(self, quality): self.log.debug('quality: %s', quality) if quality == LIVE: self._send_signal(SIGUSR2) elif quality == FINAL: if self._mythread and self._mythread.is_alive(): self._mythread.join(1.) if self._mythread.is_alive(): self.log.exception("Couldn't join readout thread.") else: self._mythread = None self._readpsd(quality) return self._buf def _prepare_params(self): """Return a list of key=value strings. Each entry defines a parameter setting for the mcstas simulation call. examples: param=10 """ raise NotImplementedError('Please implement _prepare_params method') def doPrepare(self): self._mcstas_params = ' '.join(self._prepare_params()) self.log.debug('McStas parameters: %s', self._mcstas_params) self._buf = np.zeros(self.size[::-1]) self.readresult = [0] self._start_time = None self._mcstasdirpath = session.experiment.data.expandNameTemplates( self.mcstasdir)[0] def valueInfo(self): return (Value(self.name + '.sum', unit='cts', type='counter', errors='sqrt', fmtstr='%d'), ) def doStart(self): self._started = True self._mythread = createThread('detector %s' % self, self._run) def doStatus(self, maxage=0): if self._started or (self._mythread and self._mythread.is_alive()): return status.BUSY, 'busy' return status.OK, 'idle' def doFinish(self): self.log.debug('finish') self._started = None self._send_signal(SIGTERM) def _send_signal(self, sig): if self._process and self._process.is_running(): self._process.send_signal(sig) # wait for mcstas releasing fds datafile = path.join(self._mcstasdirpath, self.mcstasfile) siminfo = path.join(self._mcstasdirpath, self.mcsiminfo) try: while self._process and self._process.is_running(): fnames = [f.path for f in self._process.open_files()] if siminfo not in fnames and datafile not in fnames: break session.delay(.01) except (AccessDenied, NoSuchProcess): self.log.debug( 'McStas process already terminated in _send_signal(%r)', sig) self.log.debug('McStas process has written file on signal (%r)', sig) def _run(self): """Run McStas simulation executable. The current settings of the instrument parameters will be transferred to it. """ try: shutil.rmtree(self._mcstasdirpath) except OSError: self.log.warning('could not remove old data') command = '%s -n %d -d %s %s' % ( self.mcstasprog, self.ratio * self.preselection, self._mcstasdirpath, self._mcstas_params, ) self.log.debug('run %s', command) try: self._start_time = time.time() self._process = Popen(command.split(), stdout=PIPE, stderr=PIPE, cwd=self._workdir) out, err = self._process.communicate() if out: self.log.debug('McStas output:') for line in out.splitlines(): self.log.debug('[McStas] %s', line.decode('utf-8', 'ignore')) if err: self.log.warning('McStas found some problems:') for line in err.splitlines(): self.log.warning('[McStas] %s', line.decode('utf-8', 'ignore')) except OSError as e: self.log.error('Execution failed: %s', e) if self._process: self._process.wait() self._process = None self._started = None def _readpsd(self, quality): try: with open(path.join(self._mcstasdirpath, self.mcstasfile), 'r') as f: lines = f.readlines()[-3 * (self.size[0] + 1):] if lines[0].startswith('# Data') and self.mcstasfile in lines[0]: if quality == FINAL: seconds = self.preselection else: seconds = min(time.time() - self._start_time, self.preselection) self._buf = ( np.loadtxt(lines[1:self.size[0] + 1], dtype=np.float32) * self.ci * seconds).astype(np.uint32) self.readresult = [self._buf.sum()] elif quality != LIVE: raise OSError('Did not find start line: %s' % lines[0]) except OSError: if quality != LIVE: self.log.exception('Could not read result file')
class EpicsDevice(DeviceMixinBase): """ Basic EPICS device. """ hardware_access = True valuetype = anytype parameters = { 'epicstimeout': Param('Timeout for getting EPICS PVs', type=none_or(floatrange(0.1, 60)), default=1.0), 'usepva': Param('If true, PVAcess is used instead of ChannelAccess', type=bool, default=False, preinit=True) } # A set of all parameters that indicate PV names. Since PVs are very # limited, an EpicsDevice is expected to use many different PVs a lot # of times. pv_parameters = set() pv_cache_relations = {} # This will store PV objects for each PV param. _pvs = {} _pvctrls = {} def doPreinit(self, mode): # Don't create PVs in simulation mode self._pvs = {} self._pvctrls = {} if mode != SIMULATION: # When there are standard names for PVs (see motor record), the PV names # may be derived from some prefix. To make this more flexible, the pv_parameters # are obtained via a method that can be overridden in subclasses. pv_parameters = self._get_pv_parameters() # For cases where for example readpv and writepv are the same, this dict makes # sure that only one Channel object is created per PV. pv_names = {} for pvparam in pv_parameters: # Retrieve the actual PV-name from (potentially overridden) method pv_name = self._get_pv_name(pvparam) try: pv = pv_names.setdefault(pv_name, self._create_pv(pv_name)) self._pvs[pvparam] = pv pv.setTimeout(self.epicstimeout) self._pvctrls[pvparam] = pv.get('display').toDict().get('display') if self._pvctrls[pvparam] is None: self._pvctrls[pvparam] = pv.get('control').toDict().get('control', {}) except pvaccess.PvaException: raise CommunicationError(self, 'could not connect to PV %r' % pv_name) else: for pvparam in self._get_pv_parameters(): self._pvs[pvparam] = HardwareStub(self) self._pvctrls[pvparam] = {} def doInit(self, mode): if mode != SIMULATION: self._register_pv_callbacks() def _create_pv(self, pv_name): return pvaccess.Channel(pv_name, pvaccess.PVA if self.usepva else pvaccess.CA) def _get_pv_parameters(self): # The default implementation of this method simply returns the pv_parameters set return self.pv_parameters def _get_pv_name(self, pvparam): # In the default case, the name of a PV-parameter is stored in a parameter. # This method can be overridden in subclasses in case the name can be derived # using some other information. return getattr(self, pvparam) def doStatus(self, maxage=0): return status.OK, '' def _setMode(self, mode): super(EpicsDevice, self)._setMode(mode) # remove the PVs on entering simulation mode, to prevent # accidental access to the hardware if mode == SIMULATION: for key in self._pvs: self._pvs[key] = HardwareStub(self) def _get_pv(self, pvparam, field='value', as_string=False): """ Uses pvaccess to obtain a field from a PV. The default field is value, so that val = self._get_pv('readpv') retrieves the value of the PV. To obtain alarm or other status information, the field parameter can be specified: alarm = self._get_pv('readpv', field='alarm') Args: pvparam: The PV parameter to be queried. Is translated to a PV name internally. field: Field of the PV to obtain, default is value. Returns: Value of the queried PV field. """ # result = self._pvs[pvparam].get(field).toDict().get(field) result = _pvget(self._pvs[pvparam], field, as_string) if result is None: # timeout or channel not connected raise CommunicationError(self, 'timed out getting PV %r from EPICS' % self._get_pv_name(pvparam)) return result def _get_pvctrl(self, pvparam, ctrl, default=None, update=False): if update: self._pvctrls[pvparam] = self._pvs[pvparam].get('display').toDict().get('display') if self._pvctrls[pvparam] is None: self._pvctrls[pvparam] = self._pvs[pvparam].get('control').toDict().get( 'control', {}) return self._pvctrls[pvparam].get(ctrl, default) def _get_pv_datatype(self, pvparam): pv_data = self._pvs[pvparam].get().getStructureDict()['value'] if not isinstance(pv_data, list): return FTYPE_TO_VALUETYPE.get(pv_data, anytype) else: return [FTYPE_TO_VALUETYPE.get(dt, anytype) for dt in pv_data] def _put_pv(self, pvparam, value, wait=True): self._pvs[pvparam].put(value) def _put_pv_blocking(self, pvparam, value, update_rate=0.1): # TODO: figure out why putGet segfaults self._put_pv(pvparam, value) def _register_pv_callbacks(self): """ If this is a poller session, monitor the PVs specified in the ``pv_cache_relations`` member for updates and put the values into the cache. This happens in addition to polling, but makes sure that values get inserted into the cache immediately when they are available. This example would map the value of readpv to the ``value`` of the device: pv_cache_relations = { 'readpv': 'value', } This method has to be called explicitly in ``doInit``, should it be re-implemented. """ if session.sessiontype == POLLER: for pvparam in self._get_pv_parameters(): corresponding_cache_key = self.pv_cache_relations.get(pvparam) if corresponding_cache_key is not None: self._register_pv_update_callback(pvparam, corresponding_cache_key) def _register_pv_update_callback(self, pvparam, cache_key, pv_field='value'): """ Subscribes to a PV monitor that updates the cache whenever the PV is updated via ChannelAccess. Args: pvparam: The pvparam to subscribe to, for example readpv or writepv cache_key: The cache key that corresponds to the PV's value pv_field: Field of the PV to obtain, default is value. """ self.log.info('Registering callback for %s (PV: %s)', pvparam, self._get_pv_name(pvparam)) def update_callback(pv_object, obj=self, key=cache_key): if isinstance(obj, Readable): if key == 'value' or key == 'status': ret = obj.poll() ct = currenttime() obj._cache.put(self, 'status', ret[0], ct, self.maxage) obj._cache.put(self, 'value', ret[1], ct, self.maxage) else: obj._pollParam(key) pv = self._pvs[pvparam] pv.setMonitorMaxQueueLength(10) pv.subscribe('_'.join((self.name, pvparam, cache_key, 'poller')), update_callback) #if not pv.isMonitorActive(): pv.startMonitor('') def _get_mapped_epics_status(self): # Checks the status and severity of all the associated PVs. # Returns the worst status (error prone first) and # a list of all associated pvs having that error status_map = {} for name in self._pvs: epics_status = self._get_pvctrl(name, 'status', update=True) epics_severity = self._get_pvctrl(name, 'severity') mapped_status = STAT_TO_STATUS.get(epics_status, None) if mapped_status is None: mapped_status = SEVERITY_TO_STATUS.get( epics_severity, status.UNKNOWN) status_map.setdefault(mapped_status, []).append( self._get_pv_name(name)) return max(status_map.items())
class Chopper(Moveable): """Switcher for the TOF setting. This controls the chopper phase and opening angle, as well as the TOF slice settings for the detector. Presets depend on the target wavelength as well as the detector position. """ hardware_access = False attached_devices = { 'selector': Attach('Selector preset switcher', SelectorSwitcher), 'det_pos': Attach('Detector preset switcher', DetectorPosSwitcherMixin), 'daq': Attach('KWSDetector device', KWSDetector), 'params': Attach('Chopper param device', Moveable), } parameters = { 'resolutions': Param('Possible values for the resolution', unit='%', type=listof(float), mandatory=True), 'channels': Param('Desired number of TOF channels', # TODO: max channels? type=intrange(1, 1024), default=64, settable=True), 'shade': Param('Desired overlap of spectrum edges', type=floatrange(0.0, 1.0), default=0.0, settable=True, category='general'), 'tauoffset': Param('Additional offset for time of flight', type=floatrange(0.0), default=0.0, settable=True, category='general'), 'nmax': Param('Maximum number of acquisition frames', type=intrange(1, 128), default=25, settable=True), 'calcresult': Param('Last calculated setting', type=tupleof(float, float, int), settable=True, internal=True), } parameter_overrides = { 'unit': Override(default='', mandatory=False), } def doInit(self, mode): self.valuetype = oneof('off', 'manual', *('%.1f%%' % v for v in self.resolutions)) def _getWaiters(self): return [self._attached_params] def doUpdateChannels(self, value): # invalidate last calculated result when changing these parameters if self._mode == MASTER and 'channels' in self._params: self.calcresult = (0, 0, 0) def doUpdateShade(self, value): if self._mode == MASTER and 'shade' in self._params: self.calcresult = (0, 0, 0) def doUpdateTauoffset(self, value): if self._mode == MASTER and 'tauoffset' in self._params: self.calcresult = (0, 0, 0) def doUpdateNmax(self, value): if self._mode == MASTER and 'nmax' in self._params: self.calcresult = (0, 0, 0) def doStart(self, value): if value == 'off': if self._attached_daq.mode == 'tof': # don't touch realtime self._attached_daq.mode = 'standard' self._attached_params.start((0, 0)) return elif value == 'manual': if self._attached_daq.mode == 'standard': self._attached_daq.mode = 'tof' return reso = float(value.strip('%')) / 100.0 sel_target = self._attached_selector.target det_target = self._attached_det_pos.target try: lam = self._attached_selector.presets[sel_target]['lam'] spread = self._attached_selector.presets[sel_target]['spread'] det_z = self._attached_det_pos.presets[sel_target][det_target]['z'] det_offset = self._attached_det_pos.offsets[det_z] except KeyError: raise PositionError( self, 'cannot calculate chopper settings: selector or ' 'detector device not at preset') from None self.log.debug('chopper calc inputs: reso=%f, lam=%f, spread=%f, ' 'det_z=%f', reso, lam, spread, det_z) freq, opening = calculate(lam, spread, reso, self.shade, 20.0 + det_z + det_offset, self.tauoffset, self.nmax) self.log.debug('calculated chopper settings: freq=%f, opening=%f', freq, opening) interval = int(1000000.0 / (freq * self.channels)) self.calcresult = freq, opening, interval self._attached_params.start((freq, opening)) self._attached_daq.mode = 'tof' self._attached_daq.tofchannels = self.channels self._attached_daq.tofinterval = interval self._attached_daq.tofprogression = 1.0 # linear channel widths self._attached_daq.tofcustom = [] # no custom channels def doRead(self, maxage=0): if self.target == 'manual': return 'manual' params = self._attached_params.read(maxage) if params[0] < 1.0: return 'off' # TODO: take from isAtTarget if abs(params[0] - self.calcresult[0]) < self._attached_params._attached_freq1.precision and \ abs(params[1] - self.calcresult[1]) < self._attached_params._attached_phase1.precision: if self._attached_daq.mode == 'tof' and \ self._attached_daq.tofchannels == self.channels and \ self._attached_daq.tofinterval == self.calcresult[2] and \ self._attached_daq.tofprogression == 1.0 and \ self._attached_daq.tofcustom == []: return self.target return 'unknown' def doStatus(self, maxage=0): move_status = self._attached_params.status(maxage) if move_status[0] not in (status.OK, status.WARN): return move_status r = self.read(maxage) if r == 'unknown': return (status.NOTREACHED, 'unconfigured chopper frequency/phase ' 'or still moving') return status.OK, ''
class SXTalSample(Sample): parameters = { 'cell': Param('Unit cell with matrix', type=SXTalCellType, settable=True, mandatory=False, default=SXTalCell.fromabc(5)), 'a': Param('a', type=float, category='sample'), 'b': Param('b', type=float, category='sample'), 'c': Param('c', type=float, category='sample'), 'alpha': Param('alpha', type=floatrange(1., 179.), category='sample'), 'beta': Param('beta', type=floatrange(1., 179.), category='sample'), 'gamma': Param('gamma', type=floatrange(1., 179.), category='sample'), 'rmat': Param('rmat', type=listof(listof(float)), default=None, userparam=False), 'ubmatrix': Param('UB matrix (rmat^T)', type=listof(listof(float)), category='sample'), 'bravais': Param('Bravais lattice', type=oneof(*symmetry.Bravais.conditions), settable=True, default='P', category='sample'), 'laue': Param('Laue group', type=oneof(*symmetry.symbols), settable=True, default='1', category='sample'), 'peaklists': Param('Lists of peaks for scanning', internal=True, type=dictof(str, list), settable=True), 'poslists': Param('Lists of positions for indexing', internal=True, type=dictof(str, list), settable=True), } def clear(self): """Clear experiment-specific information.""" Sample.clear(self) self.cell = SXTalCell.fromabc(5) self.peaklists = {} self.poslists = {} def new(self, parameters): """Accepts several ways to spell new cell params.""" lattice = parameters.pop('lattice', None) if lattice is not None: try: parameters['a'], parameters['b'], parameters['c'] = lattice except Exception: self.log.warning('invalid lattice spec ignored, should be ' '[a, b, c]') angles = parameters.pop('angles', None) if angles is not None: try: parameters['alpha'], parameters['beta'], \ parameters['gamma'] = angles except Exception: self.log.warning('invalid angles spec ignored, should be ' '[alpha, beta, gamma]') a = parameters.pop('a', None) if a is None: if 'cell' not in parameters: self.log.warning('using dummy lattice constant of 5 A') a = 5.0 b = parameters.pop('b', a) c = parameters.pop('c', a) alpha = parameters.pop('alpha', 90.0) beta = parameters.pop('beta', 90.0) gamma = parameters.pop('gamma', 90.0) # TODO: map spacegroup/bravais/laue with triple-axis bravais = parameters.pop('bravais', 'P') laue = parameters.pop('laue', '1') if 'cell' not in parameters: parameters['cell'] = [a, b, c, alpha, beta, gamma, bravais, laue] Sample.new(self, parameters) def _applyParams(self, number, parameters): Sample._applyParams(self, number, parameters) if 'cell' in parameters: self.cell = parameters['cell'] def doReadBravais(self): return self.cell.bravais.bravais def doWriteBravais(self, value): self.cell.bravais = symmetry.Bravais(value) def doReadLaue(self): return self.cell.laue.laue def doWriteLaue(self, value): self.cell.laue = symmetry.Laue(value) def doWriteCell(self, cell): params = cell.cellparams() self._setROParam('a', params.a) self._setROParam('b', params.b) self._setROParam('c', params.c) self._setROParam('alpha', params.alpha) self._setROParam('beta', params.beta) self._setROParam('gamma', params.gamma) self._setROParam('rmat', cell.rmat.tolist()) self._setROParam('ubmatrix', cell.rmat.T.tolist()) self._setROParam('bravais', cell.bravais.bravais) self._setROParam('laue', cell.laue.laue) self.log.info('New sample cell set. Parameters:') self.log.info('a = %8.3f b = %8.3f c = %8.3f', params.a, params.b, params.c) self.log.info('alpha = %8.3f beta = %8.3f gamma = %8.3f', params.alpha, params.beta, params.gamma) self.log.info('Bravais: %s Laue: %s', cell.bravais.bravais, cell.laue.laue) self.log.info('UB matrix:') for row in cell.rmat.T: self.log.info('%8.4f %8.4f %8.4f', *row)
class SatBox(HasTimeout, Moveable): """ Device Object for PANDA's Attenuator controlled by a WUT-device via a ModBusTCP interface via a ModBus TACO Server. """ valuetype = int attached_devices = { 'input': Attach('Endswitch input', Readable), 'output': Attach('Output', Moveable), } parameters = { 'blades': Param('Thickness of the blades, starting with lowest bit', type=listof(floatrange(0, 1000)), mandatory=True), 'readout': Param('Determine blade state from output or from switches', type=oneof('switches', 'outputs'), mandatory=True, default='output', chatty=True), } parameter_overrides = { 'timeout': Override(default=5), } def _readOutputs(self, maxage=0): # just read back the OUTPUT values return bits(self._attached_output.read(), len(self.blades)) def _readSwitches(self): # deduce blade state from switches state = bits(self._attached_input.read(), len(self.blades)*2) realstate = [] for i in range(0, len(state), 2): bladestate = state[i:i+2] if bladestate == (0, 1): realstate.append(1) elif bladestate == (1, 0): realstate.append(0) else: realstate.append(None) return tuple(realstate) def doRead(self, maxage=0): bladestate = self._readSwitches() if self.readout == 'switches' else \ self._readOutputs() # only sum up blades which are used for sure (0/None->ignore) return sum(b * r for b, r in zip(self.blades, bladestate) if r) def doStatus(self, maxage=0): if self.readout == 'outputs': return status.OK, '' if self._readSwitches() == self._readOutputs(): return status.OK, '' return status.BUSY, 'moving' def doStart(self, rpos): if rpos > sum(self.blades): raise InvalidValueError(self, 'Value %d too big!, maximum is %d' % (rpos, sum(self.blades))) which = 0 pos = rpos # start with biggest blade and work downwards, ignoring disabled blades for bladewidth in reversed(self.blades): if bladewidth and pos >= bladewidth: which |= 1 << self.blades.index(bladewidth) pos -= bladewidth if pos != 0: self.log.warning('Value %d impossible, trying %d instead!', rpos, rpos + 1) return self.start(rpos + 1) self._attached_output.move(which) if self.readout == 'output': # if we have no readback, give blades time to react session.delay(1) def doIsAllowed(self, target): if not (0 <= target <= sum(self.blades)): return False, 'Value outside range 0..%d' % sum(self.blades) return True, ''
class DoubleSlit(PseudoNOK, Moveable): """Double slit using two SingleSlits.""" hardware_access = False attached_devices = { 'slit_r': Attach('Reactor side single slit', SingleSlit), 'slit_s': Attach('Sample side single slit', SingleSlit), } parameters = { 'mode': Param('Modus of Beam', type=oneof(*MODES), settable=True, userparam=True, default='slit', category='experiment'), 'maxheight': Param('Max opening of the slit', type=floatrange(0), settable=False, default=12.), 'opmode': Param( 'Mode of operation for the slit', type=oneof(CENTERED), # '2blades' is possible userparam=True, settable=True, default=CENTERED, category='experiment'), } parameter_overrides = { 'nok_start': Override(volatile=True), 'nok_end': Override(volatile=True), } def doInit(self, mode): # Even if the slit could not be become closer then 0 and not more # opened the maxheight the instrument scientist want to scan over # the limits to find out the 'open' and 'closed' point for the neutrons self.valuetype = tupleof(floatrange(-1, self.maxheight + 1), float) # generate auto devices for name, idx, opmode in [('height', 0, CENTERED), ('center', 1, CENTERED)]: self.__dict__[name] = SingleSlitAxis('%s.%s' % (self.name, name), slit=self, unit=self.unit, lowlevel=True, index=idx, opmode=opmode) self._motors = [self._attached_slit_r, self._attached_slit_s] def doStatus(self, maxage=0): st = Moveable.doStatus(self, maxage) if st[0] == status.OK: return st[0], self.name # display device name return st def doWriteMode(self, mode): for d in self._adevs.values(): d.mode = mode def _calculate_slits(self, arg, direction): self.log.debug('calculate slits: dir:%s mode:%s arg %s', direction, self.mode, str(arg)) if direction: reactor, sample = arg opening = self.maxheight - (sample - reactor) height = (sample + reactor) / 2.0 res = [opening, height] else: opening, height = arg reactor = height - (self.maxheight - opening) / 2.0 sample = height + (self.maxheight - opening) / 2.0 res = [reactor, sample] self.log.debug('res %s', res) return res def doRead(self, maxage=0): return self._calculate_slits([ self._attached_slit_r.read(maxage), self._attached_slit_s.read(maxage) ], True) def doIsAllowed(self, targets): self.log.debug('DoubleSlit doIsAllowed %s', targets) why = [] try: self.valuetype((targets[0], 0)) except ValueError as e: why.append('%s' % e) for dev, pos in zip([self._attached_slit_r, self._attached_slit_s], self._calculate_slits(targets, False)): ok, _why = dev.isAllowed(pos) if not ok: why.append('%s: requested position %.3f %s out of limits; %s' % (dev, pos, dev.unit, _why)) else: self.log.debug('%s: requested position %.3f %s allowed', dev, pos, dev.unit) if why: return False, '; '.join(why) return True, '' # def doIsAtTarget(self, targets): # # check precision, only move if needed! # self.log.debug('DoubleSlit doIsAtTarget %s', targets) # targets = self.rechnen_motor(targets, False, 'doIsAtTarget') # self.log.debug('%s', targets) # traveldists = [target - dev.doRead(0) # for target, dev in zip(targets, self._devices)] # return max(abs(v) for v in traveldists) <= self.precision def doStop(self): for dev in self._adevs.values(): dev.stop() def doStart(self, targets): """Generate and start a sequence if none is running.""" for dev, target in zip([self._attached_slit_r, self._attached_slit_s], self._calculate_slits(targets, False)): dev.start(target) def doReadNok_Start(self): return self._attached_slit_r.nok_start def doReadNok_End(self): return self._attached_slit_s.nok_end def doPoll(self, n, maxage): # also poll sub-AutoDevices we created for dev in devIter(self.__dict__, baseclass=AutoDevice): dev.poll(n, maxage) def valueInfo(self): return Value('%s.height' % self, unit=self.unit, fmtstr='%.2f'), \ Value('%s.center' % self, unit=self.unit, fmtstr='%.2f')
class PumaMultiAnalyzer(CanReference, IsController, HasTimeout, BaseSequencer): """PUMA multianalyzer device. The device combines 11 devices consisting of a rotation and a translation. """ _num_axes = 11 attached_devices = { 'translations': Attach('Translation axes of the crystals', CanReference, multiple=_num_axes), 'rotations': Attach('Rotation axes of the crystals', CanReference, multiple=_num_axes), } parameters = { 'distance': Param('', type=float, settable=False, default=0), 'mosaicity': Param('Mosaicity of the crystals', type=floatrange(0, None), unit='deg', category='general', default=0.4), 'bladewidth': Param('Width of the analyzer crystals', type=floatrange(0, None), unit='mm', category='general', default=25), 'planedistance': Param('Distance between the net planes of crystals', type=floatrange(0, None), unit='AA', category='general', default=3.354), 'raildistance': Param('Distance between the rails of the crystals', type=floatrange(0, None), default=20, unit='mm', category='general'), } parameter_overrides = { 'timeout': Override(default=600), 'unit': Override(mandatory=False, default=''), 'fmtstr': Override(volatile=True), } hardware_access = False stoprequest = 0 valuetype = tupleof(*(float for i in range(2 * _num_axes))) _allowed_called = False def doPreinit(self, mode): self._rotation = self._attached_rotations self._translation = self._attached_translations @contextmanager def _allowed(self): """Indicator: position checks will done by controller itself. If the controller methods ``doStart`` or ``doIsAllowed`` are called the ``isAdevTargetAllowed`` must give back always True otherwise a no movement of any component can be achieved. """ self._allowed_called = True yield self._allowed_called = False def isAdevTargetAllowed(self, dev, target): if not self._allowed_called: stat = self.doStatus(0) if stat[0] != status.OK: return False, '%s: Controller device is busy!' % self if dev in self._translation: return self._check_translation(self._translation.index(dev), target) return self._check_rotation(self._rotation.index(dev), target) return True, '' def _check_rotation(self, rindex, target): delta = self._translation[rindex + 1].read(0) - \ self._translation[rindex].read(0) \ if 0 <= rindex < self._num_axes - 1 else 13 ll, hl = self._rotlimits(delta) self.log.debug('%s, %s, %s', ll, target, hl) if not ll <= target <= hl: return False, 'neighbour distance: %.3f; cannot move rotation ' \ 'to : %.3f!' % (delta, target) return True, '' def _check_translation(self, tindex, target): cdelta = [13, 13] # current delta between device and neighour devices tdelta = [13, 13] # delta between devices and neighbours at target rot = [None, self._rotation[tindex].read(0), None] trans = [None, self._translation[tindex].read(0), None] # determine the current and the upcoming deltas between translation # device and its neighbouring devices if tindex < self._num_axes - 1: trans[2] = self._translation[tindex + 1].read(0) rot[2] = self._rotation[tindex + 1].read(0) cdelta[1] = trans[2] - trans[1] tdelta[1] = trans[2] - target if tindex > 0: trans[0] = self._translation[tindex - 1].read(0) rot[0] = self._rotation[tindex - 1].read(0) cdelta[0] = trans[0] - trans[1] tdelta[0] = trans[0] - target for dc, dt, r in zip(cdelta, tdelta, rot[::2]): # self.log.info('dc:%s dt:%s t:%s r:%s', dc, dt, t, r) # self.log.info('s(dc): %s s(dt): %s', sign(dc), sign(dt)) if 0 in (sign(dc), sign(dt)) or sign(dc) == sign(dt): # No passing of the other translation devices # self.log.info('No passing device: %s %s', dc, dt) if abs(dt) < abs(dc): # self.log.info('Come closer to other device') if r: ll, hl = self._rotlimits(dt) if not ll <= r <= hl: self.log.info('(%s): %s, %s, %s', target, ll, r, hl) self.log.info('%r, %r; %r', trans, rot, tdelta) return False, 'neighbour distance: %.3f; cannot ' \ 'move translation to : %.3f!' % (dt, target) # elif -13 < dc < 0 and dt < 0: # # self.log.info('It is critical') # if (r is not None and r < -23.33) or rot[1] < -23.33: # return False, 'Path %s to %s is not free. One of the '\ # 'mirrors would hit another one. (%s, %s)' % ( # trans[1], target, r, rot[1]) else: # Passing another translation device # self.log.info('Passing device: %s %s', dc, dt) # self.log.info('rotations: %s %s', r, rot[1]) if (r is not None and r < -23.33) or rot[1] < -23.33: return False, 'Path %s to %s is not free. One of the ' \ 'mirrors would hit another one. (%s, %s)' % ( trans[1], target, r, rot[1]) return True, '' def doIsAllowed(self, target): """Check if requested targets are within allowed range for devices.""" with self._allowed(): why = [] for i, (ax, t) in enumerate( zip(self._rotation, target[self._num_axes:])): ok, w = ax.isAllowed(t) if ok: self.log.debug( 'requested rotation %2d to %.2f deg ' 'allowed', i + 1, t) else: why.append('requested rotation %d to %.2f deg out of ' 'limits: %s' % (i + 1, t, w)) for i, (ax, t) in enumerate( zip(self._translation, target[:self._num_axes])): ok, w = ax.isAllowed(t) if ok: self.log.debug( 'requested translation %2d to %.2f mm ' 'allowed', i + 1, t) else: why.append('requested translation %2d to %.2f mm out of ' 'limits: %s' % (i + 1, t, w)) for i, rot in enumerate(target[self._num_axes:]): ll, hl = self._calc_rotlimits(i, target) if not ll <= rot <= hl: why.append('requested rotation %2d to %.2f deg out of ' 'limits: (%.3f, %3f)' % (i + 1, rot, ll, hl)) if why: self.log.info('target: %s', target) return False, '; '.join(why) return True, '' def valueInfo(self): ret = [] for dev in self._translation + self._rotation: ret.extend(dev.valueInfo()) return tuple(ret) def doReadFmtstr(self): return ', '.join('%s = %s' % (v.name, v.fmtstr) for v in self.valueInfo()) def _generateSequence(self, target): """Move multidetector to correct scattering angle of multi analyzer. It takes account into the different origins of the analyzer blades. """ # check if requested positions already reached within precision if self.isAtTarget(target): self.log.debug('device already at position, nothing to do!') return [] return [ SeqMethod(self, '_move_translations', target), SeqMethod(self, '_move_rotations', target), ] def _move_translations(self, target): # first check for translation mvt = self._checkPositionReachedTrans(target) if mvt: self.log.debug('The following translation axes start moving: %r', mvt) # check if translation movement is allowed, i.e. if all # rotation axis at reference switch if not self._checkRefSwitchRotation(): if not self._refrotation(): raise PositionError(self, 'Could not reference rotations') self.log.debug('all rotation at refswitch, start translation') for i, dev in enumerate(self._translation): with self._allowed(): dev.move(target[i]) self._hw_wait(self._translation) if self._checkPositionReachedTrans(target): raise PositionError(self, 'Translation drive not successful') self.log.debug('translation movement done') def _move_rotations(self, target): # Rotation Movement mvr = self._checkPositionReachedRot(target) if mvr: self.log.debug('The following rotation axes start moving: %r', mvr) for i in mvr: ll, hl = self._calc_rotlimits(i, target) if not ll <= target[self._num_axes + i] <= hl: self.log.warning('neighbour is to close; cannot move ' 'rotation!') continue with self._allowed(): self._rotation[i].move(target[self._num_axes + i]) self._hw_wait([self._rotation[i] for i in mvr]) if self._checkPositionReachedRot(target): raise PositionError( self, 'Rotation drive not successful: ' '%r' % ['%s' % d for d in mvr]) self.log.debug('rotation movement done') def doReset(self): for dev in self._rotation + self._translation: dev.reset() def doReference(self, *args): if self._seq_is_running(): if self._mode == SIMULATION: self._seq_thread.join() self._seq_thread = None else: raise MoveError( self, 'Cannot reference device, device is ' 'still moving (at %s)!' % self._seq_status[1]) with self._allowed(): self._startSequence([ SeqMethod(self, '_checkedRefRot'), SeqMethod(self, '_checkedRefTrans') ]) def _checkedRefRot(self): if not self._refrotation(): self.log.warning('reference of rotations not successful') def _checkedRefTrans(self): if not self._reftranslation(): self.log.warning('reference of translations not successful') def _hw_wait(self, devices): loops = 0 final_exc = None devlist = devices[:] # make a 'real' copy of the list while devlist: loops += 1 for dev in devlist[:]: try: done = dev.doStatus(0)[0] except Exception: dev.log.exception('while waiting') final_exc = filterExceptions(sys.exc_info(), final_exc) # remove this device from the waiters - we might still # have its subdevices in the list so that _hw_wait() # should not return until everything is either OK or # ERROR devlist.remove(dev) if done == status.BUSY: # we found one busy dev, normally go to next iteration # until this one is done (saves going through the whole # list of devices and doing unnecessary HW communication) if loops % 10: break # every 10 loops, go through everything to get an accurate # display in the action line continue devlist.remove(dev) if devlist: session.delay(self._base_loop_delay) if final_exc: reraise(*final_exc) def _reference(self, devlist): if self.stoprequest == 0: for ax in devlist: if not ax.motor.isAtReference(): self.log.debug('reference: %s', ax.name) ax.motor.reference() multiWait([ax.motor for ax in devlist]) check = 0 for ax in devlist: if ax.motor.isAtReference(): check += 1 else: self.log.warning('%s is not at reference: %r %r', ax.name, ax.motor.refpos, ax.motor.read(0)) return check == len(devlist) def _refrotation(self): return self._reference(self._rotation) def _reftranslation(self): return self._reference(self._translation) def doRead(self, maxage=0): return [dev.read(maxage) for dev in self._translation + self._rotation] def _printPos(self): out = [] for i, dev in enumerate(self._translation): out.append('translation %2d: %7.2f %s' % (i, dev.read(), dev.unit)) for i, dev in enumerate(self._rotation): out.append('rotation %2d: %7.2f %s' % (i, dev.read(), dev.unit)) self.log.debug('%s', '\n'.join(out)) def _checkRefSwitchRotation(self, rotation=None): if rotation is None: tocheck = [r.motor for r in self._rotation] else: tocheck = [self._rotation[i].motor for i in rotation] checked = [m.isAtReference() for m in tocheck] for c, d in zip(checked, tocheck): self.log.debug('rot switch for %s ok, check: %s', d, c) return all(checked) def _rotlimits(self, delta): rmini = -60. if -13 < delta < -11.9: rmini = -50 + (delta + 20.) * 3 elif -11.9 <= delta <= -2.8: rmini = -23.3 elif delta > -2.8: rmini = -34.5 - delta * 4 if rmini < -60: rmini = -60. return rmini, 1.7 # 1.7 max of the absolute limits of the rotations def _calc_rotlimits(self, trans, target): delta = target[trans + 1] - target[trans] \ if 0 <= trans < self._num_axes - 1 else 12 rmin, rmax = self._rotlimits(delta) self.log.debug('calculated rot limits (%d %d): %.1f -> [%.1f, %.1f]', trans + 1, trans, delta, rmin, rmax) return rmin, rmax def _checkPositionReachedTrans(self, position): mv = [] request = self.doRead(0)[0:self._num_axes] for i in range(len(self._translation)): if abs(position[i] - request[i]) > self._translation[i].precision: self.log.debug('xx%2d translation start moving', i + 1) mv.append(i) else: self.log.debug('xx%2d translation: nothing to do', i + 1) return mv def _checkPositionReachedRot(self, position): mv = [] request = self.doRead(0)[self._num_axes:2 * self._num_axes] for i in range(len(self._rotation)): if abs(position[i + self._num_axes] - request[i]) > \ self._rotation[i].precision: self.log.debug('xx%2d rotation start moving', i + 1) mv.append(i) else: self.log.debug('xx%2d rotation: nothing to do', i + 1) return mv def doIsAtTarget(self, target): self._printPos() mvt = self._checkPositionReachedTrans(target) mvr = self._checkPositionReachedRot(target) return not mvt and not mvr
class McStasImage(ImageChannelMixin, PassiveChannel): """Image channel based on McStas simulation.""" _mythread = None _process = None parameters = { 'size': Param( 'Detector size in pixels (x, y)', settable=False, type=tupleof(intrange(1, 8192), intrange(1, 8192)), default=(1, 1), ), 'mcstasprog': Param('Name of the McStas simulation executable', type=str, settable=False), 'mcstasdir': Param('Directory where McStas stores results', type=str, default='singlecount', settable=False), 'mcstasfile': Param('Name of the McStas data file', type=str, settable=False), 'mcsiminfo': Param('Name for the McStas Siminfo file', settable=False, type=str, default='mccode.sim'), 'ci': Param('Constant ci applied to simulated intensity I', settable=False, type=floatrange(0.), default=1e3) } def doInit(self, mode): self.arraydesc = ArrayDesc(self.name, self.size, '<u4') self._workdir = os.getcwd() def doReadArray(self, quality): self.log.debug('quality: %s', quality) if quality == LIVE: self._send_signal(SIGUSR2) elif quality == FINAL: if self._mythread and self._mythread.is_alive(): self._mythread.join(1.) if self._mythread.is_alive(): self.log.exception("Couldn't join readout thread.") else: self._mythread = None self._readpsd(quality == LIVE) return self._buf def _prepare_params(self): """Return a list of key=value strings. Each entry defines a parameter setting for the mcstas simulation call. examples: param=10 """ raise NotImplementedError('Please implement _prepare_params method') def doPrepare(self): self._mcstas_params = ' '.join(self._prepare_params()) self.log.debug('McStas parameters: %s', self._mcstas_params) self._buf = np.zeros(self.size[::-1]) self.readresult = [0] def valueInfo(self): return (Value(self.name + '.sum', unit='cts', type='counter', errors='sqrt', fmtstr='%d'), ) def doStart(self): self._mythread = createThread('detector %s' % self, self._run) def doStatus(self, maxage=0): if self._mythread and self._mythread.is_alive(): return status.BUSY, 'busy' return status.OK, 'idle' def doFinish(self): self.log.debug('finish') self._send_signal(SIGTERM) def _send_signal(self, sig): if self._process and self._process.is_running(): self._process.send_signal(sig) # wait for mcstas releasing fds datafile = path.join(self._workdir, self.mcstasdir, self.mcstasfile) siminfo = path.join(self._workdir, self.mcstasdir, self.mcsiminfo) try: while self._process and self._process.is_running(): fnames = [f.path for f in self._process.open_files()] if siminfo not in fnames and datafile not in fnames: break session.delay(.01) except (AccessDenied, NoSuchProcess): self.log.debug( 'McStas process already terminated in _send_signal(%r)', sig) self.log.debug('McStas process has written file on signal (%r)', sig) def _run(self): """Run McStas simulation executable. The current settings of the instrument parameters will be transferred to it. """ try: shutil.rmtree(self.mcstasdir) except (IOError, OSError): self.log.info('could not remove old data') command = '%s -n 1e8 -d %s %s' % (self.mcstasprog, self.mcstasdir, self._mcstas_params) self.log.debug('run %s', command) try: self._process = Popen(command.split(), stdout=PIPE, stderr=PIPE, cwd=self._workdir) out, err = self._process.communicate() if out: self.log.debug('McStas output:') for line in out.splitlines(): self.log.debug('[McStas] %s', line) if err: self.log.warning('McStas found some problems:') for line in err.splitlines(): self.log.warning('[McStas] %s', line) except OSError as e: self.log.error('Execution failed: %s', e) self._process.wait() self._process = None def _readpsd(self, ignore_error=False): try: with open( path.join(self._workdir, self.mcstasdir, self.mcstasfile), 'r') as f: lines = f.readlines()[-3 * (self.size[0] + 1):] if lines[0].startswith('# Data') and self.mcstasfile in lines[0]: self._buf = ( np.loadtxt(lines[1:self.size[0] + 1], dtype=np.float32) * self.ci).astype(np.uint32) self.readresult = [self._buf.sum()] elif not ignore_error: raise IOError('Did not find start line: %s' % lines[0]) except IOError: if not ignore_error: self.log.exception('Could not read result file')
class Axis(CanReference, AbstractAxis): """Axis implemented in Python, with NICOS devices for motor and coders.""" attached_devices = { 'motor': Attach('Axis motor device', Motor), 'coder': Attach('Main axis encoder device', Coder, optional=True), 'obs': Attach('Auxiliary encoders used to verify position', Coder, optional=True, multiple=True), } parameter_overrides = { 'precision': Override(mandatory=True, ), # these are not mandatory for the axis: the motor should have them # defined anyway, and by default they are correct for the axis as well 'abslimits': Override(mandatory=False, volatile=True), 'userlimits': Override(volatile=True), } parameters = { 'speed': Param('Motor speed', unit='main/s', volatile=True, settable=True), 'jitter': Param('Amount of position jitter allowed', unit='main', type=floatrange(0.0, 10.0), settable=True), 'obsreadings': Param( 'Number of observer readings to average over ' 'when determining current position', type=int, default=100, settable=True), } hardware_access = False errorstates = {} def doInit(self, mode): if self._attached_coder is None: self.log.debug('using the motor as coder too as no coder was ' 'specified in the setup file') # Check that motor and coder have the same unit elif self._attached_coder.unit != self._attached_motor.unit: raise ConfigurationError( self, 'different units for motor and ' 'coder (%s vs %s)' % (self._attached_motor.unit, self._attached_coder.unit)) # Check that all observers have the same unit as the motor for ob in self._attached_obs: if self._attached_motor.unit != ob.unit: raise ConfigurationError( self, 'different units for motor ' 'and observer %s' % ob) # Check for userlimits in configuration if 'userlimits' in self._config: self.log.warning('userlimits in setup file ignored; configure ' 'them on the motor device if really needed') if getattr(self._attached_motor, 'offset', 0) != 0: self.log.warning('motor has a nonzero offset; this will cause ' 'general confusion and problems with userlimits') self._hascoder = self._attached_coder is not None and \ self._attached_motor != self._attached_coder self._errorstate = None self._posthread = None self._stoprequest = 0 self._maxdiff = self.dragerror if self._hascoder else 0.0 if mode == MASTER and self._hascoder and \ self.motor.status()[0] != status.BUSY and \ abs(self.motor.read() - self.coder.read()) > self.precision: self.log.warning( 'motor and encoder have different positions ' '(%s vs. %s), setting motor position to coder ' 'position' % (self.motor.format( self.motor.read()), self.coder.format(self.coder.read()))) self.motor.setPosition(self._getReading()) # legacy properties for users, DO NOT USE lazy_property here! @property def motor(self): return self._attached_motor @property def coder(self): return self._attached_coder def doReadUnit(self): return self._attached_motor.unit def doReadAbslimits(self): mot_amin, mot_amax = self._attached_motor.abslimits # if abslimits are configured, use them, but they can only restrict, # not widen, the motor's abslimits if 'abslimits' in self._config: amin, amax = self._config['abslimits'] if amin < mot_amin - abs(mot_amin * 1e-12): raise ConfigurationError( self, 'abslimits: min (%s) below ' "motor's min (%s)" % (amin, mot_amin)) if amax > mot_amax + abs(mot_amax * 1e-12): raise ConfigurationError( self, 'abslimits: max (%s) above ' "motor's max (%s)" % (amax, mot_amax)) else: amin, amax = mot_amin, mot_amax return amin, amax def doReadUserlimits(self): # userlimits are always taken from the motor to avoid multiple # conflicting limit settings umin, umax = self._attached_motor.userlimits return umin - self.offset, umax - self.offset def doWriteUserlimits(self, limits): # pylint: disable=assignment-from-none rval = AbstractAxis.doWriteUserlimits(self, limits) if rval: limits = rval # if the offset is currently changing, we need to use _new_offset self._attached_motor.userlimits = ( limits[0] + getattr(self, '_new_offset', self.offset), limits[1] + getattr(self, '_new_offset', self.offset)) def doIsAllowed(self, target): # do limit check here already instead of in the thread ok, why = self._attached_motor.isAllowed(target + self.offset) if not ok: return ok, 'motor cannot move there (offset = %.3f): %s' % ( self.offset, why) return True, '' def doStart(self, target): """Starts the movement of the axis to target.""" if self._mode == SIMULATION: if not self._checkTargetPosition(self.read(0), target, error=False): self._attached_motor.start(target + self.offset) if self._hascoder: self._attached_coder._sim_setValue(target + self.offset) return if self.status(0)[0] == status.BUSY: self.log.debug('need to stop axis first') self.stop() waitForCompletion(self, ignore_errors=True) self._startPositioningThread(target) def _startPositioningThread(self, target): if self._posthread: if self._posthread.is_alive(): self._posthread.join() self._posthread = None self._stoprequest = 0 self._errorstate = None if self._checkTargetPosition(self.read(0), target, error=False): self.log.debug('not moving, already at %.4f within precision', target) return self._target = target self._posthread = createThread('positioning thread %s' % self, self.__positioningThread) def _getWaiters(self): if self._mode == SIMULATION: return [self._attached_motor] # Except for dry runs, the Axis does its own status control, there is # no need to wait for the motor as well. return [] def doStatus(self, maxage=0): """Return the status of the motor controller.""" if self._mode == SIMULATION: return (status.OK, '') elif self._posthread and self._posthread.is_alive(): return (status.BUSY, 'moving') elif self._errorstate: return (status.ERROR, str(self._errorstate)) else: self.log.debug('no motion thread, using motor status') return self._attached_motor.status(maxage) def doRead(self, maxage=0): """Return the current position from coder controller.""" return (self._attached_coder if self._hascoder else self._attached_motor).read(maxage) - self.offset def doPoll(self, i, maxage): self._attached_motor.poll(i, maxage) if self._hascoder: self._attached_coder.poll(i, maxage) for dev in self._attached_obs: dev.poll(i, maxage) def _getReading(self): """Find a good value from the observers. Taking into account that they usually have lower resolution, so we have to average of a few readings to get a (more) precise value. """ # if coder != motor -> use coder (its more precise!) # if no observers, rely on coder (even if its == motor) if self._hascoder: # read the coder return self._attached_coder.read(0) if self._attached_obs: obs = self._attached_obs rounds = self.obsreadings pos = sum(o.doRead() for _ in range(rounds) for o in obs) return pos / float(rounds * len(obs)) return self._attached_motor.read(0) def doReset(self): """Reset the motor/coder controller.""" self._attached_motor.reset() if self._hascoder: self._attached_coder.reset() for obs in self._attached_obs: obs.reset() if self.status(0)[0] != status.BUSY: self._errorstate = None self._attached_motor.setPosition(self._getReading()) if not self._hascoder: self.log.info( 'reset done; use %s.reference() to do a reference ' 'drive', self) def doReference(self): """Do a reference drive, if the motor supports it.""" if self._hascoder: self.log.error('this is an encoded axis, ' 'referencing makes no sense') return motor = self._attached_motor if isinstance(motor, CanReference): return motor.reference() else: self.log.error('motor %s does not have a reference routine', motor) def doStop(self): """Stops the movement of the motor.""" self._stoprequest = 1 # send a stop in case the motor was started via # the lowlevel device or externally. self._attached_motor.stop() def doFinish(self): if self._errorstate: errorstate = self._errorstate self._errorstate = None raise errorstate # pylint: disable=raising-bad-type def doTime(self, start, end): if hasattr(self._attached_motor, 'doTime'): return self._attached_motor.doTime(start, end) return abs(end - start) / self.speed if self.speed != 0 else 0. def doWriteDragerror(self, value): if not self._hascoder: if value != 0: self.log.warning( 'setting a nonzero value for dragerror only ' 'works if a coder was specified in the setup, ' 'which is different from the motor') return 0.0 else: self._maxdiff = value def doWriteSpeed(self, value): self._attached_motor.speed = value def doReadSpeed(self): return self._attached_motor.speed def doWriteOffset(self, value): """Called on adjust(), overridden to forbid adjusting while moving.""" self._new_offset = value if self.status(0)[0] == status.BUSY: raise NicosError( self, 'axis is moving now, please issue a stop ' 'command and try it again') if self._errorstate: raise self._errorstate # pylint: disable=raising-bad-type del self._new_offset HasOffset.doWriteOffset(self, value) def _preMoveAction(self): """This method will be called before the motor will be moved. It should be overwritten in derived classes for special actions. To abort the move, raise an exception from this method. """ def _postMoveAction(self): """This method will be called after the axis reached the position or will be stopped. It should be overwritten in derived classes for special actions. To signal an error, raise an exception from this method. """ def _duringMoveAction(self, position): """This method will be called during every cycle in positioning thread. It should be used to do some special actions like changing shielding blocks, checking for air pressure etc. It should be overwritten in derived classes. To abort the move, raise an exception from this method. """ def _checkDragerror(self): """Check if a "drag error" occurred. The values of motor and coder deviate too much. This indicates that the movement is blocked. This method sets the error state and returns False if a drag error occurs, and returns True otherwise. """ if self._maxdiff <= 0: return True diff = abs(self._attached_motor.read() - self._attached_coder.read()) self.log.debug('motor/coder diff: %s', diff) if diff > self._maxdiff: self._errorstate = MoveError( self, 'drag error (primary coder): ' 'difference %.4g, maximum %.4g' % (diff, self._maxdiff)) return False for obs in self._attached_obs: diff = abs(self._attached_motor.read() - obs.read()) if diff > self._maxdiff: self._errorstate = PositionError( self, 'drag error (%s): difference %.4g, maximum %.4g' % (obs.name, diff, self._maxdiff)) return False return True def _checkMoveToTarget(self, target, pos): """Check that the axis actually moves towards the target position. This method sets the error state and returns False if a drag error occurs, and returns True otherwise. """ delta_last = self._lastdiff delta_curr = abs(pos - target) self.log.debug('position delta: %s, was %s', delta_curr, delta_last) # at the end of the move, the motor can slightly overshoot during # movement we also allow for small jitter, since airpads usually wiggle # a little resulting in non monotonic movement! ok = (delta_last >= (delta_curr - self.jitter)) or \ delta_curr < self.precision # since we allow to move away a little, we want to remember the # smallest distance so far so that we can detect a slow crawl away from # the target which we would otherwise miss self._lastdiff = min(delta_last, delta_curr) if not ok: self._errorstate = MoveError( self, 'not moving to target: ' 'last delta %.4g, current delta %.4g' % (delta_last, delta_curr)) return False return True def _checkTargetPosition(self, target, pos, error=True): """Check if the axis is at the target position. This method returns False if not arrived at target, or True otherwise. """ diff = abs(pos - target) prec = self.precision if (0 < prec <= diff) or (prec == 0 and diff): msg = 'precision error: difference %.4g, precision %.4g' % \ (diff, self.precision) if error: self._errorstate = MoveError(msg) else: self.log.debug(msg) return False maxdiff = self.dragerror for obs in self._attached_obs: diff = abs(target - (obs.read() - self.offset)) if 0 < maxdiff < diff: msg = 'precision error (%s): difference %.4g, maximum %.4g' % \ (obs, diff, maxdiff) if error: self._errorstate = PositionError(msg) else: self.log.debug(msg) return False return True def _setErrorState(self, cls, text): self._errorstate = cls(self, text) self.log.error(text) return False def __positioningThread(self): try: self._preMoveAction() except Exception as err: self._setErrorState(MoveError, 'error in pre-move action: %s' % err) return target = self._target self._errorstate = None positions = [(target, True)] if self.backlash: backlash = self.backlash lastpos = self.read(0) # make sure not to move twice if coming from the side in the # direction of the backlash backlashpos = target + backlash if (backlash > 0 and lastpos < target) or \ (backlash < 0 and lastpos > target): # if backlash position is not allowed, just don't use it if self.isAllowed(backlashpos)[0]: positions.insert(0, (backlashpos, False)) else: # at least try to move to limit if backlashpos > target: limit = self.userlimits[1] else: limit = self.userlimits[0] if self.isAllowed(limit)[0]: positions.insert(0, (limit, False)) else: self.log.debug('cannot move to backlash position') for (pos, precise) in positions: try: self.__positioning(pos, precise) except Exception as err: self._setErrorState(MoveError, 'error in positioning: %s' % err) if self._stoprequest == 2 or self._errorstate: break try: self._postMoveAction() except Exception as err: self._setErrorState(MoveError, 'error in post-move action: %s' % err) def __positioning(self, target, precise=True): self.log.debug('start positioning, target is %s', target) moving = False offset = self.offset tries = self.maxtries # enforce initial good agreement between motor and coder if not self._checkDragerror(): self._attached_motor.setPosition(self._getReading()) self._errorstate = None self._lastdiff = abs(target - self.read(0)) self._attached_motor.start(target + offset) moving = True stoptries = 0 while moving: if self._stoprequest == 1: self.log.debug('stopping motor') self._attached_motor.stop() self._stoprequest = 2 stoptries = 10 continue sleep(self.loopdelay) # poll accurate current values and status of child devices so that # we can use read() and status() subsequently # always poll motor first! mstatus, mstatusinfo = self._attached_motor.status(0) _status, pos = self.poll() if mstatus != status.BUSY: # motor stopped; check why if self._stoprequest == 1: self.log.debug('stop requested') # will stop on next loop elif self._stoprequest == 2: self.log.debug('stop requested, leaving positioning') # manual stop moving = False elif not precise and not self._errorstate: self.log.debug('motor stopped and precise positioning ' 'not requested') moving = False elif self._checkTargetPosition(target, pos, error=not self._errorstate): self.log.debug('target reached, leaving positioning') # target reached moving = False elif mstatus == status.ERROR: self.log.debug('motor in error status (%s), trying reset', mstatusinfo) # motor in error state -> try resetting newstatus = self._attached_motor.reset() # if that failed, stop immediately if newstatus[0] == status.ERROR: moving = False self._setErrorState( MoveError, 'motor in error state: ' '%s' % newstatus[1]) elif tries > 0: if tries == 1: self.log.warning('last try: %s', self._errorstate) else: self.log.debug('target not reached, retrying: %s', self._errorstate) self._errorstate = None # target not reached, get the current position, set the # motor to this position and restart it. _getReading is the # 'real' value, may ask the coder again (so could slow # down!) self._attached_motor.setPosition(self._getReading()) self._attached_motor.start(target + self.offset) tries -= 1 else: moving = False self._setErrorState( MoveError, 'target not reached after ' '%d tries: %s' % (self.maxtries, self._errorstate)) elif not self._checkMoveToTarget(target, pos): self.log.debug('stopping motor because not moving to target') self._attached_motor.stop() # should now go into next try elif not self._checkDragerror(): self.log.debug('stopping motor due to drag error') self._attached_motor.stop() # should now go into next try elif self._stoprequest == 0: try: self._duringMoveAction(pos) except Exception as err: self._setErrorState( MoveError, 'error in during-move ' 'action: %s' % err) self._stoprequest = 1 elif self._stoprequest == 2: # motor should stop, but does not want to? stoptries -= 1 if stoptries < 0: self._setErrorState( MoveError, 'motor did not stop after ' 'stop request, aborting') moving = False self.log.debug('inner positioning loop finshed')