class PM8742(model.Actuator):
"""
Represents one New Focus picomotor controller 8742.
"""
def __init__(self, name, role, address, axes, stepsize, sn=None, **kwargs):
"""
address (str): ip address (use "autoip" to automatically scan and find the
controller, "fake" for a simulator)
axes (list of str): names of the axes, from the 1st to the 4th, if present.
if an axis is not connected, put a "".
stepsize (list of float): size of a step in m (the smaller, the
bigger will be a move for a given distance in m)
sn (str or None): serial number of the device (eg, "11500"). If None, the
driver will use whichever controller is first found.
inverted (set of str): names of the axes which are inverted (IOW, either
empty or the name of the axis)
"""
if not 1 <= len(axes) <= 4:
raise ValueError(
"Axes must be a list of 1 to 4 axis names (got %s)" % (axes, ))
if len(axes) != len(stepsize):
raise ValueError("Expecting %d stepsize (got %s)" %
(len(axes), stepsize))
self._name_to_axis = {} # str -> int: name -> axis number
for i, n in enumerate(axes):
if n == "": # skip this non-connected axis
continue
self._name_to_axis[n] = i + 1
for sz in stepsize:
if sz > 10e-3: # sz is typically ~1µm, so > 1 cm is very fishy
raise ValueError("stepsize should be in meter, but got %g" %
(sz, ))
self._stepsize = stepsize
self._address = address
self._sn = sn
self._accesser = self._openConnection(address, sn)
self._recover = False
self._resynchonise()
if name is None and role is None: # For scan only
return
# Seems to really be the device, so handle connection errors fully
self._recover = True
modl, fw, sn = self.GetIdentification()
if modl != "8742":
logging.warning("Controller %s is not supported, will try anyway",
modl)
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(
max_workers=1) # one task at a time
# Let the controller check the actuators are connected
self.MotorCheck()
axes_def = {}
speed = {}
for n, i in self._name_to_axis.items():
sz = self._stepsize[i - 1]
# TODO: allow to pass the range in m in the arguments
# Position supports ±2³¹, probably not that much in reality, but
# there is no info.
rng = [(-2**31) * sz, (2**31 - 1) * sz]
# Check the actuator is connected
mt = self.GetMotorType(i)
if mt in {MT_NONE, MT_UNKNOWN}:
raise HwError(
"Controller failed to detect motor %d, check the "
"actuator is connected to the controller" % (i, ))
max_stp_s = {MT_STANDARD: 2000, MT_TINY: 1750}[mt]
srng = (0, self._speedToMS(i, max_stp_s))
speed[n] = self._speedToMS(i, self.GetVelocity(i))
axes_def[n] = model.Axis(range=rng, speed=srng, unit="m")
model.Actuator.__init__(self, name, role, axes=axes_def, **kwargs)
self._swVersion = "%s (IP connection)" % (odemis.__version__, )
self._hwVersion = "New Focus %s (firmware %s, S/N %s)" % (modl, fw, sn)
# Note that the "0" position is just the position at which the
# controller turned on
self.position = model.VigilantAttribute({}, unit="m", readonly=True)
self._updatePosition()
max_speed = max(a.speed[1] for a in axes_def.values())
self.speed = model.MultiSpeedVA(speed,
range=(0, max_speed),
unit="m/s",
setter=self._setSpeed)
def terminate(self):
if self._executor:
self.stop()
self._executor.shutdown(wait=True)
self._executor = None
if self._accesser:
self._accesser.terminate()
self._accesser = None
def _sendOrderCommand(self, cmd, val="", axis=None):
return self._accesser.sendOrderCommand(cmd, val, axis)
def _sendQueryCommand(self, cmd, val="", axis=None):
"""
Same as accesser's sendQueryCommand, but with error recovery
"""
trials = 0
while True:
try:
return self._accesser.sendQueryCommand(cmd, val, axis)
except IOError: # Typically due to timeout
trials += 1
if not self._recover and trials < 5:
raise
self._recover = False
try:
# can also happen just due to error
# => first read error and see if that explains anything
self._checkError()
except IOError:
# Sometimes the hardware seems to lose connection
# => try to reconnect
logging.warning(
"Device seems disconnected, will try to reconnect")
# Sometimes the device gets confused and answers are shifted.
# Reset helps, but it also reset the current position, which
# is not handy.
# self._accesser.sendOrderCommand("RS")
self._accesser.terminate()
time.sleep(0.5)
self._accesser = self._openConnection(
self._address, self._sn)
self._checkError()
logging.info("Recovered lost connection to device %s",
self.name)
finally:
self._recover = True
# Low level functions
def GetIdentification(self):
"""
return (str, str, str):
Model name
Firmware version (and date)
serial number
"""
resp = self._sendQueryCommand("*IDN")
# expects something like this:
# New_Focus 8742 v2.2 08/01/13 11511
try:
m = re.match("\w+ (?P<model>\w+) (?P<fw>v\S+ \S+) (?P<sn>\d+)",
resp)
modl, fw, sn = m.groups()
except Exception:
raise IOError("Failed to decode firmware answer '%s'" %
resp.encode('string_escape'))
return modl, fw, sn
def GetMotorType(self, axis):
"""
Read the motor type.
The motor check action must have been performed before to get correct
values.
axis (1<=int<=4): axis number
return (0<=int<=3): the motor type
"""
resp = self._sendQueryCommand("QM", axis=axis)
return int(resp)
def GetVelocity(self, axis):
"""
Read the max speed
axis (1<=int<=4): axis number
return (0<=int<=2000): the speed in step/s
"""
resp = self._sendQueryCommand("VA", axis=axis)
return int(resp)
def SetVelocity(self, axis, val):
"""
Write the max speed
axis (1<=int<=4): axis number
val (1<=int<=2000): the speed in step/s
"""
if not 1 <= val <= 2000:
raise ValueError("Velocity outside of the range 0->2000")
self._sendOrderCommand("VA", "%d" % (val, ), axis)
def GetAccel(self, axis):
"""
Read the acceleration
axis (1<=int<=4): axis number
return (0<=int): the acceleration in step/s²
"""
resp = self._sendQueryCommand("AC", axis=axis)
return int(resp)
def SetAccel(self, axis, val):
"""
Write the acceleration
axis (1<=int<=4): axis number
val (1<=int<=200000): the acceleration in step/s²
"""
if not 1 <= val <= 200000:
raise ValueError("Acceleration outside of the range 0->200000")
self._sendOrderCommand("AC", "%d" % (val, ), axis)
def MotorCheck(self):
"""
Run the motor check command, that automatically configure the right
values based on the type of motors connected.
"""
self._sendOrderCommand("MC")
def MoveAbs(self, axis, pos):
"""
Requests a move to an absolute position. This is non-blocking.
axis (1<=int<=4): axis number
pos (-2**31 <= int 2*31-1): position in step
"""
self._sendOrderCommand("PA", "%d" % (pos, ), axis)
def GetTarget(self, axis):
"""
Read the target position for the given axis
axis (1<=int<=4): axis number
return (int): the position in steps
"""
# Note, it's not clear what's the difference with PR?
resp = self._sendQueryCommand("PA", axis=axis)
return int(resp)
def MoveRel(self, axis, offset):
"""
Requests a move to a relative position. This is non-blocking.
axis (1<=int<=4): axis number
offset (-2**31 <= int 2*31-1): offset in step
"""
self._sendOrderCommand("PR", "%d" % (offset, ), axis)
def GetPosition(self, axis):
"""
Read the actual position for the given axis
axis (1<=int<=4): axis number
return (int): the position in steps
"""
resp = self._sendQueryCommand("TP", axis=axis)
return int(resp)
def IsMotionDone(self, axis):
"""
Check whether the axis is in motion
axis (1<=int<=4): axis number
return (bool): False if in motion, True if motion is finished
"""
resp = self._sendQueryCommand("MD", axis=axis)
if resp == "0": # motion in progress
return False
elif resp == "1": # no motion
return True
else:
raise IOError("Failed to decode answer about motion '%s'" %
resp.encode('string_escape'))
def AbortMotion(self):
"""
Stop immediately the motion on all the axes
"""
self._sendOrderCommand("AB")
def StopMotion(self, axis):
"""
Stop nicely the motion (using accel/decel values)
axis (1<=int<=4): axis number
"""
self._sendOrderCommand("ST", axis=axis)
def GetError(self):
"""
Read the oldest error in memory.
The error buffer is FIFO with 10 elements, so it might not be the
latest error if multiple errors have happened since the last time this
function was called.
return (None or (int, str)): the error number and message
"""
# Note: there is another one "TE" which only returns the number, and so
# is faster, but then there is no way to get the message
resp = self._sendQueryCommand("TB")
# returns something like "108, MOTOR NOT CONNECTED"
try:
m = re.match("(?P<no>\d+), (?P<msg>.+)", resp)
no, msg = int(m.group("no")), m.group("msg")
except Exception:
raise IOError("Failed to decode error info '%s'" %
resp.encode('string_escape'))
if no == 0:
return None
else:
return no, msg
def _checkError(self):
"""
Check if an error happened and convert to a python exception
return None
raise NewFocusError if an error happened
"""
err = self.GetError()
if err:
errno, msg = err
raise NewFocusError(errno, msg)
def _resynchonise(self):
"""
Ensures the device communication is "synchronised"
"""
self._accesser.flushInput()
# drop all the errors
while self.GetError():
pass
# high-level methods (interface)
def _updatePosition(self, axes=None):
"""
update the position VA
axes (set of str): names of the axes to update or None if all should be
updated
"""
# uses the current values (converted to internal representation)
pos = self._applyInversion(self.position.value)
for n, i in self._name_to_axis.items():
if axes is None or n in axes:
pos[n] = self.GetPosition(i) * self._stepsize[i - 1]
pos = self._applyInversion(pos)
# it's read-only, so we change it via _value
self.position._value = pos
self.position.notify(self.position.value)
def _speedToMS(self, axis, sps):
"""
Convert speed in step/s to m/s
axis (1<=int<=4): axis number
sps (int): steps/s
return (float): m/s
"""
return sps * self._stepsize[axis - 1]
def _setSpeed(self, value):
"""
value (dict string-> float): speed for each axis
returns (dict string-> float): the new value
"""
if set(value.keys()) != set(self._axes.keys()):
raise ValueError("Requested speed %s doesn't specify all axes %s" %
(value, self._axes.keys()))
for axis, v in value.items():
rng = self._axes[axis].speed
if not rng[0] < v <= rng[1]:
raise ValueError(
"Requested speed of %f for axis %s not within %f->%f" %
(v, axis, rng[0], rng[1]))
i = self._name_to_axis[axis]
sps = max(1, int(round(v / self._stepsize[i - 1])))
self.SetVelocity(i, sps)
return value
def _createFuture(self):
"""
Return (CancellableFuture): a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event(
) # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
@isasync
def moveRel(self, shift):
self._checkMoveRel(shift)
shift = self._applyInversion(shift)
# Check if the distance is big enough to make sense
for an, v in shift.items():
aid = self._name_to_axis[an]
if abs(v) < self._stepsize[aid - 1]:
# TODO: store and accumulate all the small moves instead of dropping them?
del shift[an]
logging.info("Dropped too small move of %g m < %g m", abs(v),
self._stepsize[aid - 1])
if not shift:
return model.InstantaneousFuture()
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
moveRel.__doc__ = model.Actuator.moveRel.__doc__
@isasync
def moveAbs(self, pos):
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
pos = self._applyInversion(pos)
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
moveAbs.__doc__ = model.Actuator.moveAbs.__doc__
def stop(self, axes=None):
self._executor.cancel()
def _doMoveRel(self, future, pos):
"""
Blocking and cancellable relative move
future (Future): the future it handles
pos (dict str -> float): axis name -> relative target position
"""
with future._moving_lock:
end = 0 # expected end
moving_axes = set()
for an, v in pos.items():
aid = self._name_to_axis[an]
moving_axes.add(aid)
steps = int(round(v / self._stepsize[aid - 1]))
self.MoveRel(aid, steps)
# compute expected end
dur = abs(steps) * self._stepsize[aid -
1] / self.speed.value[an]
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
logging.debug("move successfully completed")
def _doMoveAbs(self, future, pos):
"""
Blocking and cancellable absolute move
future (Future): the future it handles
pos (dict str -> float): axis name -> absolute target position
"""
with future._moving_lock:
end = 0 # expected end
old_pos = self._applyInversion(self.position.value)
moving_axes = set()
for an, v in pos.items():
aid = self._name_to_axis[an]
moving_axes.add(aid)
steps = int(round(v / self._stepsize[aid - 1]))
self.MoveAbs(aid, steps)
# compute expected end
dur = abs(v - old_pos[an]) / self.speed.value[an]
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
logging.debug("move successfully completed")
def _waitEndMove(self, future, axes, end=0):
"""
Wait until all the given axes are finished moving, or a request to
stop has been received.
future (Future): the future it handles
axes (set of int): the axes IDs to check
end (float): expected end time
raise:
CancelledError: if cancelled before the end of the move
"""
moving_axes = set(axes)
last_upd = time.time()
last_axes = moving_axes.copy()
try:
while not future._must_stop.is_set():
for aid in moving_axes.copy(
): # need copy to remove during iteration
if self.IsMotionDone(aid):
moving_axes.discard(aid)
if not moving_axes:
# no more axes to wait for
break
# Update the position from time to time (10 Hz)
if time.time() - last_upd > 0.1 or last_axes != moving_axes:
last_names = set(n for n, i in self._name_to_axis.items()
if i in last_axes)
self._updatePosition(last_names)
last_upd = time.time()
last_axes = moving_axes.copy()
# Wait half of the time left (maximum 0.1 s)
left = end - time.time()
sleept = max(0.001, min(left / 2, 0.1))
future._must_stop.wait(sleept)
# TODO: timeout if really too long
else:
logging.debug("Move of axes %s cancelled before the end", axes)
# stop all axes still moving them
for i in moving_axes:
self.StopMotion(i)
future._was_stopped = True
raise CancelledError()
except Exception:
raise
else:
# Did everything really finished fine?
self._checkError()
finally:
self._updatePosition() # update (all axes) with final position
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set(
) # tell the thread taking care of the move it's over
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
@classmethod
def scan(cls):
"""
returns (list of (str, dict)): name, kwargs
Note: it's obviously not advised to call this function if a device is already under use
"""
logging.info("Scanning for New Focus controllers in progress...")
found = [] # (list of 2-tuple): name, kwargs
try:
conts = cls._scanOverIP()
except IOError as exp:
logging.exception("Failed to scan for New Focus controllers: %s",
exp)
for hn, host, port in conts:
try:
logging.debug("Trying controller at %s", host)
dev = cls(None,
None,
address=host,
axes=["a"],
stepsize=[1e-6])
modl, fw, sn = dev.GetIdentification()
# find out about the axes
dev.MotorCheck()
axes = []
stepsize = []
for i in range(1, 5):
mt = dev.GetMotorType(i)
n = chr(ord('a') + i - 1)
# No idea about the stepsize, but make it different to allow
# distinguishing between motor types
if mt == MT_STANDARD:
ss = 1e-6
elif mt == MT_TINY:
ss = 0.1e-6
else:
n = ""
ss = 0
axes.append(n)
stepsize.append(ss)
except IOError:
# not possible to use this port? next one!
continue
except Exception:
logging.exception(
"Error while communicating with controller %s @ %s:%s", hn,
host, port)
continue
found.append(("NF-%s-%s" % (modl, sn), {
"address": host,
"axes": axes,
"stepsize": stepsize,
"sn": sn
}))
return found
@classmethod
def _openConnection(cls, address, sn=None):
"""
return (Accesser)
"""
if address == "fake":
host, port = "fake", 23
elif address == "autoip":
conts = cls._scanOverIP()
if sn is not None:
for hn, host, port in conts:
# Open connection to each controller and ask for their SN
dev = cls(None,
None,
address=host,
axes=["a"],
stepsize=[1e-6])
_, _, devsn = dev.GetIdentification()
if sn == devsn:
break
else:
raise HwError(
"Failed to find New Focus controller %s over the "
"network. Ensure it is turned on and connected to "
"the network." % (sn, ))
else:
# just pick the first one
# TODO: only pick the ones of model 8742
try:
hn, host, port = conts[0]
logging.info("Connecting to New Focus %s", hn)
except IndexError:
raise HwError(
"Failed to find New Focus controller over the "
"network. Ensure it is turned on and connected to "
"the network.")
else:
# split the (IP) port, separated by a :
if ":" in address:
host, ipport_str = address.split(":")
port = int(ipport_str)
else:
host = address
port = 23 # default
return IPAccesser(host, port)
@staticmethod
def _scanOverIP():
"""
Scan the network for all the responding new focus controllers
Note: it actually calls a separate executable because it relies on opening
a network port which needs special privileges.
return (list of (str, str, int)): hostname, ip address, and port number
"""
# Run the separate program via authbind
try:
exc = os.path.join(os.path.dirname(__file__), "nfpm_netscan.py")
out = subprocess.check_output(["authbind", "python", exc])
except CalledProcessError as exp:
# and handle all the possible errors:
# - no authbind (127)
# - cannot find the separate program (2)
# - no authorisation (13)
ret = exp.returncode
if ret == 127:
raise IOError("Failed to find authbind")
elif ret == 2:
raise IOError("Failed to find %s" % exc)
elif ret == 13:
raise IOError("No permission to open network port 23")
# or decode the output
# hostname \t host \t port
ret = []
for l in out.split("\n"):
if not l:
continue
try:
hn, host, port = l.split("\t")
except Exception:
logging.exception("Failed to decode scanner line '%s'", l)
ret.append((hn, host, port))
return ret
class ESP(model.Actuator):
def __init__(self, name, role, port, axes=None, **kwargs):
"""
A driver for a Newport ESP 301 Stage Actuator.
This driver supports a serial connection. Note that as of the Linux
kernel 4.13, the USB connection is known to _not_ work, as the TI 3410
chipset apparently behind is not handled properly. Use a of the
RS-232 port is required (via a USB adapter if necessary).
name: (str)
role: (str)
port: (str) port name. Can be a pattern, in which case all the ports
fitting the pattern will be tried.
Use /dev/fake for a simulator
axes: dict str (axis name) -> dict (axis parameters)
axis parameters: {
number (1 <= int <= 3): axis number on the hardware
range: [float, float], default is -1 -> 1
unit (str): the external unit of the axis (internal is mm),
default is "m".
conv_factor (float): a conversion factor that converts to the
device internal unit (mm), default is 1000.
}
inverted: (bool) defines if the axes are inverted
The offset can be specified by setting MD_POS_COR as a coordinate dictionary
"""
if len(axes) == 0:
raise ValueError("Needs at least 1 axis.")
# Connect to serial port
self._ser_access = threading.Lock()
self._serial = None
self._file = None
self._port, self._version = self._findDevice(
port) # sets ._serial and ._file
logging.info("Found Newport ESP301 device on port %s, Ver: %s",
self._port, self._version)
self.LockKeypad(
KEYPAD_LOCK_EXCEPT_STOP) # lock user input for the controller
# Clear errors at start
try:
self.checkError()
except ESPError:
pass
self._offset = {}
self._axis_conv_factor = {}
# Not to be mistaken with axes which is a simple public view
self._axis_map = {} # axis name -> axis number used by controller
axes_def = {} # axis name -> Axis object
speed = {}
accel = {}
decel = {}
self._id = {}
for axis_name, axis_par in axes.items():
# Unpack axis parameters from the definitions in the YAML
try:
axis_num = axis_par['number']
except KeyError:
raise ValueError(
"Axis %s must have a number to identify it. " %
(axis_name, ))
try:
axis_range = axis_par['range']
except KeyError:
logging.info("Axis %s has no range. Assuming (-1, 1)",
axis_name)
axis_range = (-1, 1)
try:
axis_unit = axis_par['unit']
except KeyError:
logging.info("Axis %s has no unit. Assuming m", axis_name)
axis_unit = "m"
try:
conv_factor = float(axis_par['conv_factor'])
except KeyError:
logging.info(
"Axis %s has no conversion factor. Assuming 1000 (m to mm)",
axis_name)
conv_factor = 1000.0
self._axis_map[axis_name] = axis_num
self._axis_conv_factor[axis_num] = conv_factor
self._id[axis_num] = self.GetIdentification(axis_num)
speed[axis_name] = self.GetSpeed(axis_num)
accel[axis_name] = self.GetAcceleration(axis_num)
decel[axis_name] = self.GetDeceleration(axis_num)
# Force millimetres for consistency as the internal unit.
self.SetAxisUnit(axis_num, "mm")
# initialize each motor
self.MotorOn(axis_num)
ad = model.Axis(canAbs=True, unit=axis_unit, range=axis_range)
axes_def[axis_name] = ad
model.Actuator.__init__(self, name, role, axes=axes_def, **kwargs)
# whether the axes are referenced
self.referenced = model.VigilantAttribute({a: False
for a in axes},
readonly=True)
self._hwVersion = str(self._id)
self._swVersion = self._version
# Get the position in object coord with the offset applied.
# RO, as to modify it the client must use .moveRel() or .moveAbs()
self.position = model.VigilantAttribute({}, readonly=True)
self._updatePosition()
self._speed = speed
self._accel = accel
self._decel = decel
# set offset due to mounting of components (float)
self._metadata[model.MD_POS_COR] = {}
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(1) # one task at a time
# Check the error state
self.checkError()
def terminate(self):
if self._serial.isOpen():
self.LockKeypad(
KEYPAD_UNLOCK) # unlock user input for the controller
with self._ser_access:
self._serial.close()
model.Actuator.terminate(self)
def updateMetadata(self, md):
super(ESP, self).updateMetadata(md)
try:
value = md[model.MD_POS_COR]
except KeyError:
# there is no offset set.
return
if not isinstance(value, dict):
raise ValueError(
"Invalid metadata, should be a coordinate dictionary but got %s."
% (value, ))
# update all axes
for n in self._axis_map.keys():
if n in value:
self._offset[n] = value[n]
logging.debug("Updating offset to %s.", value)
self._updatePosition()
# Connection methods
@staticmethod
def _openSerialPort(port, baudrate):
"""
Opens the given serial port the right way for a Power control device.
port (string): the name of the serial port (e.g., /dev/ttyUSB0)
baudrate (int)
return (serial): the opened serial port
"""
ser = serial.Serial(
port=port,
baudrate=baudrate,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
rtscts=True,
timeout=1 # s
)
# Purge
ser.flush()
ser.flushInput()
# Try to read until timeout to be extra safe that we properly flushed
ser.timeout = 0
while True:
char = ser.read()
if char == b'':
break
ser.timeout = 1
return ser
def _findDevice(self, ports, baudrate=19200):
"""
Look for a compatible device
ports (str): pattern for the port name
baudrate (0<int)
return:
(str): the name of the port used
(str): the hardware version string
Note: will also update ._file and ._serial
raises:
IOError: if no devices are found
"""
# For debugging purpose
if ports == "/dev/fake":
self._serial = ESPSimulator(timeout=1)
self._file = None
ve = self.GetVersion()
return ports, ve
if os.name == "nt":
raise NotImplementedError("Windows not supported")
else:
names = glob.glob(ports)
for n in names:
try:
# Ensure no one will talk to it simultaneously, and we don't talk to devices already in use
self._file = open(n) # Open in RO, just to check for lock
try:
fcntl.flock(
self._file.fileno(), fcntl.LOCK_EX
| fcntl.LOCK_NB) # Raises IOError if cannot lock
except IOError:
logging.info("Port %s is busy, will not use", n)
continue
self._serial = self._openSerialPort(n, baudrate)
try:
ve = self.GetVersion()
if not "ESP301" in ve.upper():
raise IOError(
"Device at %s is not an ESP301 controller. Reported version string: %s"
% (ports, ve))
except ESPError as e:
# Can happen if the device has received some weird characters
# => try again (now that it's flushed)
logging.info(
"Device answered by an error %s, will try again", e)
ve = self.GetVersion()
return n, ve
except (IOError, ESPError) as e:
logging.debug(e)
logging.info(
"Skipping device on port %s, which didn't seem to be compatible",
n)
# not possible to use this port? next one!
continue
else:
raise HwError(
"Failed to find a device on ports '%s'. "
"Check that the device is turned on and connected to "
"the computer." % (ports, ))
def _sendOrder(self, cmd):
"""
cmd (byte str): command to be sent to device (without the CR)
"""
cmd = cmd + b"\r"
with self._ser_access:
logging.debug("Sending command %s", to_str_escape(cmd))
self._serial.write(cmd)
def _sendQuery(self, cmd):
"""
cmd (byte str): command to be sent to device (without the CR, but with the ?)
returns (str): answer received from the device (without \n or \r)
raise:
IOError if no answer is returned in time
"""
cmd = cmd + b"\r"
with self._ser_access:
logging.debug("Sending command %s", to_str_escape(cmd))
self._serial.write(cmd)
self._serial.timeout = 1
ans = b''
while ans[-1:] != b'\r':
char = self._serial.read()
if not char:
raise IOError("Timeout after receiving %s" %
to_str_escape(ans))
ans += char
logging.debug("Received answer %s", to_str_escape(ans))
return ans.strip().decode("latin1")
# Low level serial commands.
# Note: These all convert to internal units of the controller
def GetErrorCode(self):
# Checks the device error register
return int(self._sendQuery(b"TE?"))
def checkError(self):
# Checks if an error occurred and raises an exception accordingly.
err_q = []
# Get all of the errors in the error FIFO stack
while True:
errcode = self.GetErrorCode()
if errcode == 0: # No error
break
else:
err_q.append(errcode)
# After errors are collected
if len(err_q) > 0:
for err in err_q[:-1]:
logging.warning("Discarding error %d", err)
raise ESPError("Error code %d" % (err_q[-1], ), err_q[-1])
def SetAxisUnit(self, axis_num, unit):
# Set the internal unit used by the controller
if not unit in UNIT_DEF:
raise ValueError("Unknown unit name %s" % (unit, ))
self._sendOrder(b"%d SN %d" % (axis_num, UNIT_DEF[unit]))
def MoveLimit(self, aid, limit):
"""
Requests a move to the positive or negative limit.
limit (str): either '+' or '-', defining positive or negative limit
"""
if not limit in ("+", "-"):
raise ValueError(
"Asked to move %d to %s limit. Only + or - allowed." % (
aid,
limit,
))
self._sendOrder(b"%d MV %s" % (aid, limit))
def LockKeypad(self, lock_type):
"""
Lock keypad on device from preventing bad user input
lock_type (KEYPAD_*)
"""
self._sendOrder(b"LC %d" % (lock_type, ))
def HomeSearch(self, aid, search_type):
"""
Searches for home using a search type (int 0,1,2,3,4,5,6) as per manual
"""
self._sendOrder(b"%d OR %d" % (aid, search_type))
def SetHome(self, aid, value):
"""
Set the position value to use at the origin (home)
"""
self._sendOrder(b"%d SH %f" % (aid, value))
def SaveMemory(self):
"""
Save configuration to non - volatile memory
"""
self._sendOrder(b"SM")
def MoveAbsPos(self, axis_num, pos):
"""
Requests a move to an absolute position. This is non-blocking.
Converts to internal unit of the controller
"""
self._sendOrder(b"%d PA %f" % (axis_num, pos))
def MoveRelPos(self, axis_num, rel):
"""
Requests a move to a relative position. This is non-blocking.
"""
self._sendOrder(b"%d PR %f" % (axis_num, rel)) # 0 = absolute
def GetDesiredPos(self, axis_num):
# Get the target position programmed into the controller
return float(self._sendQuery(b"%d DP?" % (axis_num, )))
def StopMotion(self, axis):
# Stop the motion on the specified axis
self._sendOrder(b"%d ST" % (axis, ))
def MotorOn(self, axis):
# Start the motor
self._sendOrder(b"%d MO" % (axis, ))
def MotorOff(self, axis):
# Stop the motor
self._sendOrder(b"%d MF" % (axis, ))
def GetMotionDone(self, axis_n):
# Return true or false based on if the axis is still moving.
done = int(self._sendQuery(b"%d MD?" % axis_n))
logging.debug("Motion done: %d", done)
return bool(done)
def GetPosition(self, axis_n):
# Get the position of the axis
return float(self._sendQuery(b"%d TP?" % axis_n))
def GetSpeed(self, axis_n):
# Get the speed of the axis
return float(self._sendQuery(b"%d VA?" % axis_n))
def SetSpeed(self, axis_n, speed):
# Set the axis speed
self._sendOrder(b"%d VA %f" % (
axis_n,
speed,
))
def GetAcceleration(self, axis_n):
# Get axis accel
return float(self._sendQuery(b"%d AC?" % axis_n))
def SetAcceleration(self, axis_n, ac):
# Set axis accel
self._sendOrder(b"%d AC %f" % (
axis_n,
ac,
))
def GetDeceleration(self, axis_n):
return float(self._sendQuery(b"%d AG?" % axis_n))
def SetDeceleration(self, axis_n, dc):
self._sendOrder(b"%d AG %f" % (
axis_n,
dc,
))
def GetIdentification(self, axis):
"""
return (str): the identification string as-is for the first axis
"""
return self._sendQuery(b"%d ID?" % (axis, ))
def GetVersion(self):
"""
return (str): the version string as-is
"""
return self._sendQuery(b"VE?")
def SaveMem(self):
"""
Instruct the controller to save the current settings to non-volatile memory
"""
self._sendOrder(b"SM")
"""
High level commands (ie, Odemis Actuator API)
"""
def _applyOffset(self, pos):
"""
Apply the offset to the position and return it
"""
ret = dict(pos)
for axis in self._offset:
if axis in ret:
ret[axis] -= self._offset[axis]
return ret
def _removeOffset(self, pos):
"""
Remove the offset from the position and return it
"""
ret = dict(pos)
for axis in self._offset:
if axis in ret:
ret[axis] += self._offset[axis]
return ret
@isasync
def moveAbs(self, pos):
if not pos:
return model.InstantaneousFuture()
pos = self._removeOffset(pos) # Get the position in controller coord.
self._checkMoveAbs(pos)
pos = self._applyInversion(pos)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
@isasync
def moveRel(self, shift):
self._checkMoveRel(shift)
shift = self._applyInversion(shift)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
def _doMoveRel(self, future, pos):
"""
Blocking and cancellable relative move
future (Future): the future it handles
_pos (dict str -> float): axis name -> relative target position
raise:
ValueError: if the target position is
TMCLError: if the controller reported an error
CancelledError: if cancelled before the end of the move
"""
with future._moving_lock:
end = 0 # expected end
moving_axes = set()
for an, v in pos.items():
aid = self._axis_map[an]
moving_axes.add(aid)
self.MoveRelPos(aid, v * self._axis_conv_factor[aid])
# compute expected end
# convert to mm units
dur = driver.estimateMoveDuration(
abs(v) * self._axis_conv_factor[aid], self._speed[an],
self._accel[an])
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
self.checkError()
logging.debug("move successfully completed")
def _doMoveAbs(self, future, pos):
"""
Blocking and cancellable absolute move
future (Future): the future it handles
_pos (dict str -> float): axis name -> absolute target position
raise:
TMCLError: if the controller reported an error
CancelledError: if cancelled before the end of the move
"""
with future._moving_lock:
end = 0 # expected end
old_pos = self._applyInversion(self.position.value)
moving_axes = set()
for an, v in pos.items():
aid = self._axis_map[an]
moving_axes.add(aid)
self.MoveAbsPos(aid, v * self._axis_conv_factor[aid])
d = abs(v - old_pos[an])
# convert displacement unit to mm
dur = driver.estimateMoveDuration(
d * self._axis_conv_factor[aid], self._speed[an],
self._accel[an])
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
self.checkError()
logging.debug("move successfully completed")
def _waitEndMove(self, future, axes, end=0):
"""
Wait until all the given axes are finished moving, or a request to
stop has been received.
future (Future): the future it handles
axes (set of int): the axes IDs to check
end (float): expected end time
raise:
TimeoutError: if took too long to finish the move
CancelledError: if cancelled before the end of the move
"""
moving_axes = set(axes)
last_upd = time.time()
dur = max(0.01, min(end - last_upd, 60))
max_dur = dur * 2 + 1
logging.debug("Expecting a move of %g s, will wait up to %g s", dur,
max_dur)
timeout = last_upd + max_dur
last_axes = moving_axes.copy()
try:
while not future._must_stop.is_set():
for aid in moving_axes.copy(
): # need copy to remove during iteration
if self.GetMotionDone(aid):
moving_axes.discard(aid)
if not moving_axes:
# no more axes to wait for
break
now = time.time()
if now > timeout:
logging.warning("Stopping move due to timeout after %g s.",
max_dur)
for i in moving_axes:
self.StopMotion(i)
raise TimeoutError("Move is not over after %g s, while "
"expected it takes only %g s" %
(max_dur, dur))
# Update the position from time to time (10 Hz)
if now - last_upd > 0.1 or last_axes != moving_axes:
last_names = set(n for n, i in self._axis_map.items()
if i in last_axes)
self._updatePosition(last_names)
last_upd = time.time()
last_axes = moving_axes.copy()
# Wait half of the time left (maximum 0.1 s)
left = end - time.time()
sleept = max(0.001, min(left / 2, 0.1))
future._must_stop.wait(sleept)
else:
logging.debug("Move of axes %s cancelled before the end", axes)
# stop all axes still moving them
for i in moving_axes:
self.StopMotion(i)
future._was_stopped = True
raise CancelledError()
finally:
# TODO: check if the move succeded ? (= Not failed due to stallguard/limit switch)
self._updatePosition() # update (all axes) with final position
# high-level methods (interface)
def _updatePosition(self, axes=None):
"""
update the position VA
axes (set of str): names of the axes to update or None if all should be
updated
"""
# uses the current values (converted to internal representation)
pos = self._applyInversion(self.position.value)
for n, i in self._axis_map.items():
if axes is None or n in axes:
pos[n] = self.GetPosition(i) / self._axis_conv_factor[i]
pos = self._applyInversion(pos)
pos = self._applyOffset(pos) # Appy the offset back for display
logging.debug("Updated position to %s", pos)
self.position._set_value(pos, force_write=True)
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set(
) # tell the thread taking care of the move it's over
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
def stop(self, axes=None):
self._executor.cancel()
# For safety, just force stop every axis
for an, aid in self._axis_map.items():
if axes is None or an in axes:
self.StopMotion(aid)
try:
self.checkError()
except ESPError as e:
logging.warning("Cancellation error %d", e.code)
# Should now turn the motor back on
self.MotorOn(aid)
def _createMoveFuture(self):
"""
Return (CancellableFuture): a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event(
) # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
@isasync
def reference(self, axes):
if not axes:
return model.InstantaneousFuture()
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doReference, f, axes)
return f
def _doReference(self, future, axes):
"""
Actually runs the referencing code
axes (set of str)
raise:
IOError: if referencing failed due to hardware
CancelledError if was cancelled
"""
# Reset reference so that if it fails, it states the axes are not
# referenced (anymore)
with future._moving_lock:
try:
# do the referencing for each axis sequentially
# (because each referencing is synchronous)
for a in axes:
if future._must_stop.is_set():
raise CancelledError()
aid = self._axis_map[a]
self.referenced._value[a] = False
self.HomeSearch(
aid, REF_NEGATIVE_LIMIT
) # search for the negative limit signal to set an origin
self._waitEndMove(future, (aid, ),
time.time() +
100) # block until it's over
self.SetHome(aid, 0.0) # set negative limit as origin
self.referenced._value[a] = True
except CancelledError:
# FIXME: if the referencing is stopped, the device refuses to
# move until referencing is run (and successful).
# => Need to put back the device into a mode where at least
# relative moves work.
logging.warning(
"Referencing cancelled, device will not move until another referencing"
)
future._was_stopped = True
raise
except Exception:
logging.exception("Referencing failure")
raise
finally:
# We only notify after updating the position so that when a listener
# receives updates both values are already updated.
self._updatePosition(
axes) # all the referenced axes should be back to 0
# read-only so manually notify
self.referenced.notify(self.referenced.value)
class Chamber(model.Actuator):
"""
Simulated chamber component. Just pretends to be able to change pressure
"""
def __init__(self, name, role, positions, has_pressure=True, **kwargs):
"""
Initialises the component
positions (list of str): each pressure positions supported by the
component (among the allowed ones)
has_pressure (boolean): if True, has a pressure VA with the current
pressure.
"""
# TODO: or just provide .targetPressure (like .targetTemperature) ?
# Or maybe provide .targetPosition: position that would be reached if
# all the requested move were instantly applied?
chp = {}
for p in positions:
try:
chp[PRESSURES[p]] = p
except KeyError:
raise ValueError("Pressure position %s is unknown" % (p,))
axes = {"pressure": model.Axis(unit="Pa", choices=chp)}
model.Actuator.__init__(self, name, role, axes=axes, **kwargs)
# For simulating moves
self._position = PRESSURE_VENTED # last official position
self._goal = PRESSURE_VENTED
self._time_goal = 0 # time the goal was/will be reached
self._time_start = 0 # time the move started
# RO, as to modify it the client must use .moveRel() or .moveAbs()
self.position = model.VigilantAttribute(
{"pressure": self._position},
unit="Pa", readonly=True)
if has_pressure:
# Almost the same as position, but gives the current position
self.pressure = model.VigilantAttribute(self._position,
unit="Pa", readonly=True)
self._press_timer = util.RepeatingTimer(1, self._updatePressure,
"Simulated pressure update")
self._press_timer.start()
else:
self._press_timer = None
# Indicates whether the chamber is opened or not
# Just pretend it's always closed, and allow the user to change that
# for instance via CLI.
self.opened = model.BooleanVA(False)
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(max_workers=1) # one task at a time
def terminate(self):
if self._press_timer:
self._press_timer.cancel()
self._press_timer = None
if self._executor:
self.stop()
self._executor.shutdown()
self._executor = None
def _updatePressure(self):
"""
update the pressure VA (called regularly from a thread)
"""
# Compute the current pressure
now = time.time()
if self._time_goal < now: # done
# goal ±5%
pos = self._goal * random.uniform(0.95, 1.05)
else:
# TODO make it logarithmic
ratio = (now - self._time_start) / (self._time_goal - self._time_start)
pos = self._position + (self._goal - self._position) * ratio
# it's read-only, so we change it via _value
self.pressure._value = pos
self.pressure.notify(pos)
def _updatePosition(self):
"""
update the position VA
"""
# .position contains the last known/valid position
# it's read-only, so we change it via _value
self.position._value = {"pressure": self._position}
self.position.notify(self.position.value)
@isasync
def moveRel(self, shift):
self._checkMoveRel(shift)
# convert into an absolute move
pos = {}
for a, v in shift.items:
pos[a] = self.position.value[a] + v
return self.moveAbs(pos)
@isasync
def moveAbs(self, pos):
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
new_pres = pos["pressure"]
est_start = time.time() + 0.1
f = model.ProgressiveFuture(start=est_start,
end=est_start + self._getDuration(new_pres))
return self._executor.submitf(f, self._changePressure, f, new_pres)
def _getDuration(self, pos):
return abs(self._position - pos) / SPEED_PUMP
def _changePressure(self, f, p):
"""
Synchronous change of the pressure
p (float): target pressure
"""
# TODO: allow to cancel during the change
now = time.time()
duration = self._getDuration(p) # s
self._time_start = now
self._time_goal = now + duration # s
self._goal = p
time.sleep(duration / 2)
# DEBUG: for testing wrong time estimation
# f.set_progress(start=self._time_start, end=self._time_goal + 10)
time.sleep(duration / 2)
self._position = p
self._updatePosition()
def stop(self, axes=None):
self._executor.cancel()
logging.warning("Stopped pressure change")
class AntiBacklashActuator(model.Actuator):
"""
This is a stage wrapper that takes a stage and ensures that every move
always finishes in the same direction.
"""
def __init__(self, name, role, children, backlash, **kwargs):
"""
children (dict str -> Stage): dict containing one component, the stage
to wrap
backlash (dict str -> float): for each axis of the stage, the additional
distance to move (and the direction). If an axis of the stage is not
present, then it’s the same as having 0 as backlash (=> no antibacklash
motion is performed for this axis)
"""
if len(children) != 1:
raise ValueError("AntiBacklashActuator needs 1 child")
for a, v in backlash.items():
if not isinstance(a, basestring):
raise ValueError("Backlash key must be a string but got '%s'" % (a,))
if not isinstance(v, numbers.Real):
raise ValueError("Backlash value of %s must be a number but got '%s'" % (a, v))
self._child = children.values()[0]
self._backlash = backlash
axes_def = {}
for an, ax in self._child.axes.items():
axes_def[an] = copy.deepcopy(ax)
axes_def[an].canUpdate = True
# Whether currently a backlash shift is applied on an axis
# If True, moving the axis by the backlash value would restore its expected position
# _shifted_lock must be taken before modifying this attribute
self._shifted = dict((a, False) for a in axes_def.keys())
self._shifted_lock = threading.Lock()
# look for axes in backlash not existing in the child
missing = set(backlash.keys()) - set(axes_def.keys())
if missing:
raise ValueError("Child actuator doesn't have the axes %s" % (missing,))
model.Actuator.__init__(self, name, role, axes=axes_def,
children=children, **kwargs)
# will take care of executing axis moves asynchronously
self._executor = CancellableThreadPoolExecutor(max_workers=1) # one task at a time
# Duplicate VAs which are just identical
# TODO: shall we "hide" the antibacklash move by not updating position
# while doing this move?
self.position = self._child.position
if model.hasVA(self._child, "referenced"):
self.referenced = self._child.referenced
if model.hasVA(self._child, "speed"):
self.speed = self._child.speed
def terminate(self):
if self._executor:
self.stop()
self._executor.shutdown()
self._executor = None
def _antiBacklashMove(self, axes):
"""
Moves back the axes to their official position by reverting the anti-backlash shift
axes (list of str): the axes to revert
"""
sub_backlash = {} # same as backlash but only contains the axes moved
with self._shifted_lock:
for a in axes:
if self._shifted[a]:
if a in self._backlash:
sub_backlash[a] = self._backlash[a]
self._shifted[a] = False
if sub_backlash:
logging.debug("Running anti-backlash move %s", sub_backlash)
self._child.moveRelSync(sub_backlash)
def _doMoveRel(self, future, shift):
# move with the backlash subtracted
sub_shift = {}
for a, v in shift.items():
if a not in self._backlash:
sub_shift[a] = v
else:
# optimisation: if move goes in the same direction as backlash
# correction, then no need to do the correction
# TODO: only do this if backlash correction has already been applied once?
if v * self._backlash[a] >= 0:
sub_shift[a] = v
else:
with self._shifted_lock:
if self._shifted[a]:
sub_shift[a] = v
else:
sub_shift[a] = v - self._backlash[a]
self._shifted[a] = True
# Do the backlash + move
axes = set(shift.keys())
if not any(self._shifted):
# a tiny bit faster as we don't sleep
self._child.moveRelSync(sub_shift)
else:
# some antibacklash move needed afterwards => update might be worthy
f = self._child.moveRel(sub_shift)
done = False
while not done:
try:
f.result(timeout=0.01)
except futures.TimeoutError:
pass # Keep waiting for end of move
else:
done = True
# Check if there is already a new move to do
nf = self._executor.get_next_future(future)
if nf is not None and axes <= nf._update_axes:
logging.debug("Ending move control early as next move is an update containing %s", axes)
return
# backlash move
self._antiBacklashMove(shift.keys())
def _doMoveAbs(self, future, pos):
sub_pos = {}
for a, v in pos.items():
if a not in self._backlash:
sub_pos[a] = v
else:
shift = v - self.position.value[a]
with self._shifted_lock:
if shift * self._backlash[a] >= 0:
sub_pos[a] = v
self._shifted[a] = False
else:
sub_pos[a] = v - self._backlash[a]
self._shifted[a] = True
# Do the backlash + move
axes = set(pos.keys())
if not any(self._shifted):
# a tiny bit faster as we don't sleep
self._child.moveAbsSync(sub_pos)
else: # some antibacklash move needed afterwards => update might be worthy
f = self._child.moveAbs(sub_pos)
done = False
while not done:
try:
f.result(timeout=0.01)
except futures.TimeoutError:
pass # Keep waiting for end of move
else:
done = True
# Check if there is already a new move to do
nf = self._executor.get_next_future(future)
if nf is not None and axes <= nf._update_axes:
logging.debug("Ending move control early as next move is an update containing %s", axes)
return
# anti-backlash move
self._antiBacklashMove(axes)
def _createFuture(self, axes, update):
"""
Return (CancellableFuture): a future that can be used to manage a move
axes (set of str): the axes that are moved
update (bool): if it's an update move
"""
# TODO: do this via the __init__ of subclass of Future?
f = CancellableFuture() # TODO: make it cancellable too
f._update_axes = set() # axes handled by the move, if update
if update:
# Check if all the axes support it
if all(self.axes[a].canUpdate for a in axes):
f._update_axes = axes
else:
logging.warning("Trying to do a update move on axes %s not supporting update", axes)
return f
@isasync
def moveRel(self, shift, update=False):
if not shift:
return model.InstantaneousFuture()
self._checkMoveRel(shift)
f = self._createFuture(set(shift.keys()), update)
return self._executor.submitf(f, self._doMoveRel, f, shift)
@isasync
def moveAbs(self, pos, update=False):
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
f = self._createFuture(set(pos.keys()), update)
return self._executor.submitf(f, self._doMoveAbs, f, pos)
def stop(self, axes=None):
self._child.stop(axes=axes)
@isasync
def reference(self, axes):
f = self._child.reference(axes)
return f
class PM8742(model.Actuator):
"""
Represents one New Focus picomotor controller 8742.
"""
def __init__(self, name, role, address, axes, stepsize, sn=None, **kwargs):
"""
address (str): ip address (use "autoip" to automatically scan and find the
controller, "fake" for a simulator)
axes (list of str): names of the axes, from the 1st to the 4th, if present.
if an axis is not connected, put a "".
stepsize (list of float): size of a step in m (the smaller, the
bigger will be a move for a given distance in m)
sn (str or None): serial number of the device (eg, "11500"). If None, the
driver will use whichever controller is first found.
inverted (set of str): names of the axes which are inverted (IOW, either
empty or the name of the axis)
"""
if not 1 <= len(axes) <= 4:
raise ValueError("Axes must be a list of 1 to 4 axis names (got %s)" % (axes,))
if len(axes) != len(stepsize):
raise ValueError("Expecting %d stepsize (got %s)" %
(len(axes), stepsize))
self._name_to_axis = {} # str -> int: name -> axis number
for i, n in enumerate(axes):
if n == "": # skip this non-connected axis
continue
self._name_to_axis[n] = i + 1
for sz in stepsize:
if sz > 10e-3: # sz is typically ~1µm, so > 1 cm is very fishy
raise ValueError("stepsize should be in meter, but got %g" % (sz,))
self._stepsize = stepsize
self._address = address
self._sn = sn
self._accesser = self._openConnection(address, sn)
self._recover = False
self._resynchonise()
if name is None and role is None: # For scan only
return
# Seems to really be the device, so handle connection errors fully
self._recover = True
modl, fw, sn = self.GetIdentification()
if modl != "8742":
logging.warning("Controller %s is not supported, will try anyway", modl)
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(max_workers=1) # one task at a time
# Let the controller check the actuators are connected
self.MotorCheck()
axes_def = {}
speed = {}
for n, i in self._name_to_axis.items():
sz = self._stepsize[i - 1]
# TODO: allow to pass the range in m in the arguments
# Position supports ±2³¹, probably not that much in reality, but
# there is no info.
rng = [(-2 ** 31) * sz, (2 ** 31 - 1) * sz]
# Check the actuator is connected
mt = self.GetMotorType(i)
if mt in {MT_NONE, MT_UNKNOWN}:
raise HwError("Controller failed to detect motor %d, check the "
"actuator is connected to the controller" %
(i,))
max_stp_s = {MT_STANDARD: 2000, MT_TINY: 1750}[mt]
srng = (0, self._speedToMS(i, max_stp_s))
speed[n] = self._speedToMS(i, self.GetVelocity(i))
axes_def[n] = model.Axis(range=rng, speed=srng, unit="m")
model.Actuator.__init__(self, name, role, axes=axes_def, **kwargs)
self._swVersion = "%s (IP connection)" % (odemis.__version__,)
self._hwVersion = "New Focus %s (firmware %s, S/N %s)" % (modl, fw, sn)
# Note that the "0" position is just the position at which the
# controller turned on
self.position = model.VigilantAttribute({}, unit="m", readonly=True)
self._updatePosition()
max_speed = max(a.speed[1] for a in axes_def.values())
self.speed = model.MultiSpeedVA(speed, range=(0, max_speed),
unit="m/s", setter=self._setSpeed)
def terminate(self):
if self._executor:
self.stop()
self._executor.shutdown(wait=True)
self._executor = None
if self._accesser:
self._accesser.terminate()
self._accesser = None
def _sendOrderCommand(self, cmd, val="", axis=None):
return self._accesser.sendOrderCommand(cmd, val, axis)
def _sendQueryCommand(self, cmd, val="", axis=None):
"""
Same as accesser's sendQueryCommand, but with error recovery
"""
trials = 0
while True:
try:
return self._accesser.sendQueryCommand(cmd, val, axis)
except IOError: # Typically due to timeout
trials += 1
if not self._recover and trials < 5:
raise
self._recover = False
try:
# can also happen just due to error
# => first read error and see if that explains anything
self._checkError()
except IOError:
# Sometimes the hardware seems to lose connection
# => try to reconnect
logging.warning("Device seems disconnected, will try to reconnect")
# Sometimes the device gets confused and answers are shifted.
# Reset helps, but it also reset the current position, which
# is not handy.
# self._accesser.sendOrderCommand("RS")
self._accesser.terminate()
time.sleep(0.5)
self._accesser = self._openConnection(self._address, self._sn)
self._checkError()
logging.info("Recovered lost connection to device %s", self.name)
finally:
self._recover = True
# Low level functions
def GetIdentification(self):
"""
return (str, str, str):
Model name
Firmware version (and date)
serial number
"""
resp = self._sendQueryCommand("*IDN")
# expects something like this:
# New_Focus 8742 v2.2 08/01/13 11511
try:
m = re.match("\w+ (?P<model>\w+) (?P<fw>v\S+ \S+) (?P<sn>\d+)", resp)
modl, fw, sn = m.groups()
except Exception:
raise IOError("Failed to decode firmware answer '%s'" %
resp.encode('string_escape'))
return modl, fw, sn
def GetMotorType(self, axis):
"""
Read the motor type.
The motor check action must have been performed before to get correct
values.
axis (1<=int<=4): axis number
return (0<=int<=3): the motor type
"""
resp = self._sendQueryCommand("QM", axis=axis)
return int(resp)
def GetVelocity(self, axis):
"""
Read the max speed
axis (1<=int<=4): axis number
return (0<=int<=2000): the speed in step/s
"""
resp = self._sendQueryCommand("VA", axis=axis)
return int(resp)
def SetVelocity(self, axis, val):
"""
Write the max speed
axis (1<=int<=4): axis number
val (1<=int<=2000): the speed in step/s
"""
if not 1 <= val <= 2000:
raise ValueError("Velocity outside of the range 0->2000")
self._sendOrderCommand("VA", "%d" % (val,), axis)
def GetAccel(self, axis):
"""
Read the acceleration
axis (1<=int<=4): axis number
return (0<=int): the acceleration in step/s²
"""
resp = self._sendQueryCommand("AC", axis=axis)
return int(resp)
def SetAccel(self, axis, val):
"""
Write the acceleration
axis (1<=int<=4): axis number
val (1<=int<=200000): the acceleration in step/s²
"""
if not 1 <= val <= 200000:
raise ValueError("Acceleration outside of the range 0->200000")
self._sendOrderCommand("AC", "%d" % (val,), axis)
def MotorCheck(self):
"""
Run the motor check command, that automatically configure the right
values based on the type of motors connected.
"""
self._sendOrderCommand("MC")
def MoveAbs(self, axis, pos):
"""
Requests a move to an absolute position. This is non-blocking.
axis (1<=int<=4): axis number
pos (-2**31 <= int 2*31-1): position in step
"""
self._sendOrderCommand("PA", "%d" % (pos,), axis)
def GetTarget(self, axis):
"""
Read the target position for the given axis
axis (1<=int<=4): axis number
return (int): the position in steps
"""
# Note, it's not clear what's the difference with PR?
resp = self._sendQueryCommand("PA", axis=axis)
return int(resp)
def MoveRel(self, axis, offset):
"""
Requests a move to a relative position. This is non-blocking.
axis (1<=int<=4): axis number
offset (-2**31 <= int 2*31-1): offset in step
"""
self._sendOrderCommand("PR", "%d" % (offset,), axis)
def GetPosition(self, axis):
"""
Read the actual position for the given axis
axis (1<=int<=4): axis number
return (int): the position in steps
"""
resp = self._sendQueryCommand("TP", axis=axis)
return int(resp)
def IsMotionDone(self, axis):
"""
Check whether the axis is in motion
axis (1<=int<=4): axis number
return (bool): False if in motion, True if motion is finished
"""
resp = self._sendQueryCommand("MD", axis=axis)
if resp == "0": # motion in progress
return False
elif resp == "1": # no motion
return True
else:
raise IOError("Failed to decode answer about motion '%s'" %
resp.encode('string_escape'))
def AbortMotion(self):
"""
Stop immediately the motion on all the axes
"""
self._sendOrderCommand("AB")
def StopMotion(self, axis):
"""
Stop nicely the motion (using accel/decel values)
axis (1<=int<=4): axis number
"""
self._sendOrderCommand("ST", axis=axis)
def GetError(self):
"""
Read the oldest error in memory.
The error buffer is FIFO with 10 elements, so it might not be the
latest error if multiple errors have happened since the last time this
function was called.
return (None or (int, str)): the error number and message
"""
# Note: there is another one "TE" which only returns the number, and so
# is faster, but then there is no way to get the message
resp = self._sendQueryCommand("TB")
# returns something like "108, MOTOR NOT CONNECTED"
try:
m = re.match("(?P<no>\d+), (?P<msg>.+)", resp)
no, msg = int(m.group("no")), m.group("msg")
except Exception:
raise IOError("Failed to decode error info '%s'" %
resp.encode('string_escape'))
if no == 0:
return None
else:
return no, msg
def _checkError(self):
"""
Check if an error happened and convert to a python exception
return None
raise NewFocusError if an error happened
"""
err = self.GetError()
if err:
errno, msg = err
raise NewFocusError(errno, msg)
def _resynchonise(self):
"""
Ensures the device communication is "synchronised"
"""
self._accesser.flushInput()
# drop all the errors
while self.GetError():
pass
# high-level methods (interface)
def _updatePosition(self, axes=None):
"""
update the position VA
axes (set of str): names of the axes to update or None if all should be
updated
"""
# uses the current values (converted to internal representation)
pos = self._applyInversion(self.position.value)
for n, i in self._name_to_axis.items():
if axes is None or n in axes:
pos[n] = self.GetPosition(i) * self._stepsize[i - 1]
pos = self._applyInversion(pos)
# it's read-only, so we change it via _value
self.position._value = pos
self.position.notify(self.position.value)
def _speedToMS(self, axis, sps):
"""
Convert speed in step/s to m/s
axis (1<=int<=4): axis number
sps (int): steps/s
return (float): m/s
"""
return sps * self._stepsize[axis - 1]
def _setSpeed(self, value):
"""
value (dict string-> float): speed for each axis
returns (dict string-> float): the new value
"""
if set(value.keys()) != set(self._axes.keys()):
raise ValueError("Requested speed %s doesn't specify all axes %s" %
(value, self._axes.keys()))
for axis, v in value.items():
rng = self._axes[axis].speed
if not rng[0] < v <= rng[1]:
raise ValueError("Requested speed of %f for axis %s not within %f->%f" %
(v, axis, rng[0], rng[1]))
i = self._name_to_axis[axis]
sps = max(1, int(round(v / self._stepsize[i - 1])))
self.SetVelocity(i, sps)
return value
def _createFuture(self):
"""
Return (CancellableFuture): a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event() # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
@isasync
def moveRel(self, shift):
self._checkMoveRel(shift)
shift = self._applyInversion(shift)
# Check if the distance is big enough to make sense
for an, v in shift.items():
aid = self._name_to_axis[an]
if abs(v) < self._stepsize[aid - 1]:
# TODO: store and accumulate all the small moves instead of dropping them?
del shift[an]
logging.info("Dropped too small move of %g m < %g m",
abs(v), self._stepsize[aid - 1])
if not shift:
return model.InstantaneousFuture()
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
@isasync
def moveAbs(self, pos):
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
pos = self._applyInversion(pos)
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
moveAbs.__doc__ = model.Actuator.moveAbs.__doc__
def stop(self, axes=None):
self._executor.cancel()
def _doMoveRel(self, future, pos):
"""
Blocking and cancellable relative move
future (Future): the future it handles
pos (dict str -> float): axis name -> relative target position
"""
with future._moving_lock:
end = 0 # expected end
moving_axes = set()
for an, v in pos.items():
aid = self._name_to_axis[an]
moving_axes.add(aid)
steps = int(round(v / self._stepsize[aid - 1]))
self.MoveRel(aid, steps)
# compute expected end
dur = abs(steps) * self._stepsize[aid - 1] / self.speed.value[an]
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
logging.debug("move successfully completed")
def _doMoveAbs(self, future, pos):
"""
Blocking and cancellable absolute move
future (Future): the future it handles
pos (dict str -> float): axis name -> absolute target position
"""
with future._moving_lock:
end = 0 # expected end
old_pos = self._applyInversion(self.position.value)
moving_axes = set()
for an, v in pos.items():
aid = self._name_to_axis[an]
moving_axes.add(aid)
steps = int(round(v / self._stepsize[aid - 1]))
self.MoveAbs(aid, steps)
# compute expected end
dur = abs(v - old_pos[an]) / self.speed.value[an]
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
logging.debug("move successfully completed")
def _waitEndMove(self, future, axes, end=0):
"""
Wait until all the given axes are finished moving, or a request to
stop has been received.
future (Future): the future it handles
axes (set of int): the axes IDs to check
end (float): expected end time
raise:
CancelledError: if cancelled before the end of the move
"""
moving_axes = set(axes)
last_upd = time.time()
last_axes = moving_axes.copy()
try:
while not future._must_stop.is_set():
for aid in moving_axes.copy(): # need copy to remove during iteration
if self.IsMotionDone(aid):
moving_axes.discard(aid)
if not moving_axes:
# no more axes to wait for
break
# Update the position from time to time (10 Hz)
if time.time() - last_upd > 0.1 or last_axes != moving_axes:
last_names = set(n for n, i in self._name_to_axis.items() if i in last_axes)
self._updatePosition(last_names)
last_upd = time.time()
last_axes = moving_axes.copy()
# Wait half of the time left (maximum 0.1 s)
left = end - time.time()
sleept = max(0.001, min(left / 2, 0.1))
future._must_stop.wait(sleept)
# TODO: timeout if really too long
else:
logging.debug("Move of axes %s cancelled before the end", axes)
# stop all axes still moving them
for i in moving_axes:
self.StopMotion(i)
future._was_stopped = True
raise CancelledError()
except Exception:
raise
else:
# Did everything really finished fine?
self._checkError()
finally:
self._updatePosition() # update (all axes) with final position
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set() # tell the thread taking care of the move it's over
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
@classmethod
def scan(cls):
"""
returns (list of (str, dict)): name, kwargs
Note: it's obviously not advised to call this function if a device is already under use
"""
logging.info("Scanning for New Focus controllers in progress...")
found = [] # (list of 2-tuple): name, kwargs
try:
conts = cls._scanOverIP()
except IOError as exp:
logging.exception("Failed to scan for New Focus controllers: %s", exp)
for hn, host, port in conts:
try:
logging.debug("Trying controller at %s", host)
dev = cls(None, None, address=host, axes=["a"], stepsize=[1e-6])
modl, fw, sn = dev.GetIdentification()
# find out about the axes
dev.MotorCheck()
axes = []
stepsize = []
for i in range(1, 5):
mt = dev.GetMotorType(i)
n = chr(ord('a') + i - 1)
# No idea about the stepsize, but make it different to allow
# distinguishing between motor types
if mt == MT_STANDARD:
ss = 1e-6
elif mt == MT_TINY:
ss = 0.1e-6
else:
n = ""
ss = 0
axes.append(n)
stepsize.append(ss)
except IOError:
# not possible to use this port? next one!
continue
except Exception:
logging.exception("Error while communicating with controller %s @ %s:%s",
hn, host, port)
continue
found.append(("NF-%s-%s" % (modl, sn),
{"address": host,
"axes": axes,
"stepsize": stepsize,
"sn": sn})
)
return found
@classmethod
def _openConnection(cls, address, sn=None):
"""
return (Accesser)
"""
if address == "fake":
host, port = "fake", 23
elif address == "autoip":
conts = cls._scanOverIP()
if sn is not None:
for hn, host, port in conts:
# Open connection to each controller and ask for their SN
dev = cls(None, None, address=host, axes=["a"], stepsize=[1e-6])
_, _, devsn = dev.GetIdentification()
if sn == devsn:
break
else:
raise HwError("Failed to find New Focus controller %s over the "
"network. Ensure it is turned on and connected to "
"the network." % (sn))
else:
# just pick the first one
# TODO: only pick the ones of model 8742
try:
hn, host, port = conts[0]
logging.info("Connecting to New Focus %s", hn)
except IndexError:
raise HwError("Failed to find New Focus controller over the "
"network. Ensure it is turned on and connected to "
"the network.")
else:
# split the (IP) port, separated by a :
if ":" in address:
host, ipport_str = port.split(":")
port = int(ipport_str)
else:
host = address
port = 23 # default
return IPAccesser(host, port)
@staticmethod
def _scanOverIP():
"""
Scan the network for all the responding new focus controllers
Note: it actually calls a separate executable because it relies on opening
a network port which needs special privileges.
return (list of (str, str, int)): hostname, ip address, and port number
"""
# Run the separate program via authbind
try:
exc = os.path.join(os.path.dirname(__file__), "nfpm_netscan.py")
out = subprocess.check_output(["authbind", "python", exc])
except CalledProcessError as exp:
# and handle all the possible errors:
# - no authbind (127)
# - cannot find the separate program (2)
# - no authorisation (13)
ret = exp.returncode
if ret == 127:
raise IOError("Failed to find authbind")
elif ret == 2:
raise IOError("Failed to find %s" % exc)
elif ret == 13:
raise IOError("No permission to open network port 23")
# or decode the output
# hostname \t host \t port
ret = []
for l in out.split("\n"):
if not l:
continue
try:
hn, host, port = l.split("\t")
except Exception:
logging.exception("Failed to decode scanner line '%s'", l)
ret.append((hn, host, port))
return ret
class PMD401Bus(Actuator):
"""
This represents the PMD401 motor controller bus for the PiezoMotor LEGS motors. It supports multiple controllers
(each one handling one axis), connected in a daisy chain. Only the first
controller is directly connected to the computer.
The specification for the hardware interface can be found in the document
"PiezoMotor_PMD401_Technical_Manual.pdf".
This driver works equally well with the PMD301 controller.
"""
def __init__(self,
name,
role,
port,
axes,
inverted=None,
param_file=None,
**kwargs):
"""
:param axes (dict: str -> dict): axis name --> axis parameters
Each axis is specified by a set of parameters.
After successful configuration with the pmconfig.py script, the only required parameter for a default motor
is the address which was set during the configuration process.
The spc parameter (conversion between motor steps and encoder counts) is typically saved in the flash
memory of the controller during the configuration process. The flash value is overridden by the
value in the parameter dict.
Depending on the type of motor, the encoder_resolution and range might need to be adjusted.
Axis parameters:
axis_number (0 <= int <= 127): typically 1-3 for x-z, required
closed_loop (bool): True for closed loop (with encoder), default to False
encoder_resolution (float): number of encoder counts per meter, default to 1.22e-9
motorstep_resolution (float): number of motor steps per m, default to 5e-6
range (tuple of float): in m, default to STROKE_RANGE
speed (float): speed in m/s
unit (str), default to m
:param param_file (str or None): (absolute or relative) path to a tmcm.tsv file which will be used to initialise
the axis parameters.
"""
self._axis_map = {} # axis name -> axis number used by controller
self._closed_loop = {} # axis name (str) -> bool (True if closed loop)
self._speed = {} # axis name (str) -> speed in unit/s
self._speed_steps = {
} # axis name (str) -> int, speed in steps per meter
self._counts_per_meter = {} # axis name (str) -> float
self._steps_per_meter = {} # axis name (str) -> float
self._portpattern = port
# Parse axis parameters and create axis
axes_def = {} # axis name -> Axis object
for axis_name, axis_par in axes.items():
if 'axis_number' in axis_par:
axis_num = axis_par['axis_number']
if axis_num not in range(128):
raise ValueError(
"Invalid axis number %s, needs to be 0 <= int <= 127."
% axis_num)
elif axis_num in self._axis_map.values():
axname = self._axis_map[axis_num]
raise ValueError(
"Invalid axis number %s, already assigned to axis %s."
% (axis_num, axname))
else:
self._axis_map[axis_name] = axis_par['axis_number']
else:
raise ValueError("Axis %s has no axis number." % axis_name)
if 'closed_loop' in axis_par:
closed_loop = axis_par['closed_loop']
else:
closed_loop = False
logging.info(
"Axis parameter \"closed_loop\" not specified for axis %s. Assuming open-loop.",
axis_name)
self._closed_loop[axis_name] = closed_loop
if 'motorstep_resolution' in axis_par:
self._steps_per_meter[
axis_name] = 1 / axis_par['motorstep_resolution']
else:
self._steps_per_meter[
axis_name] = 1 / DEFAULT_MOTORSTEP_RESOLUTION
logging.info(
"Axis %s has no motorstep resolution, assuming %s.",
axis_name, DEFAULT_MOTORSTEP_RESOLUTION)
if 'encoder_resolution' in axis_par:
self._counts_per_meter[
axis_name] = 1 / axis_par['encoder_resolution']
else:
self._counts_per_meter[
axis_name] = 1 / DEFAULT_ENCODER_RESOLUTION
logging.info("Axis %s has no encoder resolution, assuming %s.",
axis_name, DEFAULT_ENCODER_RESOLUTION)
if 'range' in axis_par:
axis_range = [
float(axis_par['range'][0]),
float(axis_par['range'][1])
]
else:
axis_range = STROKE_RANGE
logging.info("Axis %s has no range. Assuming %s", axis_name,
axis_range)
if 'speed' in axis_par:
self._speed[axis_name] = axis_par['speed']
else:
self._speed[axis_name] = DEFAULT_AXIS_SPEED
logging.info(
"Axis %s was not given a speed value. Assuming %s",
axis_name, self._speed[axis_name])
self._speed_steps[axis_name] = int(
round(self._speed[axis_name] *
self._steps_per_meter[axis_name]))
if 'unit' in axis_par:
axis_unit = axis_par['unit']
else:
axis_unit = "m"
logging.info("Axis %s has no unit. Assuming %s", axis_name,
axis_unit)
ad = model.Axis(canAbs=closed_loop,
unit=axis_unit,
range=axis_range)
axes_def[axis_name] = ad
Actuator.__init__(self,
name,
role,
axes=axes_def,
inverted=inverted,
**kwargs)
self._executor = CancellableThreadPoolExecutor(
max_workers=1) # one task at a time
self._ser_access = threading.RLock()
# Connect to hardware
self._port = None # port number
min_axis = min(self._axis_map.values())
self._serial = self._findDevice(port, min_axis)
self._recovering = False
# Get version
hwVersions = []
for ax_name, ax_num in self._axis_map.items():
ver = self.getVersion(ax_num)
sn = self.getSerialNumber(ax_num)
hwVersions.append("Axis %s ('%s') version: %s, " %
(ax_num, ax_name, ver) +
"serial number: %s" % sn)
self._hwVersion = ", ".join(hwVersions)
logging.debug("Hardware versions: %s", hwVersions)
# Configuration
for axis in self._axis_map.values():
self.setWaveform(axis, WAVEFORM_DELTA)
driver_name = getSerialDriver(self._port)
self._swVersion = "Serial driver: %s" % (driver_name, )
# Position and referenced VAs
self.position = model.VigilantAttribute({}, unit="m", readonly=True)
self.referenced = model.VigilantAttribute({}, readonly=True)
self._updatePosition()
for axname in self._axis_map.keys():
self.referenced.value[
axname] = False # just assume they haven't been referenced
self.speed = model.VigilantAttribute(self._speed,
unit="m/s",
readonly=True)
# Write parameters from parameter file
if param_file:
if not os.path.isabs(param_file):
param_file = os.path.join(os.path.dirname(__file__),
param_file)
try:
f = open(param_file)
except Exception as ex:
raise ValueError("Failed to open file %s: %s" %
(param_file, ex))
try:
axis_params = self.parse_tsv_config(f)
except Exception as ex:
raise ValueError("Failed to parse file %s: %s" %
(param_file, ex))
f.close()
logging.debug("Extracted param file config: %s", axis_params)
self.apply_params(axis_params)
def terminate(self):
# terminate can be called several times, do nothing if ._serial is already None
if self._serial is None:
return
self._serial.close()
self._serial = None
for axis in self._axis_map.values():
self.setWaveform(axis, WAVEFORM_PARK) # power off
super(PMD401Bus, self).terminate()
def stop(self, axes=None):
self._executor.cancel()
axes = axes or self._axis_map.keys()
for ax in axes:
self.stopAxis(self._axis_map[ax])
@isasync
def moveRel(self, shift):
if not shift:
return model.InstantaneousFuture()
self._checkMoveRel(shift)
shift = self._applyInversion(shift)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
@isasync
def moveAbs(self, pos):
self._checkMoveAbs(pos)
pos = self._applyInversion(pos)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
@isasync
def reference(self, axes):
self._checkReference(axes)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doReference, f, axes)
return f
def _doReference(self, f, axes):
self._check_hw_error()
# Request referencing on all axes
# Referencing procedure: index signal is in the middle of the rod (when using encoder)
# * move to the limit switch in the negative direction (fixed end of the rod)
# * once we reach the limit switch, PMD error 6 will be raised
# * move back in the opposite direction until indexing signal is registered
# In case there is no encoder, it is still possible to reference. The motor has an internal indexing
# signal 8.9 µm from fixed end of the rod (the end of the rod should be the most negative position if
# the axis is not inverted). By referencing, this position can be set to 0 and absolute moves are possible
# (although with less accuracy). However, we do not currently support this type of referencing without
# encoder. With our hardware attached to the motor, it is impossible to reach the 8.9 µm indexing position.
for axname in axes:
if f._must_stop.is_set():
self.stopAxis(self._axis_map[axname])
raise CancelledError()
axis = self._axis_map[axname]
self.referenced._value[axname] = False
# First, search for the index in negative direction.
idx_found = self._search_index(f, axname, direction=-1)
# If it wasn't found, try again in positive direction.
if not idx_found:
logging.debug("Referencing axis %s in the positive direction",
axis)
idx_found = self._search_index(f, axname, direction=1)
# If it's still not found, something went wrong.
if not idx_found:
raise ValueError(
"Couldn't find index on axis %s (%s), referencing failed."
% (axis, axname))
# Referencing complete
logging.debug("Finished referencing axis %s." % axname)
self.stopAxis(
axis
) # the axis should already be stopped, make sure for safety
self.referenced._value[axname] = True
# read-only so manually notify
self.referenced.notify(self.referenced.value)
self._updatePosition()
def _search_index(self, f, axname, direction):
"""
:param f (Future)
:param axname (str): axis name (as seen by the user)
:param direction (-1 or 1): -1 for negative direction (beginning of the rod), 1 for positive direction
returns (bool): True if index was found, false if limit was reached
raises PMDError for all other errors except limit exceeded error
IOError in case of timeout
"""
axis = self._axis_map[axname]
maxdist = self._axes[axname].range[1] - self._axes[axname].range[
0] # complete rodlength
steps = int(maxdist * self._steps_per_meter[axname])
maxdur = maxdist / self._speed[axname] + 1
end_time = time.time() + 2 * maxdur
logging.debug("Searching for index in direction %s.", direction)
self.startIndexMode(axis)
self.moveToIndex(axis, steps * direction)
index_found = False
while not index_found:
if f._must_stop.is_set():
self.stopAxis(axis)
raise CancelledError()
# Check for timeout
if time.time() > end_time:
self.stopAxis(axis) # exit index mode
raise IOError(
"Timeout while waiting for end of motion on axis %s" %
axis)
# Check if limit is reached
try:
self._check_hw_error()
except PMDError as ex:
if ex.errno == 6: # external limit reached
logging.debug("Axis %d limit reached during referencing",
axis)
self.stopAxis(
axis
) # that seems to be necessary after reaching the limit
break
else:
raise
# Get index status
index_found = self.getIndexStatus(self._axis_map[axname])[-1]
time.sleep(0.05)
return index_found
def _doMoveAbs(self, f, pos):
self._check_hw_error()
self._updatePosition()
current_pos = self._applyInversion(self.position.value)
shifts = {}
if f._must_stop.is_set():
raise CancelledError()
for axname, val in pos.items():
if self._closed_loop[axname]:
shifts[axname] = val - current_pos[axname]
encoder_cnts = round(val * self._counts_per_meter[axname])
self.runAbsTargetMove(self._axis_map[axname], encoder_cnts,
self._speed_steps[axname])
else:
# No absolute move for open-loop => convert to relative move
shifts[axname] = val - current_pos[axname]
steps_float = shifts[axname] * self._steps_per_meter[axname]
steps = int(steps_float)
usteps = int((steps_float - steps) * USTEPS_PER_STEP)
self.runMotorJog(self._axis_map[axname], steps, usteps,
self._speed_steps[axname])
try:
self._waitEndMotion(f, shifts)
finally:
# Leave target mode in case of closed-loop move
for ax in pos:
self.stopAxis(self._axis_map[ax])
self._updatePosition()
def _doMoveRel(self, f, shift):
self._check_hw_error()
shifts = {}
if f._must_stop.is_set():
raise CancelledError()
for axname, val in shift.items():
if self._closed_loop[axname]:
shifts[axname] = val
encoder_cnts = val * self._counts_per_meter[axname]
self.runRelTargetMove(self._axis_map[axname], encoder_cnts,
self._speed_steps[axname])
else:
shifts[axname] = val
steps_float = val * self._steps_per_meter[axname]
steps = int(steps_float)
usteps = int((steps_float - steps) * USTEPS_PER_STEP)
self.runMotorJog(self._axis_map[axname], steps, usteps,
self._speed_steps[axname])
try:
self._waitEndMotion(f, shifts)
finally:
# Leave target mode in case of closed-loop move
for ax in shift:
self.stopAxis(self._axis_map[ax])
self._updatePosition()
def _waitEndMotion(self, f, shifts):
"""
Wait until move is done.
:param f: (CancellableFuture) move future
:param shifts: (dict: str --> floats) relative move (in m) between current position and previous position
"""
dur = 0
for ax, shift in shifts.items():
dur = max(abs(shift / self._speed[ax]), dur)
max_dur = dur * 2 + 1
logging.debug("Expecting a move of %g s, will wait up to %g s", dur,
max_dur)
end_time = time.time() + max_dur
moving_axes = set(shifts.keys()) # All axes (still) moving
logging.debug(f"Axes {moving_axes} are moving.")
while moving_axes:
if f._must_stop.is_set():
for axname in moving_axes:
self.stopAxis(self._axis_map[axname])
raise CancelledError()
if time.time() > end_time:
raise TimeoutError(
"Timeout while waiting for end of motion on axes %s for %g s"
% (moving_axes, max_dur))
for axname in moving_axes.copy(
): # Copy as the set can change during the iteration
axis = self._axis_map[axname]
if self._closed_loop[axname]:
moving = self.isMovingClosedLoop(axis)
logging.debug(f"Axis {axis} is moving closed loop."
) if moving else None
else:
moving = self.isMovingOpenLoop(axis)
logging.debug(f"Axis {axis} is moving open loop."
) if moving else None
if not moving:
logging.debug(f"Axis {axis} finished moving.")
moving_axes.discard(axname)
self._check_hw_error()
time.sleep(0.05)
logging.debug("All axis finished moving")
def _check_hw_error(self):
"""
Read hardware status and raise exception if error is detected.
"""
for ax, axnum in self._axis_map.items():
status = self.getStatus(axnum)
# Always log the status
logging.debug("Device status: %s", status)
if status[0] & 8:
raise PMDError(
1,
"Communication Error on axis %s (wrong baudrate, data collision, "
"or buffer overflow)" % ax)
elif status[0] & 4:
raise PMDError(
2,
"Encoder error on axis %s (serial communication or reported error from "
"serial encoder)" % ax)
elif status[0] & 2:
raise PMDError(
3,
"Supply voltage or motor fault was detected on axis %s." %
ax)
elif status[0] & 1:
raise PMDError(
4,
"Command timeout occurred or a syntax error was detected on axis %s when "
"response was not allowed." % ax)
elif status[1] & 8:
# That's really not a big deal since everything is working fine after power-on, so don't raise an error.
logging.debug(
"Power-on/reset has occurred and detected on axis %s." %
ax)
elif status[1] & 4:
raise PMDError(
6, "External limit reached, detected on axis %s." % ax)
def _updatePosition(self):
"""
Update the position VA.
"""
pos = {}
for axname, axis in self._axis_map.items():
# TODO: if not in closed-loop, it's probably because there is no encoder, so we need a different way
pos[axname] = self.getEncoderPosition(axis)
logging.debug("Reporting new position at %s", pos)
pos = self._applyInversion(pos)
self.position._set_value(pos, force_write=True)
def stopAxis(self, axis):
self._sendCommand(b'X%dS' % axis)
def getVersion(self, axis):
"""
:param axis: (int) axis number
:returns (str): controller type and firmware version, e.g. 'PMD401 V13'
"""
return self._sendCommand(b'X%d?' % axis)
def getSerialNumber(self, axis):
"""
:param axis: (int) axis number
:returns (str): serial number
"""
return self._sendCommand(b'X%dY42' % axis)
def initFromFlash(self, axis):
"""
Initialize settings from values stored in flash.
:param axis: (int) axis number
"""
# 2 for init from flash, 3 for factory values
self._sendCommand(b'X%dY1,2' % axis)
def setLimitType(self, axis, limit_type):
"""
:param axis: (int) axis number
:param limit_type: (0 <= int <= 3) 0 no limit, 1 active high, 2 active low
"""
self._sendCommand(b'X%dY2,%d' % (axis, limit_type))
def getEncoderPosition(self, axis):
"""
:param axis: (int) axis number
:returns (float): current position of the axis as reported by encoders (in m)
"""
axname = [name for name, num in self._axis_map.items()
if num == axis][0] # get axis name from number
return int(self._sendCommand(
b'X%dE' % axis)) / self._counts_per_meter[axname]
def runRelTargetMove(self, axis, encoder_cnts, speed):
"""
Closed-loop relative move.
:param axis: (int) axis number
:param encoder_cnts: (int)
:param speed: (int) speed in motor steps per s
"""
# There are two possibilities: move relative to current position (XC) and move relative to
# target position (XR). We are moving relative to the current position (might be more intuitive
# if something goes wrong and we're stuck in the wrong position).
self._sendCommand(b'X%dC%d,%d' % (axis, encoder_cnts, speed))
def runMotorJog(self, axis, motor_steps, usteps, speed):
"""
Open loop stepping.
:param axis: (int) axis number
:param motor_steps: (int) number of motor steps to move
:param usteps: (int) number of microsteps (1 motor step = 8192 microsteps)
:param speed: (int) speed in steps / m
"""
self._sendCommand(b'X%dJ%d,%d,%d' % (axis, motor_steps, usteps, speed))
def runAbsTargetMove(self, axis, encoder_cnts, speed):
"""
Closed loop move.
:param axis: (int) axis number
:param encoder_cnts: (int)
:param speed: speed in motor steps per s
"""
self._sendCommand(b'X%dT%d,%d' % (axis, encoder_cnts, speed))
def setWaveform(self, axis, wf):
"""
:param axis: (int) axis number
:param wf: (WAVEFORM_RHOMB, WAVEFORM_DELTA, WAVEFORM_PARK) waveform to set
"""
if wf not in (WAVEFORM_DELTA, WAVEFORM_RHOMB, WAVEFORM_PARK):
raise ValueError("wf %s not a valid waveform" % wf)
self._sendCommand(b'X%dM%d' % (axis, wf))
def getStatus(self, axis):
"""
:returns (list of 4 int): 4-bit status code
The most important values are the following
First bit:
8: communication error (wrong baudrate, data collision, or buffer overflow)
4: encoder error (serial communication or reported error from serial encoder)
2: voltage error (supply voltage or motor fault was detected)
1: command error (command timeout occurred or a syntax error was detected when response was not allowed)
Second bit:
8: reset (power on/ reset occurred)
4: set if the last motor movement was stopped by external limit switch
1: index (index signal was detected since last report)
For all codes, please refer to the PMD-401 manual.
"""
return [int(i) for i in self._sendCommand(b'X%dU0' % axis)]
def isMovingClosedLoop(self, axis):
"""
:param axis: (int) axis number
:returns: (bool) True if moving, False otherwise
"""
_, d2, d3, _ = self.getStatus(axis)
# Check d2 (second status value) bit 2 (external limit)
if d2 & 0b100:
logging.debug(
f"External limit reached on axis {axis}, current position is {self.position.value}"
)
raise PMDError(6, f"External limit reached on axis {axis}.")
# Check d3 (third status value) bit 2 (targetLimit: position limit reached) and bit 0 (targetReached)
if not d3 & 0b010: # closed loop not active, thus it is not moving in closed loop
logging.debug(
f"Closed loop not active, therefore not moving in closed loop on axis {axis}."
)
return False
elif d3 & 0b101:
logging.debug(
f"Target reached or position limit reached on axis {axis}.")
return False
else:
return True
def isMovingOpenLoop(self, axis):
"""
:param axis: (int) axis number
:returns: (bool) True if moving, False otherwise
"""
resp = self._sendCommand(
b'X%dJ' % axis
) # will be 0 if finished, otherwise +/-222 (contrary to manual!)
return int(resp) != 0
def startIndexMode(self, axis):
"""
Enters index mode.
"""
self._sendCommand(b'X%dN4' % axis)
def moveToIndex(self, axis, dist):
"""
Move towards the index until it's found.
"""
axname = [name for name, num in self._axis_map.items()
if num == axis][0] # get axis name from number
self._sendCommand(b'X%dI%d,0,%d' %
(axis, dist, self._speed_steps[axname]))
def getIndexStatus(self, axis):
"""
Returns a description of the index status.
:returns (tuple of 4):
mode (0 or 1): index mode (1 if position has been reset at index)
position (float):
logged (bool): position was logged since last report
indexed (bool): position has been reset at index
"""
# Check if referencing was successful
# Response looks like this: "1,132.,indexed"
ret = self._sendCommand(b'X%dN?' % axis).split(',')
try:
mode = ret[0]
if mode == '1':
mode = 1
else: # empty string means mode 0
mode = 0
if ret[1][-1] == '.':
# . means position was logged since last report
logged = True
position = ret[1][:-1]
else:
logged = False
position = ret[1]
if len(ret) > 2 and 'indexed' in ret[2]:
indexed = True
else:
indexed = False
return mode, position, logged, indexed
except Exception as ex:
logging.exception("Failed to parse index status %s, ex: %s", ret,
ex)
raise
def setAxisAddress(self, current_address, new_address):
"""
Set the address of the axis. The factory default is 0 for all boards. Don't use this
command if multiple axes with the same number are connected.
:param current_address: (int) current axis number
:param new_address: (int) new axis number
"""
self._sendCommand(b"X%dY40,%d" % (current_address, new_address))
def runAutoConf(self, axis):
"""
Runs automatic configuration for the encoder parameters.
:param axis: (int) axis number
"""
self._sendCommand(b"X%dY25,1" % axis)
def writeParamsToFlash(self, axis):
self._sendCommand(b"X%dY32" % axis)
def setParam(self, axis, param, value):
self._sendCommand(b"X%dY%d,%d" % (axis, param, value))
def readParam(self, axis, param):
"""
:returns (str): parameter value from device
"""
return self._sendCommand(b"X%dY%d" % (axis, param))
def _createMoveFuture(self):
"""
:returns: (CancellableFuture) a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event(
) # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set(
) # tell the thread taking care of the move it's over
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
def _sendCommand(self, cmd):
"""
:param cmd: (bytes) command to be sent to the hardware
:returns: (str) response
"""
cmd += EOL
with self._ser_access:
logging.debug("Sending command %s", to_str_escape(cmd))
try:
self._serial.write(cmd)
# TODO: what kind of exception is raised? Needs to be more specific.
except:
logging.warning("Failed to read from PMT Control firmware, "
"trying to reconnect.")
if self._recovering:
raise
else:
self._tryRecover()
# don't send command again
raise IOError("Failed to read from PMT Control firmware, "
"restarted serial connection.")
resp = b""
while resp[-len(EOL):] != EOL:
try:
char = self._serial.read()
# TODO: what kind of exception is raised? Needs to be more specific.
except:
logging.warning(
"Failed to read from PMT Control firmware, "
"trying to reconnect.")
if self._recovering:
raise
else:
self._tryRecover()
# don't send command again
raise IOError(
"Failed to read from PMT Control firmware, "
"restarted serial connection.")
if not char:
raise IOError("Timeout after receiving %s" %
to_str_escape(resp))
else:
resp += char
logging.debug("Received response %s", to_str_escape(resp))
# Check response (command should be echoed back)
if not resp.startswith(cmd[:-len(EOL) - 1]):
raise IOError("Response starts with %s != %s" %
(to_str_escape(resp[:len(cmd)]), cmd))
if b"_??_" in resp:
raise ValueError(
"Received response %s, command %s not understood." %
(to_str_escape(resp), cmd))
if b"!" in resp:
raise PMDError(0, to_str_escape(resp))
# Format:
# * for query with response: <cmd>:<ret><EOL> (will return <ret>)
# * for set command without response: <cmd><EOL> (will return "")
return resp[len(cmd) + 1 - len(EOL):-len(EOL)].decode("latin1")
def _tryRecover(self):
self._recovering = True
self.state._set_value(HwError("Connection lost, reconnecting..."),
force_write=True)
# Retry to open the serial port (in case it was unplugged)
# _ser_access should already be acquired, but since it's an RLock it can be acquired
# again in the same thread
try:
with self._ser_access:
while True:
if self._serial:
self._serial.close()
self._serial = None
try:
logging.debug("Searching for the device on port %s",
self._portpattern)
min_axis = min(self._axis_map.values())
self._port = self._findDevice(self._portpattern,
min_axis)
except IOError:
time.sleep(2)
except Exception:
logging.exception(
"Unexpected error while trying to recover device")
raise
else:
# We found it back!
break
# it now should be accessible again
self.state._set_value(model.ST_RUNNING, force_write=True)
logging.info("Recovered device on port %s", self._port)
finally:
self._recovering = False
def _findDevice(self, port, address=0):
"""
Look for a compatible device
port (str): pattern for the port name
address (None or int): the address of the stage controller
return (serial, int): the (opened) serial port used, and the actual address
raises:
IOError: if no device are found
"""
if port.startswith("/dev/fake"):
names = [port]
elif os.name == "nt":
raise NotImplementedError("Windows not supported")
else:
names = glob.glob(port)
for n in names:
try:
ser = self._openSerialPort(n)
except IOError as ex:
# not possible to use this port? next one!
logging.info("Skipping port %s, which is not available (%s)",
n, ex)
continue
# check whether it answers with the right address
try:
# If any garbage was previously received, make it discarded.
self._serial = ser
self._serial.flush()
v = self.getVersion(address)
# The driver was writte for PMD401, but PMD301 has the same API and is more compatible with our hardware
if 'PMD401 ' in v or 'PMD301 ' in v:
self._port = n
return ser # found it!
except Exception as ex:
logging.debug(
"Port %s doesn't seem to have a PMD device connected: %s",
n, ex)
ser.close() # make sure to close/unlock that port
else:
raise IOError(
"Failed to find a PMD controller with adress %s on ports '%s'. "
"Check that the device is turned on and "
"connected to the computer." % (
address,
port,
))
@staticmethod
def _openSerialPort(port):
"""
Opens the given serial port the right way for a PiezoMotor PMD device.
port (string): the name of the serial port (e.g., /dev/ttyUSB0)
return (serial): the opened serial port
raise HwError: if the serial port cannot be opened (doesn't exist, or
already opened)
"""
# For debugging purpose
if port == "/dev/fake":
return PMDSimulator(timeout=0.1)
try:
ser = serial.Serial(
port=port,
baudrate=BAUDRATE,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=0.3, # s
)
except IOError:
raise HwError("Failed to find a PMD controller on port '%s'. "
"Check that the device is turned on and "
"connected to the computer." % (port, ))
return ser
@classmethod
def scan(cls):
"""
returns (list of 2-tuple): name, kwargs
Note: it's obviously not advised to call this function if a device is already under use
"""
ports = [p.device for p in serial.tools.list_ports.comports()]
logging.info("Scanning for Piezomotor controllers in progress...")
found = [] # (list of 2-tuple): name, kwargs
for p in ports:
axes = {}
for axis in range(5): # most of the time, it's a small axis number
try:
logging.debug("Trying port %s, axis %s", p, axis)
dev = cls(None, None, p, axes={"x": {"axis_number": axis}})
except IOError:
# not possible to use this port? next one!
continue
except Exception:
logging.exception("Error while communicating with port %s",
p)
continue
axes["a%s" % axis] = {"axis_number": axis}
try:
ver = dev.getVersion(axis)
except Exception as ex:
ver = "Unknown"
logging.error(
"Could not get version number for device on axis %s, ex: %s",
axis, ex)
if axes:
found.append(("%s" % ver, {"port": p, "axes": axes}))
return found
@staticmethod
def parse_tsv_config(f):
"""
Parse a tab-separated value (TSV) file in the following format:
axis param value # comment
axis 0->127 (axis: number)
param is the parameter number (int)
value is a number (int))
f (File): opened file
return:
axis_params (dict (int, int) -> int): axis number/param number -> value
"""
axis_params = {} # (axis/add) -> val (int)
# read the parameters "database" the file
for l in f:
# comment or empty line?
mc = re.match(r"\s*(#|$)", l)
if mc:
logging.debug("Comment line skipped: '%s'", l.rstrip("\n\r"))
continue
m = re.match(
r"(?P<num>[0-9]+)\t(?P<param>[0-9]+)\t(?P<value>[-+]?[0-9]+)\s*(#.*)?$",
l)
if not m:
raise ValueError("Failed to parse line '%s'" %
l.rstrip("\n\r"))
num, add, val = int(m.group("num")), int(m.group("param")), int(
m.group("value"))
axis_params[(num, add)] = val
return axis_params
def apply_params(self, params):
for (axis, param), val in params.items():
self.setParam(axis, param, val)
class PMD401Bus(Actuator):
"""
This represents the PMD401 motor controller bus for the PiezoMotor LEGS motors. It supports multiple controllers
(each one handling one axis), connected in a daisy chain. Only the first
controller is directly connected to the computer.
The specification for the hardware interface can be found in the document
"PiezoMotor_PMD401_Technical_Manual.pdf".
"""
def __init__(self, name, role, port, axes, inverted=None, **kwargs):
"""
:param axes (dict: {"x", "y", "z"} --> dict): axis name --> axis parameters
Each axis is specified by a set of parameters.
After successful configuration with the pmconfig.py script, the only required parameter for a default motor
is the address which was set during the configuration process.
The spc parameter (conversion between motor steps and encoder counts) is typically saved in the flash
memory of the controller during the configuration process. The flash value is overridden by the
value in the parameter dict.
Depending on the type of motor, the encoder_resolution and range might need to be adjusted.
Axis parameters:
axis_number (0 <= int <= 127): typically 1-3 for x-z, required
closed_loop (bool): True for closed loop (with encoder), default to True
encoder_resolution (float): encoder resolution in m/step
spc (float): motor steps per encoder count, default to value in non-volatile memory
limit_type (0 <= int <= 2): type of limit switch, 0: no limit, 1: active high, 2: active low, default 0
range (tuple of float): in m, default to (0, STROKE_RANGE)
speed (float): speed in m/s
unit (str), default to m
"""
self._axis_map = {} # axis name -> axis number used by controller
self._closed_loop = {} # axis name (str) -> bool (True if closed loop)
self._speed_steps = {
} # axis name (str) -> int, speed in steps per meter
self._portpattern = port
# Conversion factors
# Count refers to encoder counts, step refers to motor steps. The encoder counts are fixed and given
# as a parameter to the axis, the motor counts are determined during configuration and are usually
# stored in flash memory.
# ._steps_per_meter is redundant, but convenient
self._steps_per_count = {} # axis name (str) -> float
self._steps_per_meter = {} # axis name (str) -> float
self._counts_per_meter = {} # axis name (str) -> float
# Parse axis parameters and create axis
axes_def = {} # axis name -> Axis object
for axis_name, axis_par in axes.items():
if 'axis_number' in axis_par:
axis_num = axis_par['axis_number']
if axis_num not in range(128):
raise ValueError(
"Invalid axis number %s, needs to be 0 <= int <= 127."
% axis_num)
elif axis_num in self._axis_map.values():
axname = self._axis_map[axis_num]
raise ValueError(
"Invalid axis number %s, already assigned to axis %s."
% (axis_num, axname))
else:
self._axis_map[axis_name] = axis_par['axis_number']
else:
raise ValueError("Axis %s has no axis number." % axis_name)
if 'closed_loop' in axis_par:
closed_loop = axis_par['closed_loop']
else:
closed_loop = False
logging.info(
"Axis mode (closed/open loop) not specified for axis %s. Assuming closed loop.",
axis_name)
self._closed_loop[axis_name] = closed_loop
if 'encoder_resolution' in axis_par:
self._counts_per_meter[axis_name] = 1 / axis_par[
'encoder_resolution'] # approximately 5e-6 m / step
else:
self._counts_per_meter[axis_name] = DEFAULT_COUNTS_PER_METER
logging.info(
"Axis %s has no encoder resolution, assuming %s." %
(axis_name, 1 / DEFAULT_COUNTS_PER_METER))
if 'limit_type' in axis_par:
limit_type = axis_par['limit_type']
else:
logging.info("Axis %s has not limit switch." % axis_name)
limit_type = 0
if 'range' in axis_par:
axis_range = axis_par['range']
else:
axis_range = (0, STROKE_RANGE)
logging.info("Axis %s has no range. Assuming %s", axis_name,
axis_range)
if 'spc' in axis_par:
self._steps_per_count[axis_name] = axis_par[
'spc'] # approximately 5e-6 m / step
else:
logging.info(
"Axis %s has no spc parameter, will use value from flash."
% axis_name)
# None for now, will read value from flash later.
self._steps_per_count[axis_name] = None
if 'speed' in axis_par:
self._speed = axis_par['speed']
else:
self._speed = DEFAULT_AXIS_SPEED
logging.info(
"Axis %s was not given a speed value. Assuming %s",
axis_name, self._speed)
if 'unit' in axis_par:
axis_unit = axis_par['unit']
else:
axis_unit = "m"
logging.info("Axis %s has no unit. Assuming %s", axis_name,
axis_unit)
ad = model.Axis(canAbs=closed_loop,
unit=axis_unit,
range=axis_range)
axes_def[axis_name] = ad
Actuator.__init__(self,
name,
role,
axes=axes_def,
inverted=inverted,
**kwargs)
self._executor = CancellableThreadPoolExecutor(
max_workers=1) # one task at a time
self._ser_access = threading.RLock()
# Connect to hardware
self._port = None # port number
min_axis = min(self._axis_map.values())
self._serial = self._findDevice(port, min_axis)
self._recovering = False
# Get version
hwVersions = []
for ax_name, ax_num in self._axis_map.items():
ver = self.getVersion(ax_num)
sn = self.getSerialNumber(ax_num)
hwVersions.append("Axis %s ('%s') version: %s, " %
(ax_num, ax_name, ver) +
"serial number: %s" % sn)
self._hwVersion = ", ".join(hwVersions)
# Configuration
for axis in self._axis_map.values():
self.setWaveform(axis, WAVEFORM_DELTA)
driver_name = getSerialDriver(self._port)
self._swVersion = "Serial driver: %s" % (driver_name, )
# Position and referenced VAs
self.position = model.VigilantAttribute({}, unit="m", readonly=True)
self.referenced = model.VigilantAttribute({}, unit="m", readonly=True)
self._updatePosition()
for axname in self._axis_map.keys():
self.referenced.value[
axname] = False # just assume they haven't been referenced
# Load values from flash, write spc if provided, otherwise read spc
for axname, axis in self._axis_map.items():
# Load values from flash (most importantly spc parameter)
self.initFromFlash(axis)
if self._steps_per_count[axname]:
# Write SPC if provided
# Value that's written to register needs to be multiplied by (65536 * 4) (see manual)
self.writeParam(axis, 11,
self._steps_per_count[axname] * (65536 * 4))
else:
# Read spc from flash. If value is not reasonable, use default
val = int(self.readParam(axis, 11))
if not 20000 <= val <= 150000:
# that's not a reasonable value, the flash was probably not configured
logging.warning(
"Axis %s spc value not configured properly, current value: %s"
% (axis, val))
logging.info("Axis %s using spc value %s" %
(axis, DEFAULT_SPC))
val = DEFAULT_SPC
else:
val = val / (65536 * 4)
logging.info("Axis %s is using spc value from flash: %s" %
(axis, val))
self._steps_per_count[axname] = val
self._steps_per_meter[axname] = self._steps_per_count[
axname] * self._counts_per_meter[axname]
self._speed_steps[axis_name] = round(
self._speed * self._steps_per_meter[axis_name])
# Limit switch
for axis in self._axis_map.values():
self.setLimitType(axis, limit_type)
def terminate(self):
# terminate can be called several times, do nothing if ._serial is already None
if self._serial is None:
return
self._serial.close()
self._serial = None
for axis in self._axis_map.values():
self.setWaveform(axis, WAVEFORM_PARK) # power off
super(PMD401Bus, self).terminate()
def stop(self, axes=None):
self._executor.cancel()
axes = axes or self._axis_map.keys()
for ax in axes:
self.stopAxis(self._axis_map[ax])
def moveRel(self, shift):
if not shift:
return model.InstantaneousFuture()
self._checkMoveRel(shift)
shift = self._applyInversion(shift)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
def moveAbs(self, pos):
self._checkMoveAbs(pos)
pos = self._applyInversion(pos)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
def reference(self, axes):
self._checkReference(axes)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doReference, f, axes)
return f
def _doReference(self, f, axes):
self._check_hw_error()
# Request referencing on all axes
for axname in axes:
# If we're already at the index position or behind (index is at 8.9 µm from limit),
# referencing doesn't work.
# To avoid this, we first make a small move in the opposite direction (10 µm).
fmove = self._createMoveFuture()
self._doMoveRel(fmove, {axname: 1e-5})
self.startIndexMode(self._axis_map[axname])
self.moveToIndex(self._axis_map[axname])
# Wait until referencing is done
timeout = 2 * (abs(self.axes[axname].range[1] -
self.axes[axname].range[0]) / self._speed)
startt = time.time()
for axname in axes:
self.referenced._value[axname] = False
while not self.getIndexStatus(self._axis_map[axname])[-1]:
if f._must_stop.is_set():
return
if time.time() - startt > timeout:
self.stop() # exit index mode
raise ValueError("Referencing axis %s failed." %
self._axis_map[axname])
time.sleep(0.1)
logging.debug("Finished referencing axis %s." % axname)
self.stopAxis(self._axis_map[axname])
self.referenced._value[axname] = True
# read-only so manually notify
self.referenced.notify(self.referenced.value)
self._updatePosition()
def _doMoveAbs(self, f, pos):
self._check_hw_error()
targets = {}
for axname, val in pos.items():
if self._closed_loop[axname]:
encoder_cnts = round(val * self._counts_per_meter[axname])
self.runAbsTargetMove(self._axis_map[axname], encoder_cnts,
self._speed_steps[axname])
targets[axname] = val
else:
target = val - self.position.value[axname]
steps_float = target * self._steps_per_meter[axname]
steps = int(steps_float)
usteps = int((steps_float - steps) * USTEPS_PER_STEP)
self.runMotorJog(self._axis_map[axname], steps, usteps,
self._speed_steps[axname])
targets[axname] = None
self._waitEndMotion(f, targets)
# Leave target mode in case of closed-loop move
for ax in pos:
self.stopAxis(self._axis_map[ax])
self._updatePosition()
def _doMoveRel(self, f, shift):
self._check_hw_error()
targets = {}
self._updatePosition()
for axname, val in shift.items():
if self._closed_loop[axname]:
targets[axname] = self.position.value[axname] + val
encoder_cnts = val * self._counts_per_meter[axname]
self.runRelTargetMove(self._axis_map[axname], encoder_cnts)
else:
steps_float = val * self._steps_per_meter[axname]
steps = int(steps_float)
usteps = int((steps_float - steps) * USTEPS_PER_STEP)
self.runMotorJog(self._axis_map[axname], steps, usteps,
self._speed_steps[axname])
targets[axname] = None
self._waitEndMotion(f, targets)
# Leave target mode in case of closed-loop move
for ax in shift:
self.stopAxis(self._axis_map[ax])
self._updatePosition()
def _waitEndMotion(self, f, targets):
"""
Wait until move is done.
:param f: (CancellableFuture) move future
:param targets: (dict: str --> int) target (for closed-loop), None for open loop
"""
move_length = 0
self._updatePosition()
for ax, target in targets.items():
if target is not None:
move_length = max(abs(self.position.value[ax] - target),
move_length)
if move_length == 0: # open loop
move_length = STROKE_RANGE
dur = move_length / self._speed
max_dur = max(dur * 2, 0.1) # wait at least 0.1 s
logging.debug("Expecting a move of %g s, will wait up to %g s", dur,
max_dur)
for axname, target in targets.items():
axis = self._axis_map[axname]
moving = True
end_time = time.time() + max_dur
while moving:
if f._must_stop.is_set():
return
if time.time() > end_time:
raise IOError(
"Timeout while waiting for end of motion on axis %s" %
axis)
if self._closed_loop[axname]:
if target is None:
raise ValueError(
"No target provided for closed-loop move on axis %s."
% axis)
moving = self.isMovingClosedLoop(axis)
else:
moving = self.isMovingOpenLoop(axis)
self._check_hw_error()
time.sleep(0.05)
def _check_hw_error(self):
"""
Read hardware status and raise exception if error is detected.
"""
for ax, axnum in self._axis_map.items():
status = self.getStatus(axnum)
if status[0] & 8:
raise PMDError(
1,
"Communication Error (wrong baudrate, data collision, or buffer overflow)"
)
elif status[0] & 4:
raise PMDError(
2,
"Encoder error(serial communication or reported error from serial encoder)"
)
elif status[0] & 2:
raise PMDError(3,
"Supply voltage or motor fault was detected.")
elif status[0] & 1:
raise PMDError(
4,
"Command timeout occurred or a syntax error was detected when response was not "
"allowed.")
elif status[1] & 8:
raise PMDError(5, "Power-on/reset has occurred.")
def _updatePosition(self):
"""
Update the position VA.
"""
pos = {}
for axname, axis in self._axis_map.items():
pos[axname] = self.getEncoderPosition(axis)
logging.debug("Reporting new position at %s", pos)
pos = self._applyInversion(pos)
self.position._set_value(pos, force_write=True)
def stopAxis(self, axis):
self._sendCommand(b'X%dS' % axis)
def getVersion(self, axis):
"""
:param axis: (int) axis number
:returns (str): controller type and firmware version, e.g. 'PMD401 V13'
"""
return self._sendCommand(b'X%d?' % axis)
def getSerialNumber(self, axis):
"""
:param axis: (int) axis number
:returns (str): serial number
"""
return self._sendCommand(b'X%dY42' % axis)
def initFromFlash(self, axis):
"""
Initialize settings from values stored in flash.
:param axis: (int) axis number
"""
# 2 for init from flash, 3 for factory values
self._sendCommand(b'X%dY1,2' % axis)
def setLimitType(self, axis, limit_type):
"""
:param axis: (int) axis number
:param limit_type: (0 <= int <= 3) 0 no limit, 1 active high, 2 active low
"""
self._sendCommand(b'X%dY2,%d' % (axis, limit_type))
def getEncoderPosition(self, axis):
"""
:param axis: (int) axis number
:returns (float): current position of the axis as reported by encoders (in m)
"""
axname = [name for name, num in self._axis_map.items()
if num == axis][0] # get axis name from number
return int(self._sendCommand(
b'X%dE' % axis)) / self._counts_per_meter[axname]
def runRelTargetMove(self, axis, encoder_cnts):
"""
Closed-loop relative move.
:param axis: (int) axis number
:param encoder_cnts: (int)
"""
# There are two possibilities: move relative to current position (XC) and move relative to
# target position (XR). We are moving relative to the current position (might be more intuitive
# if something goes wrong and we're stuck in the wrong position).
self._sendCommand(b'X%dC%d,%d' %
(axis, encoder_cnts, self._speed_steps['x']))
def runMotorJog(self, axis, motor_steps, usteps, speed):
"""
Open loop stepping.
:param axis: (int) axis number
:param motor_steps: (int) number of motor steps to move
:param usteps: (int) number of microsteps (1 motor step = 8192 microsteps)
:param speed: (int) speed in steps / m
"""
self._sendCommand(b'X%dJ%d,%d,%d' % (axis, motor_steps, usteps, speed))
def runAbsTargetMove(self, axis, encoder_cnts, speed):
"""
Closed loop move.
:param axis: (int) axis number
:param encoder_cnts: (int)
:param speed: speed in motor steps per s
"""
self._sendCommand(b'X%dT%d,%d' % (axis, encoder_cnts, speed))
def setWaveform(self, axis, wf):
"""
:param wf: (WAVEFORM_RHOMB, WAVEFORM_DELTA, WAVEFORM_PARK) waveform to set
"""
if wf not in (WAVEFORM_DELTA, WAVEFORM_RHOMB, WAVEFORM_PARK):
raise ValueError("wf %s not a valid waveform" % wf)
self._sendCommand(b'X%dM%d' % (axis, wf))
def getStatus(self, axis):
"""
:returns (list of 4 int): 4-bit status code
The most important values are the following
First bit:
8: communication error (wrong baudrate, data collision, or buffer overflow)
4: encoder error (serial communication or reported error from serial encoder)
2: voltage error (supply voltage or motor fault was detected)
1: command error (command timeout occurred or a syntax error was detected when response was not allowed)
Second bit:
8: reset (power on/ reset occurred)
1: index (index signal was detected since last report)
For all codes, please refer to the PMD-401 manual.
"""
return [int(i) for i in self._sendCommand(b'X%dU0' % axis)]
def isMovingClosedLoop(self, axis):
"""
:param axis: (int) axis number
:param target: (float) target position for axes
:returns: (bool) True if moving, False otherwise
"""
# Check d3 (third status value) bit 2 (targetLimit) and bit 0 (targetReached)
_, _, d3, _ = self.getStatus(axis)
if d3 & 0b0101:
# target or limit reached
return False
else:
return True
def isMovingOpenLoop(self, axis):
"""
:param axis: (int) axis number
:returns: (bool) True if moving, False otherwise
"""
resp = self._sendCommand(
b'X%dJ' % axis
) # will be 0 if finished, otherwise +/-222 (contrary to manual!)
return int(resp) != 0
def startIndexMode(self, axis):
"""
Enters index mode.
"""
self._sendCommand(b'X%dN4' % axis)
def moveToIndex(self, axis):
"""
Move towards the index until it's found.
"""
axname = [name for name, num in self._axis_map.items()
if num == axis][0] # get axis name from number
# Move towards fixed end (negative direction)
maxdist = int(-self._axes[axname].range[1] *
self._counts_per_meter[axname] *
DEFAULT_SPC) # complete rodlength
self._sendCommand(b'X%dI%d,0,%d' %
(axis, maxdist, self._speed_steps[axname]))
def getIndexStatus(self, axis):
"""
Returns a description of the index status.
:returns (tuple of 4):
mode (0 or 1): index mode (1 if position has been reset at index)
position (float):
logged (bool): position was logged since last report
indexed (bool): position has been reset at index
"""
# Check if referencing was successful
# Response looks like this: "1,132.,indexed"
ret = self._sendCommand(b'X%dN?' % axis).split(',')
try:
mode = ret[0]
if mode == '1':
mode = 1
else: # empty string means mode 0
mode = 0
if ret[1][-1] == '.':
# . means position was logged since last report
logged = True
position = ret[1][:-1]
else:
logged = False
position = ret[1]
if len(ret) > 2 and 'indexed' in ret[2]:
indexed = True
else:
indexed = False
return mode, position, logged, indexed
except Exception as ex:
logging.exception("Failed to parse index status %s", ret)
raise
def setAxisAddress(self, current_address, new_address):
"""
Set the address of the axis. The factory default is 0 for all boards. Don't use this
command if multiple axes with the same number are connected.
:param current_address: (int) current axis number
:param new_address: (int) new axis number
"""
self._sendCommand(b"X%dY40,%d" % (current_address, new_address))
def runAutoConf(self, axis):
"""
Runs automatic configuration for the encoder parameters.
:param axis: (int) axis number
"""
self._sendCommand(b"X%dY25,1" % axis)
def writeParamsToFlash(self, axis):
self._sendCommand(b"X%dY32" % axis)
def setParam(self, axis, param, value):
self._sendCommand(b"X%dY%d,%d" % (axis, param, value))
def readParam(self, axis, param):
"""
:returns (str): parameter value from device
"""
return self._sendCommand(b"X%dY%d" % (axis, param))
def _createMoveFuture(self):
"""
:returns: (CancellableFuture) a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event(
) # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set(
) # tell the thread taking care of the move it's over
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
def _sendCommand(self, cmd):
"""
:param cmd: (bytes) command to be sent to the hardware
:returns: (str) response
"""
cmd += EOL
with self._ser_access:
logging.debug("Sending command %s", to_str_escape(cmd))
self._serial.write(cmd)
resp = b""
while resp[-len(EOL):] != EOL:
try:
char = self._serial.read()
except IOError:
logging.warning(
"Failed to read from PMT Control firmware, "
"trying to reconnect.")
if self._recovering:
raise
else:
self._tryRecover()
# don't send command again
raise IOError(
"Failed to read from PMT Control firmware, "
"restarted serial connection.")
if not char:
raise IOError("Timeout after receiving %s" %
to_str_escape(resp))
else:
resp += char
logging.debug("Received response %s", to_str_escape(resp))
# Check response (command should be echoed back)
if not resp.startswith(cmd[:-len(EOL) - 1]):
raise IOError("Response starts with %s != %s" %
(resp[:len(cmd)], cmd))
if b"_??_" in resp:
raise ValueError(
"Received response %s, command %s not understood." %
(resp, cmd))
if b"!" in resp:
raise PMDError(0, resp)
# Format:
# * for query with response: <cmd>:<ret><EOL> (will return <ret>)
# * for set command without response: <cmd><EOL> (will return "")
return resp[len(cmd) + 1 - len(EOL):-len(EOL)].decode("latin1")
def _tryRecover(self):
self._recovering = True
self.state._set_value(HwError("Connection lost, reconnecting..."),
force_write=True)
# Retry to open the serial port (in case it was unplugged)
# _ser_access should already be acquired, but since it's an RLock it can be acquired
# again in the same thread
try:
with self._ser_access:
while True:
try:
if self._serial:
self._serial.close()
self._serial = None
except Exception:
pass
try:
logging.debug("Searching for the device on port %s",
self._portpattern)
min_axis = min(self._axis_map.values())
self._port = self._findDevice(self._portpattern,
min_axis)
except IOError:
time.sleep(2)
except Exception:
logging.exception(
"Unexpected error while trying to recover device")
raise
else:
# We found it back!
break
# it now should be accessible again
self.state._set_value(model.ST_RUNNING, force_write=True)
logging.info("Recovered device on port %s", self._port)
finally:
self._recovering = False
def _findDevice(self, port, address=0):
"""
Look for a compatible device
port (str): pattern for the port name
address (None or int): the address of the stage controller
return (serial, int): the (opened) serial port used, and the actual address
raises:
IOError: if no device are found
"""
if port.startswith("/dev/fake"):
names = [port]
elif os.name == "nt":
raise NotImplementedError("Windows not supported")
else:
names = glob.glob(port)
for n in names:
try:
serial = self._openSerialPort(n)
except IOError as ex:
# not possible to use this port? next one!
logging.info("Skipping port %s, which is not available (%s)",
n, ex)
continue
# check whether it answers with the right address
try:
# If any garbage was previously received, make it discarded.
self._serial = serial
self._serial.flush()
if 'PMD401 ' in self.getVersion(address):
self._port = n
return serial # found it!
except Exception as ex:
logging.debug(
"Port %s doesn't seem to have a PMD device connected: %s",
n, ex)
serial.close() # make sure to close/unlock that port
else:
raise IOError(
"Failed to find a PMD controller with adress %s on ports '%s'. "
"Check that the device is turned on and "
"connected to the computer." % (
address,
port,
))
@staticmethod
def _openSerialPort(port):
"""
Opens the given serial port the right way for a PiezoMotor PMD device.
port (string): the name of the serial port (e.g., /dev/ttyUSB0)
return (serial): the opened serial port
raise HwError: if the serial port cannot be opened (doesn't exist, or
already opened)
"""
# For debugging purpose
if port == "/dev/fake":
return PMDSimulator(timeout=0.1)
try:
ser = serial.Serial(
port=port,
baudrate=BAUDRATE,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=0.3, # s
)
except IOError:
raise HwError("Failed to find a PMD controller on port '%s'. "
"Check that the device is turned on and "
"connected to the computer." % (port, ))
return ser
class Stage(model.Actuator):
"""
This is an extension of the model.Actuator class. It provides functions for
moving the Zeiss stage and updating the position.
"""
def __init__(self, name, role, parent, rng=None, **kwargs):
"""
inverted (set of str): names of the axes which are inverted
rng (dict str -> (float,float)): axis name -> min/max of the position on
this axis. Note: if the axis is inverted, the range passed will be
inverted. Also, if the hardware reports position outside of the range,
move might fail, as it is considered outside of the range.
"""
if rng is None:
rng = {}
if "x" not in rng:
rng["x"] = (5e-3, 152e-3)
if "y" not in rng:
rng["y"] = (5e-3, 152e-3)
if "z" not in rng:
rng["z"] = (5e-3, 40e-3)
axes_def = {
# Ranges are from the documentation
"x": model.Axis(unit="m", range=(rng["x"][0], rng["x"][1])),
"y": model.Axis(unit="m", range=(rng["y"][0], rng["y"][1])),
"z": model.Axis(unit="m", range=(rng["z"][0], rng["z"][1])),
}
model.Actuator.__init__(self, name, role, parent=parent, axes=axes_def, **kwargs)
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(max_workers=1) # one task at a time
# RO, as to modify it the client must use .moveRel() or .moveAbs()
self.position = model.VigilantAttribute({}, unit="m", readonly=True)
self._updatePosition()
# Refresh regularly the position
self._pos_poll = util.RepeatingTimer(5, self._refreshPosition, "Position polling")
self._pos_poll.start()
def _updatePosition(self, raw_pos=None):
"""
update the position VA
raw_pos (dict str -> float): the position in mm (as received from the SEM)
"""
if raw_pos is None:
x, y, z, _ = self.parent.GetStagePosition()
else:
x, y, z = raw_pos["x"], raw_pos["y"], raw_pos["z"]
pos = {"x": x * 1e-3,
"y": y * 1e-3,
"z": z * 1e-3,
}
self.position._set_value(self._applyInversion(pos), force_write=True)
def _refreshPosition(self):
"""
Called regularly to update the current position
"""
# We don't use the VA setters, to avoid sending back to the hardware a
# set request
logging.debug("Updating SEM stage position")
try:
self._updatePosition()
except Exception:
logging.exception("Unexpected failure when updating position")
def _doMoveRel(self, future, shift):
"""
move by the shift
shift (float): unit m
"""
x, y, z, _ = self.parent.GetStagePosition()
if "x" in shift:
x += shift["x"] * 1e3
if "y" in shift:
y += shift["y"] * 1e3
if "z" in shift:
z += shift["z"] * 1e3
target_pos = self._applyInversion({"x": x * 1e-3, "y": y * 1e-3, "z": z * 1e-3})
# Check range (for the axes we are moving)
for an in shift.keys():
rng = self.axes[an].range
p = target_pos[an]
if not rng[0] <= p <= rng[1]:
raise ValueError("Relative move would cause axis %s out of bound (%g m)" % (an, p))
self._moveTo(future, x, y, z)
def _doMoveAbs(self, future, pos):
"""
move to position pos
pos (float): unit m
"""
# Don't change position for unspecified coordinates
x, y, z, _ = self.parent.GetStagePosition()
# Convert to mm
if "x" in pos:
x = pos["x"] * 1e3
if "y" in pos:
y = pos["y"] * 1e3
if "z" in pos:
z = pos["z"] * 1e3
self._moveTo(future, x, y, z)
def _moveTo(self, future, x, y, z, timeout=60):
with future._moving_lock:
try:
if future._must_stop.is_set():
raise CancelledError()
logging.debug("Moving to position (%s, %s, %s)", x, y, z)
self.parent.MoveStage(x, y, z)
# documentation suggests to wait 1s before calling
# GetStagePosition() after MoveStage()
time.sleep(1)
# Wait until the move is over
# Don't check for future._must_stop because anyway the stage will
# stop moving, and so it's nice to wait until we know the stage is
# not moving.
moving = True
tstart = time.time()
while moving:
x, y, z, moving = self.parent.GetStagePosition()
# Take the opportunity to update .position
self._updatePosition({"x": x, "y": y, "z": z})
if time.time() > tstart + timeout:
self.parent.Abort()
logging.error("Timeout after submitting stage move. Aborting move.")
break
# 50 ms is about the time it takes to read the stage status
time.sleep(50e-3)
# If it was cancelled, Abort() has stopped the stage before, and
# we still have waited until the stage stopped moving. Now let
# know the user that the move is not complete.
if future._must_stop.is_set():
raise CancelledError()
except RemconError:
if future._must_stop.is_set():
raise CancelledError()
raise
finally:
future._was_stopped = True
# Update the position, even if the move didn't entirely succeed
self._updatePosition()
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set() # tell the thread taking care of the move it's over
self.parent.Abort()
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
def _createFuture(self):
"""
Return (CancellableFuture): a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event() # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
@isasync
def moveRel(self, shift):
"""
shift (dict): shift in m
"""
if not shift:
return model.InstantaneousFuture()
self._checkMoveRel(shift)
shift = self._applyInversion(shift)
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
@isasync
def moveAbs(self, pos):
"""
pos (dict): position in m
"""
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
pos = self._applyInversion(pos)
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
def stop(self, axes=None):
# Empty the queue (and already stop the stage if a future is running)
self._executor.cancel()
# Try to stop the stage, even if no future is running, for safety
logging.warning("Stopping all axes: %s", ", ".join(self.axes))
self.parent.Abort()
try:
self._updatePosition()
except Exception:
logging.exception("Unexpected failure when updating position")
class AntiBacklashActuator(model.Actuator):
"""
This is a stage wrapper that takes a stage and ensures that every move
always finishes in the same direction.
"""
def __init__(self, name, role, children, backlash, **kwargs):
"""
children (dict str -> Stage): dict containing one component, the stage
to wrap
backlash (dict str -> float): for each axis of the stage, the additional
distance to move (and the direction). If an axis of the stage is not
present, then it’s the same as having 0 as backlash (=> no antibacklash
motion is performed for this axis)
"""
if len(children) != 1:
raise ValueError("AntiBacklashActuator needs 1 child")
for a, v in backlash.items():
if not isinstance(a, basestring):
raise ValueError("Backlash key must be a string but got '%s'" % (a,))
if not isinstance(v, numbers.Real):
raise ValueError("Backlash value of %s must be a number but got '%s'" % (a, v))
self._child = children.values()[0]
self._backlash = backlash
axes_def = {}
for an, ax in self._child.axes.items():
axes_def[an] = copy.deepcopy(ax)
axes_def[an].canUpdate = True
if an in backlash and hasattr(ax, "range"):
# Restrict the range to have some margin for the anti-backlash move
rng = ax.range
if rng[1] - rng[0] < abs(backlash[an]):
raise ValueError("Backlash of %g m is bigger than range %s" %
(backlash[an], rng))
if backlash[an] > 0:
axes_def[an].range = (rng[0] + backlash[an], rng[1])
else:
axes_def[an].range = (rng[0], rng[1] + backlash[an])
# Whether currently a backlash shift is applied on an axis
# If True, moving the axis by the backlash value would restore its expected position
# _shifted_lock must be taken before modifying this attribute
self._shifted = {a: False for a in axes_def.keys()}
self._shifted_lock = threading.Lock()
# look for axes in backlash not existing in the child
missing = set(backlash.keys()) - set(axes_def.keys())
if missing:
raise ValueError("Child actuator doesn't have the axes %s" % (missing,))
model.Actuator.__init__(self, name, role, axes=axes_def,
children=children, **kwargs)
# will take care of executing axis moves asynchronously
self._executor = CancellableThreadPoolExecutor(max_workers=1) # one task at a time
# Duplicate VAs which are just identical
# TODO: shall we "hide" the antibacklash move by not updating position
# while doing this move?
self.position = self._child.position
if model.hasVA(self._child, "referenced"):
self.referenced = self._child.referenced
if model.hasVA(self._child, "speed"):
self.speed = self._child.speed
def terminate(self):
if self._executor:
self.stop()
self._executor.shutdown()
self._executor = None
def _antiBacklashMove(self, axes):
"""
Moves back the axes to their official position by reverting the anti-backlash shift
axes (list of str): the axes to revert
"""
sub_backlash = {} # same as backlash but only contains the axes moved
with self._shifted_lock:
for a in axes:
if self._shifted[a]:
if a in self._backlash:
sub_backlash[a] = self._backlash[a]
self._shifted[a] = False
if sub_backlash:
logging.debug("Running anti-backlash move %s", sub_backlash)
self._child.moveRelSync(sub_backlash)
def _doMoveRel(self, future, shift):
# move with the backlash subtracted
sub_shift = {}
for a, v in shift.items():
if a not in self._backlash:
sub_shift[a] = v
else:
# optimisation: if move goes in the same direction as backlash
# correction, then no need to do the correction
# TODO: only do this if backlash correction has already been applied once?
if v * self._backlash[a] >= 0:
sub_shift[a] = v
else:
with self._shifted_lock:
if self._shifted[a]:
sub_shift[a] = v
else:
sub_shift[a] = v - self._backlash[a]
self._shifted[a] = True
# Do the backlash + move
axes = set(shift.keys())
if not any(self._shifted):
# a tiny bit faster as we don't sleep
self._child.moveRelSync(sub_shift)
else:
# some antibacklash move needed afterwards => update might be worthy
f = self._child.moveRel(sub_shift)
done = False
while not done:
try:
f.result(timeout=0.01)
except futures.TimeoutError:
pass # Keep waiting for end of move
else:
done = True
# Check if there is already a new move to do
nf = self._executor.get_next_future(future)
if nf is not None and axes <= nf._update_axes:
logging.debug("Ending move control early as next move is an update containing %s", axes)
return
# backlash move
self._antiBacklashMove(shift.keys())
def _doMoveAbs(self, future, pos):
sub_pos = {}
for a, v in pos.items():
if a not in self._backlash:
sub_pos[a] = v
else:
shift = v - self.position.value[a]
with self._shifted_lock:
if shift * self._backlash[a] >= 0:
sub_pos[a] = v
self._shifted[a] = False
else:
sub_pos[a] = v - self._backlash[a]
self._shifted[a] = True
# Do the backlash + move
axes = set(pos.keys())
if not any(self._shifted):
# a tiny bit faster as we don't sleep
self._child.moveAbsSync(sub_pos)
else: # some antibacklash move needed afterwards => update might be worthy
f = self._child.moveAbs(sub_pos)
done = False
while not done:
try:
f.result(timeout=0.01)
except futures.TimeoutError:
pass # Keep waiting for end of move
else:
done = True
# Check if there is already a new move to do
nf = self._executor.get_next_future(future)
if nf is not None and axes <= nf._update_axes:
logging.debug("Ending move control early as next move is an update containing %s", axes)
return
# anti-backlash move
self._antiBacklashMove(axes)
def _createFuture(self, axes, update):
"""
Return (CancellableFuture): a future that can be used to manage a move
axes (set of str): the axes that are moved
update (bool): if it's an update move
"""
# TODO: do this via the __init__ of subclass of Future?
f = CancellableFuture() # TODO: make it cancellable too
f._update_axes = set() # axes handled by the move, if update
if update:
# Check if all the axes support it
if all(self.axes[a].canUpdate for a in axes):
f._update_axes = axes
else:
logging.warning("Trying to do a update move on axes %s not supporting update", axes)
return f
@isasync
def moveRel(self, shift, update=False):
if not shift:
return model.InstantaneousFuture()
self._checkMoveRel(shift)
f = self._createFuture(set(shift.keys()), update)
return self._executor.submitf(f, self._doMoveRel, f, shift)
@isasync
def moveAbs(self, pos, update=False):
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
f = self._createFuture(set(pos.keys()), update)
return self._executor.submitf(f, self._doMoveAbs, f, pos)
def stop(self, axes=None):
self._child.stop(axes=axes)
@isasync
def reference(self, axes):
f = self._child.reference(axes)
return f
class Stage(model.Actuator):
"""
This is an extension of the model.Actuator class. It provides functions for
moving the TFS stage and updating the position.
"""
def __init__(self, name, role, parent, rng=None, **kwargs):
if rng is None:
rng = {}
stage_info = parent.stage_info()
if "x" not in rng:
rng["x"] = stage_info["range"]["x"]
if "y" not in rng:
rng["y"] = stage_info["range"]["y"]
if "z" not in rng:
rng["z"] = stage_info["range"]["z"]
axes_def = {
# Ranges are from the documentation
"x": model.Axis(unit="m", range=rng["x"]),
"y": model.Axis(unit="m", range=rng["y"]),
"z": model.Axis(unit="m", range=rng["z"]),
}
model.Actuator.__init__(self,
name,
role,
parent=parent,
axes=axes_def,
**kwargs)
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(
max_workers=1) # one task at a time
self.position = model.VigilantAttribute({},
unit=stage_info["unit"],
readonly=True)
self._updatePosition()
# Refresh regularly the position
self._pos_poll = util.RepeatingTimer(5, self._refreshPosition,
"Position polling")
self._pos_poll.start()
def _updatePosition(self, raw_pos=None):
"""
update the position VA
raw_pos (dict str -> float): the position in mm (as received from the SEM)
"""
if raw_pos is None:
position = self.parent.get_stage_position()
x, y, z = position["x"], position["y"], position["z"]
else:
x, y, z = raw_pos["x"], raw_pos["y"], raw_pos["z"]
pos = {
"x": x,
"y": y,
"z": z,
}
self.position._set_value(self._applyInversion(pos), force_write=True)
def _refreshPosition(self):
"""
Called regularly to update the current position
"""
# We don't use the VA setters, to avoid sending back to the hardware a
# set request
logging.debug("Updating SEM stage position")
try:
self._updatePosition()
except Exception:
logging.exception("Unexpected failure when updating position")
def _moveTo(self, future, pos, timeout=60):
with future._moving_lock:
try:
if future._must_stop.is_set():
raise CancelledError()
logging.debug("Moving to position {}".format(pos))
self.parent.move_stage(pos, rel=False)
time.sleep(0.5)
# Wait until the move is over.
# Don't check for future._must_stop because anyway the stage will
# stop moving, and so it's nice to wait until we know the stage is
# not moving.
moving = True
tstart = time.time()
while moving:
pos = self.parent.get_stage_position()
moving = self.parent.stage_is_moving()
# Take the opportunity to update .position
self._updatePosition(pos)
if time.time() > tstart + timeout:
self.parent.stop_stage_movement()
logging.error(
"Timeout after submitting stage move. Aborting move."
)
break
# Wait for 50ms so that we do not keep using the CPU all the time.
time.sleep(50e-3)
# If it was cancelled, Abort() has stopped the stage before, and
# we still have waited until the stage stopped moving. Now let
# know the user that the move is not complete.
if future._must_stop.is_set():
raise CancelledError()
except Exception:
if future._must_stop.is_set():
raise CancelledError()
raise
finally:
future._was_stopped = True
# Update the position, even if the move didn't entirely succeed
self._updatePosition()
def _doMoveRel(self, future, shift):
pos = self.parent.get_stage_position()
for k, v in shift.items():
pos[k] += v
target_pos = self._applyInversion(pos)
# Check range (for the axes we are moving)
for an in shift.keys():
rng = self.axes[an].range
p = target_pos[an]
if not rng[0] <= p <= rng[1]:
raise ValueError(
"Relative move would cause axis %s out of bound (%g m)" %
(an, p))
self._moveTo(future, pos)
@isasync
def moveRel(self, shift):
"""
Shift the stage the given position in meters. This is non-blocking.
Throws an error when the requested position is out of range.
Parameters
----------
shift: dict(string->float)
Relative shift to move the stage to per axes in m. Axes are 'x' and 'y'.
"""
if not shift:
return model.InstantaneousFuture()
self._checkMoveRel(shift)
shift = self._applyInversion(shift)
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
def _doMoveAbs(self, future, pos):
self._moveTo(future, pos)
@isasync
def moveAbs(self, pos):
"""
Move the stage the given position in meters. This is non-blocking.
Throws an error when the requested position is out of range.
Parameters
----------
pos: dict(string->float)
Absolute position to move the stage to per axes in m. Axes are 'x' and 'y'.
"""
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
pos = self._applyInversion(pos)
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
def stop(self, axes=None):
"""Stop the movement of the stage."""
self._executor.cancel()
self.parent.stop_stage_movement()
try:
self._updatePosition()
except Exception:
logging.exception("Unexpected failure when updating position")
def _createFuture(self):
"""
Return (CancellableFuture): a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event(
) # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set(
) # tell the thread taking care of the move it's over
self.parent.stop_stage_movement()
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
class Chamber(model.Actuator):
"""
Simulated chamber component. Just pretends to be able to change pressure
"""
def __init__(self, name, role, positions, has_pressure=True, **kwargs):
"""
Initialises the component
positions (list of str): each pressure positions supported by the
component (among the allowed ones)
has_pressure (boolean): if True, has a pressure VA with the current
pressure.
"""
# TODO: or just provide .targetPressure (like .targetTemperature) ?
# Or maybe provide .targetPosition: position that would be reached if
# all the requested move were instantly applied?
chp = {}
for p in positions:
try:
chp[PRESSURES[p]] = p
except KeyError:
raise ValueError("Pressure position %s is unknown" % (p,))
axes = {"pressure": model.Axis(unit="Pa", choices=chp)}
model.Actuator.__init__(self, name, role, axes=axes, **kwargs)
# For simulating moves
self._position = PRESSURE_VENTED # last official position
self._goal = PRESSURE_VENTED
self._time_goal = 0 # time the goal was/will be reached
self._time_start = 0 # time the move started
# RO, as to modify it the client must use .moveRel() or .moveAbs()
self.position = model.VigilantAttribute(
{"pressure": self._position},
unit="Pa", readonly=True)
if has_pressure:
# Almost the same as position, but gives the current position
self.pressure = model.VigilantAttribute(self._position,
unit="Pa", readonly=True)
self._press_timer = util.RepeatingTimer(1, self._updatePressure,
"Simulated pressure update")
self._press_timer.start()
else:
self._press_timer = None
# Indicates whether the chamber is opened or not
# Just pretend it's always closed, and allow the user to change that
# for instance via CLI.
self.opened = model.BooleanVA(False)
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(max_workers=1) # one task at a time
def terminate(self):
if self._press_timer:
self._press_timer.cancel()
self._press_timer = None
if self._executor:
self.stop()
self._executor.shutdown()
self._executor = None
def _updatePressure(self):
"""
update the pressure VA (called regularly from a thread)
"""
# Compute the current pressure
now = time.time()
if self._time_goal < now: # done
# goal ±5%
pos = self._goal * random.uniform(0.95, 1.05)
else:
# TODO make it logarithmic
ratio = (now - self._time_start) / (self._time_goal - self._time_start)
pos = self._position + (self._goal - self._position) * ratio
# it's read-only, so we change it via _value
self.pressure._value = pos
self.pressure.notify(pos)
def _updatePosition(self):
"""
update the position VA
"""
# .position contains the last known/valid position
# it's read-only, so we change it via _value
self.position._value = {"pressure": self._position}
self.position.notify(self.position.value)
@isasync
def moveRel(self, shift):
self._checkMoveRel(shift)
# convert into an absolute move
pos = {}
for a, v in shift.items:
pos[a] = self.position.value[a] + v
return self.moveAbs(pos)
@isasync
def moveAbs(self, pos):
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
new_pres = pos["pressure"]
est_start = time.time() + 0.1
f = model.ProgressiveFuture(start=est_start,
end=est_start + self._getDuration(new_pres))
return self._executor.submitf(f, self._changePressure, f, new_pres)
def _getDuration(self, pos):
return abs(self._position - pos) / SPEED_PUMP
def _changePressure(self, f, p):
"""
Synchronous change of the pressure
p (float): target pressure
"""
# TODO: allow to cancel during the change
now = time.time()
duration = self._getDuration(p) # s
self._time_start = now
self._time_goal = now + duration # s
self._goal = p
time.sleep(duration / 2)
# DEBUG: for testing wrong time estimation
# f.set_progress(start=self._time_start, end=self._time_goal + 10)
time.sleep(duration / 2)
self._position = p
self._updatePosition()
def stop(self, axes=None):
self._executor.cancel()
logging.warning("Stopped pressure change")
class TMCM3110(model.Actuator):
"""
Represents one Trinamic TMCM-3110 controller.
Note: it must be set to binary communication mode (that's the default).
"""
def __init__(self, name, role, port, axes, ustepsize, refproc=None, temp=False, **kwargs):
"""
port (str): port name (use /dev/fake for a simulator)
axes (list of str): names of the axes, from the 1st to the 3rd.
ustepsize (list of float): size of a microstep in m (the smaller, the
bigger will be a move for a given distance in m)
refproc (str or None): referencing (aka homing) procedure type. Use
None to indicate it's not possible (no reference/limit switch) or the
name of the procedure. For now only "2xFinalForward" is accepted.
temp (bool): if True, will read the temperature from the analogue input
(10 mV <-> 1 °C)
inverted (set of str): names of the axes which are inverted (IOW, either
empty or the name of the axis)
"""
if len(axes) != 3:
raise ValueError("Axes must be a list of 3 axis names (got %s)" % (axes,))
self._axes_names = axes # axes names in order
if len(axes) != len(ustepsize):
raise ValueError("Expecting %d ustepsize (got %s)" %
(len(axes), ustepsize))
if refproc not in {REFPROC_2XFF, REFPROC_FAKE, None}:
raise ValueError("Reference procedure %s unknown" % (refproc, ))
self._refproc = refproc
for sz in ustepsize:
if sz > 10e-3: # sz is typically ~1µm, so > 1 cm is very fishy
raise ValueError("ustepsize should be in meter, but got %g" % (sz,))
self._ustepsize = ustepsize
try:
self._serial = self._openSerialPort(port)
except serial.SerialException:
raise HwError("Failed to find device %s on port %s. Ensure it is "
"connected to the computer." % (name, port))
self._port = port
self._ser_access = threading.Lock()
self._target = 1 # Always one, when directly connected via USB
self._resynchonise()
modl, vmaj, vmin = self.GetVersion()
if modl != 3110:
logging.warning("Controller TMCM-%d is not supported, will try anyway",
modl)
if name is None and role is None: # For scan only
return
if port != "/dev/fake": # TODO: support programs in simulator
# Detect if it is "USB bus powered" by using the fact that programs
# don't run when USB bus powered
addr = 80 # big enough to not overlap with REFPROC_2XFF programs
prog = [(9, 50, 2, 1), # Set global param 50 to 1
(28,), # STOP
]
self.UploadProgram(prog, addr)
if not self._isFullyPowered():
# Only a warning, at the power can be connected afterwards
logging.warning("Device %s has no power, the motor will not move", name)
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(max_workers=1) # one task at a time
axes_def = {}
for n, sz in zip(self._axes_names, self._ustepsize):
# Mov abs supports ±2³¹ but the actual position is only within ±2²³
rng = [(-2 ** 23) * sz, (2 ** 23 - 1) * sz]
# Probably not that much, but there is no info unless the axis has
# limit switches and we run a referencing
axes_def[n] = model.Axis(range=rng, unit="m")
model.Actuator.__init__(self, name, role, axes=axes_def, **kwargs)
for i, a in enumerate(self._axes_names):
self._init_axis(i)
driver_name = driver.getSerialDriver(self._port)
self._swVersion = "%s (serial driver: %s)" % (odemis.__version__, driver_name)
self._hwVersion = "TMCM-%d (firmware %d.%02d)" % (modl, vmaj, vmin)
self.position = model.VigilantAttribute({}, unit="m", readonly=True)
self._updatePosition()
# TODO: add support for changing speed. cf p.68: axis param 4 + p.81 + TMC 429 p.6
self.speed = model.VigilantAttribute({}, unit="m/s", readonly=True)
self._updateSpeed()
if refproc is not None:
# str -> boolean. Indicates whether an axis has already been referenced
axes_ref = dict([(a, False) for a in axes])
self.referenced = model.VigilantAttribute(axes_ref, readonly=True)
if temp:
# One sensor is at the top, one at the bottom of the sample holder.
# The most interesting is the temperature difference, so just
# report both.
self.temperature = model.FloatVA(0, unit=u"°C", readonly=True)
self.temperature1 = model.FloatVA(0, unit=u"°C", readonly=True)
self._temp_timer = util.RepeatingTimer(10, self._updateTemperatureVA,
"TMCM temperature update")
self._updateTemperatureVA() # make sure the temperature is correct
self._temp_timer.start()
def terminate(self):
if self._executor:
self.stop()
self._executor.shutdown(wait=True)
self._executor = None
if hasattr(self, "_temp_timer"):
self._temp_timer.cancel()
del self._temp_timer
with self._ser_access:
if self._serial:
self._serial.close()
self._serial = None
def _init_axis(self, axis):
"""
Initialise the given axis with "good" values for our needs (Delphi)
axis (int): axis number
"""
self.SetAxisParam(axis, 4, 1398) # maximum velocity to 1398 == 2 mm/s
self.SetAxisParam(axis, 5, 7) # maximum acc to 7 == 20 mm/s2
self.SetAxisParam(axis, 140, 8) # number of usteps ==2^8 =256 per fullstep
self.SetAxisParam(axis, 6, 15) # maximum RMS-current to 15 == 15/255 x 2.8 = 165mA
self.SetAxisParam(axis, 7, 0) # standby current to 0
self.SetAxisParam(axis, 204, 100) # power off after 100 ms standstill
self.SetAxisParam(axis, 154, 0) # step divider to 0 ==2^0 ==1
self.SetAxisParam(axis, 153, 0) # acc divider to 0 ==2^0 ==1
self.SetAxisParam(axis, 163, 0) # chopper mode
self.SetAxisParam(axis, 162, 2) # Chopper blank time (1 = for low current applications)
self.SetAxisParam(axis, 167, 3) # Chopper off time (2 = minimum)
self.MoveRelPos(axis, 0) # activate parameter with dummy move
if self._refproc == REFPROC_2XFF:
# set up the programs needed for the referencing
# Interrupt: stop the referencing
# The original idea was to mark the current position as 0 ASAP, and then
# later on move back to there. Now, we just stop ASAP, and hope it
# takes always the same time to stop. This allows to read how far from
# a previous referencing position we were during the testing.
prog = [# (6, 1, axis), # GAP 1, Motid # read pos
# (35, 60 + axis, 2), # AGP 60, 2 # save pos to 2/60
# (32, 10 + axis, axis), # CCO 10, Motid // Save the current position # doesn't work??
# TODO: see if it's needed to do like in original procedure: set 0 ASAP
# (5, 1, axis, 0), # SAP 1, MotId, 0 // Set actual pos 0
(13, 1, axis), # RFS STOP, MotId // Stop the reference search
(38,), # RETI
]
addr = 50 + 10 * axis # at addr 50/60/70
self.UploadProgram(prog, addr)
# Program: start and wait for referencing
# It's independent enough that even if the controlling computer
# stops during the referencing the motor will always eventually stop.
timeout = 20 # s (it can take up to 20 s to reach the home as fast speed)
timeout_ticks = int(round(timeout * 100)) # 1 tick = 10 ms
gparam = 50 + axis
addr = 0 + 15 * axis # Max with 3 axes: ~40
prog = [(9, gparam, 2, 0), # Set global param to 0 (=running)
(13, 0, axis), # RFS START, MotId
(27, 4, axis, timeout_ticks), # WAIT RFS until timeout
(21, 8, 0, addr + 6), # JC ETO, to TIMEOUT (= +6)
(9, gparam, 2, 1), # Set global param to 1 (=all went fine)
(28,), # STOP
(13, 1, axis), # TIMEOUT: RFS STOP, Motid
(9, gparam, 2, 2), # Set global param to 2 (=RFS timed-out)
(28,), # STOP
]
self.UploadProgram(prog, addr)
# Communication functions
@staticmethod
def _instr_to_str(instr):
"""
instr (buffer of 9 bytes)
"""
target, n, typ, mot, val, chk = struct.unpack('>BBBBiB', instr)
s = "%d, %d, %d, %d, %d (%d)" % (target, n, typ, mot, val, chk)
return s
@staticmethod
def _reply_to_str(rep):
"""
rep (buffer of 9 bytes)
"""
ra, rt, status, rn, rval, chk = struct.unpack('>BBBBiB', rep)
s = "%d, %d, %d, %d, %d (%d)" % (ra, rt, status, rn, rval, chk)
return s
def _resynchonise(self):
"""
Ensures the device communication is "synchronised"
"""
with self._ser_access:
self._serial.flushInput()
garbage = self._serial.read(1000)
if garbage:
logging.debug("Received unexpected bytes '%s'", garbage)
if len(garbage) == 1000:
# Probably a sign that it's not the device we are expecting
logging.warning("Lots of garbage sent from device")
# In case the device has received some data before, resynchronise by
# sending one byte at a time until we receive a reply.
# On Ubuntu, when plugging the device, udev automatically checks
# whether this is a real modem, which messes up everything immediately.
# As there is no command 0, either we will receive a "wrong command" or
# a "wrong checksum", but it's unlikely to ever do anything more.
for i in range(9): # a message is 9 bytes
self._serial.write(b"\x00")
self._serial.flush()
res = self._serial.read(9)
if len(res) == 9:
break # just got synchronised
elif len(res) == 0:
continue
else:
logging.error("Device not answering with a 9 bytes reply: %s", res)
else:
logging.error("Device not answering to a 9 bytes message")
# TODO: finish this method and use where possible
def SendInstructionRecoverable(self, n, typ=0, mot=0, val=0):
try:
self.SendInstruction(n, typ, mot, val)
except IOError:
# TODO: could serial.outWaiting() give a clue on what is going on?
# One possible reason is that the device disappeared because the
# cable was pulled out, or the power got cut (unlikely, as it's
# powered via 2 sources).
# TODO: detect that the connection was lost if the port we have
# leads to nowhere. => It seems os.path.exists should fail ?
# or /proc/pid/fd/n link to a *(deleted)
# How to handle the fact it will then probably get a different name
# on replug? Use a pattern for the file name?
self._resynchonise()
def SendInstruction(self, n, typ=0, mot=0, val=0):
"""
Sends one instruction, and return the reply.
n (0<=int<=255): instruction ID
typ (0<=int<=255): instruction type
mot (0<=int<=255): motor/bank number
val (0<=int<2**32): value to send
return (0<=int<2**32): value of the reply (if status is good)
raises:
IOError: if problem with sending/receiving data over the serial port
TMCLError: if status if bad
"""
msg = numpy.empty(9, dtype=numpy.uint8)
struct.pack_into('>BBBBiB', msg, 0, self._target, n, typ, mot, val, 0)
# compute the checksum (just the sum of all the bytes)
msg[-1] = numpy.sum(msg[:-1], dtype=numpy.uint8)
with self._ser_access:
logging.debug("Sending %s", self._instr_to_str(msg))
self._serial.write(msg)
self._serial.flush()
while True:
res = self._serial.read(9)
if len(res) < 9: # TODO: TimeoutError?
raise IOError("Received only %d bytes after %s" %
(len(res), self._instr_to_str(msg)))
logging.debug("Received %s", self._reply_to_str(res))
ra, rt, status, rn, rval, chk = struct.unpack('>BBBBiB', res)
# Check it's a valid message
npres = numpy.frombuffer(res, dtype=numpy.uint8)
good_chk = numpy.sum(npres[:-1], dtype=numpy.uint8)
if chk == good_chk:
if rt != self._target:
logging.warning("Received a message from %d while expected %d",
rt, self._target)
if rn != n:
logging.info("Skipping a message about instruction %d (waiting for %d)",
rn, n)
continue
if not status in TMCL_OK_STATUS:
raise TMCLError(status, rval, self._instr_to_str(msg))
else:
# TODO: investigate more why once in a while (~1/1000 msg)
# the message is garbled
logging.warning("Message checksum incorrect (%d), will assume it's all fine", chk)
return rval
# Low level functions
def GetVersion(self):
"""
return (int, int, int):
Controller ID: 3110 for the TMCM-3110
Firmware major version number
Firmware minor version number
"""
val = self.SendInstruction(136, 1) # Ask for binary reply
cont = val >> 16
vmaj, vmin = (val & 0xff00) >> 8, (val & 0xff)
return cont, vmaj, vmin
def GetAxisParam(self, axis, param):
"""
Read the axis/parameter setting from the RAM
axis (0<=int<=2): axis number
param (0<=int<=255): parameter number
return (0<=int): the value stored for the given axis/parameter
"""
val = self.SendInstruction(6, param, axis)
return val
def SetAxisParam(self, axis, param, val):
"""
Write the axis/parameter setting from the RAM
axis (0<=int<=2): axis number
param (0<=int<=255): parameter number
val (int): the value to store
"""
self.SendInstruction(5, param, axis, val)
def GetGlobalParam(self, bank, param):
"""
Read the parameter setting from the RAM
bank (0<=int<=2): bank number
param (0<=int<=255): parameter number
return (0<=int): the value stored for the given bank/parameter
"""
val = self.SendInstruction(10, param, bank)
return val
def SetGlobalParam(self, bank, param, val):
"""
Write the parameter setting from the RAM
bank (0<=int<=2): bank number
param (0<=int<=255): parameter number
val (int): the value to store
"""
self.SendInstruction(9, param, bank, val)
def GetIO(self, bank, port):
"""
Read the input/output value
bank (0<=int<=2): bank number
port (0<=int<=255): port number
return (0<=int): the value read from the given bank/port
"""
val = self.SendInstruction(15, port, bank)
return val
def GetCoordinate(self, axis, num):
"""
Read the axis/parameter setting from the RAM
axis (0<=int<=2): axis number
num (0<=int<=20): coordinate number
return (0<=int): the coordinate stored
"""
val = self.SendInstruction(30, num, axis)
return val
def MoveAbsPos(self, axis, pos):
"""
Requests a move to an absolute position. This is non-blocking.
axis (0<=int<=2): axis number
pos (-2**31 <= int 2*31-1): position
"""
self.SendInstruction(4, 0, axis, pos) # 0 = absolute
def MoveRelPos(self, axis, offset):
"""
Requests a move to a relative position. This is non-blocking.
axis (0<=int<=2): axis number
offset (-2**31 <= int 2*31-1): relative position
"""
self.SendInstruction(4, 1, axis, offset) # 1 = relative
# it returns the expected final absolute position
def MotorStop(self, axis):
self.SendInstruction(3, mot=axis)
def StartRefSearch(self, axis):
self.SendInstruction(13, 0, axis) # 0 = start
def StopRefSearch(self, axis):
self.SendInstruction(13, 1, axis) # 1 = stop
def GetStatusRefSearch(self, axis):
"""
return (bool): False if reference is not active, True if reference is active.
"""
val = self.SendInstruction(13, 2, axis) # 2 = status
return (val != 0)
def _isOnTarget(self, axis):
"""
return (bool): True if the target position is reached
"""
reached = self.GetAxisParam(axis, 8)
return (reached != 0)
def UploadProgram(self, prog, addr):
"""
Upload a program in memory
prog (sequence of tuples of 4 ints): list of the arguments for SendInstruction
addr (int): starting address of the program
"""
# cf TMCL reference p. 50
# http://pandrv.com/ttdg/phpBB3/viewtopic.php?f=13&t=992
# To download a TMCL program into a module, the following steps have to be performed:
# - Send the "enter download mode command" to the module (command 132 with value as address of the program)
# - Send your commands to the module as usual (status byte return 101)
# - Send the "exit download mode" command (command 133 with all 0)
# Each instruction is numbered +1, starting from 0
self.SendInstruction(132, val=addr)
for inst in prog:
# TODO: the controller sometimes fails to return the correct response
# when uploading a program... not sure why, but for now we hope it
# worked anyway.
try:
self.SendInstruction(*inst)
except IOError:
logging.warning("Controller returned wrong answer, but will assume it's fine")
self.SendInstruction(133)
def RunProgram(self, addr):
"""
Run the progam at the given address
addr (int): starting address of the program
"""
self.SendInstruction(129, typ=1, val=addr) # type 1 = use specified address
# To check the program runs (ie, it's not USB bus powered), you can
# check the program counter increases:
# assert self.GetGlobalParam(0, 130) > addr
def StopProgram(self):
"""
Stop a progam if any is running
"""
self.SendInstruction(128)
def SetInterrupt(self, id, addr):
"""
Associate an interrupt to run a program at the given address
id (int): interrupt number
addr (int): starting address of the program
"""
# Note: interrupts seem to only be executed when a program is running
self.SendInstruction(37, typ=id, val=addr)
def EnableInterrupt(self, id):
"""
Enable an interrupt
See global parameters to configure the interrupts
id (int): interrupt number
"""
self.SendInstruction(25, typ=id)
def DisableInterrupt(self, id):
"""
Disable an interrupt
See global parameters to configure the interrupts
id (int): interrupt number
"""
self.SendInstruction(26, typ=id)
def _setInputInterrupt(self, axis):
"""
Setup the input interrupt handler for stopping the reference search
axis (int): axis number
"""
addr = 50 + 10 * axis # at addr 50/60/70
intid = 40 + axis # axis 0 = IN1 = 40
self.SetInterrupt(intid, addr)
self.SetGlobalParam(3, intid, 3) # configure the interrupt: look at both edges
self.EnableInterrupt(intid)
self.EnableInterrupt(255) # globally switch on interrupt processing
def _isFullyPowered(self):
"""
return (boolean): True if the device is "self-powered" (meaning the
motors will be able to move) or False if the device is "USB bus powered"
(meaning it does answer to the computer, but nothing more).
"""
# We use a strange fact that programs will not run if the device is not
# self-powered.
gparam = 50
self.SetGlobalParam(2, gparam, 0)
self.RunProgram(80) # our stupid program address
time.sleep(0.01) # 10 ms should be more than enough to run one instruction
status = self.GetGlobalParam(2, gparam)
return (status == 1)
def _doInputReference(self, axis, speed):
"""
Run synchronously one reference search
axis (int): axis number
speed (int): speed in (funky) hw units for the move
return (bool): True if the search was done in the positive direction,
otherwise False
raise:
TimeoutError: if the search failed within a timeout (20s)
"""
timeout = 20 # s
# Set speed
self.SetAxisParam(axis, 194, speed) # maximum home velocity
self.SetAxisParam(axis, 195, speed) # maximum switching point velocity (useless for us)
# Set direction
edge = self.GetIO(0, 1 + axis) # IN1 = bank 0, port 1->3
logging.debug("Going to do reference search in dir %d", edge)
if edge == 1: # Edge is high, so we need to go positive dir
self.SetAxisParam(axis, 193, 7 + 128) # RFS with positive dir
else: # Edge is low => go negative dir
self.SetAxisParam(axis, 193, 8) # RFS with negative dir
gparam = 50 + axis
self.SetGlobalParam(2, gparam, 0)
# Run the basic program (we need one, otherwise interrupt handlers are
# not processed)
addr = 0 + 15 * axis
endt = time.time() + timeout + 2 # +2 s to let the program first timeout
self.RunProgram(addr)
# Wait until referenced
status = self.GetGlobalParam(2, gparam)
while status == 0:
time.sleep(0.01)
status = self.GetGlobalParam(2, gparam)
if time.time() > endt:
self.StopRefSearch(axis)
self.StopProgram()
self.MotorStop(axis)
raise IOError("Timeout during reference search from device")
if status == 2:
# if timed out raise
raise IOError("Timeout during reference search dir %d" % edge)
return (edge == 1)
# Special methods for referencing
def _startReferencing(self, axis):
"""
Do the referencing (this is synchronous). The current implementation
only supports one axis referencing at a time.
raise:
IOError: if timeout happen
"""
logging.info("Starting referencing of axis %d", axis)
if self._refproc == REFPROC_2XFF:
if not self._isFullyPowered():
raise IOError("Device is not powered, so motors cannot move")
# Procedure devised by NTS:
# It requires the ref signal to be active for half the length. Like:
# ___________________ 1
# |
# 0 ___________________|
# ----------------------------------------> forward
# It first checks on which side of the length the actuator is, and
# then goes towards the edge. If the movement was backward, then
# it does the search a second time forward, to increase the
# repeatability.
# All this is done twice, once a fast speed finishing with negative
# direction, then at slow speed to increase precision, finishing
# in positive direction. Note that as the fast speed finishes with
# negative direction, normally only one run (in positive direction)
# is required on slow speed.
# Note also that the reference signal is IN1-3, instead of the
# official "left/home switches". It seems the reason is that it was
# because when connecting a left switch, a right switch must also
# be connected, but that's very probably false. Because of that,
# we need to set an interrupt to stop the RFS command when the edge
# changes. As interrupts only work when a program is running, we
# have a small program that waits for the RFS and report the status.
# In conclusion, RFS is used pretty much just to move at a constant
# speed.
# Note also that it seem "negative/positive" direction of the RFS
# are opposite to the move relative negative/positive direction.
try:
self._setInputInterrupt(axis)
# TODO: be able to cancel (=> set a flag + call RFS STOP)
pos_dir = self._doInputReference(axis, 350) # fast (~0.5 mm/s)
if pos_dir: # always finish first by negative direction
self._doInputReference(axis, 350) # fast (~0.5 mm/s)
# Go back far enough that the slow referencing always need quite
# a bit of move. This is not part of the official NTS procedure
# but without that, the final reference position is affected by
# the original position.
self.MoveRelPos(axis, -20000) # ~ 100µm
for i in range(100):
time.sleep(0.01)
if self._isOnTarget(axis):
break
else:
logging.warning("Relative move failed to finish in time")
pos_dir = self._doInputReference(axis, 50) # slow (~0.07 mm/s)
if not pos_dir: # if it was done in negative direction (unlikely), redo
logging.debug("Doing one last reference move, in positive dir")
# As it always wait for the edge to change, the second time
# should be positive
pos_dir = self._doInputReference(axis, 50)
if not pos_dir:
logging.warning("Second reference search was again in negative direction")
finally:
# Disable interrupt
intid = 40 + axis # axis 0 = IN1 = 40
self.DisableInterrupt(intid)
# TODO: to support multiple axes referencing simultaneously,
# only this global interrupt would need to be handle globally
# (= only disable iff noone needs interrupt).
self.DisableInterrupt(255)
# For safety, but also necessary to make sure SetAxisParam() works
self.MotorStop(axis)
# Reset the absolute 0 (by setting current pos to 0)
logging.debug("Changing referencing position by %d", self.GetAxisParam(axis, 1))
self.SetAxisParam(axis, 1, 0)
elif self._refproc == REFPROC_FAKE:
logging.debug("Simulating referencing")
self.MotorStop(axis)
self.SetAxisParam(axis, 1, 0)
else:
raise NotImplementedError("Unknown referencing procedure %s" % self._refproc)
# high-level methods (interface)
def _updatePosition(self, axes=None):
"""
update the position VA
axes (set of str): names of the axes to update or None if all should be
updated
"""
if axes is None:
axes = self._axes_names
pos = self.position.value
for i, n in enumerate(self._axes_names):
if n in axes:
# param 1 = current position
pos[n] = self.GetAxisParam(i, 1) * self._ustepsize[i]
# it's read-only, so we change it via _value
self.position._value = pos
self.position.notify(self.position.value)
def _updateSpeed(self):
"""
Update the speed VA from the controller settings
"""
speed = {}
# As described in section 3.4.1:
# fCLK * velocity
# usf = ------------------------
# 2**pulse_div * 2048 * 32
for i, n in enumerate(self._axes_names):
velocity = self.GetAxisParam(i, 4)
pulse_div = self.GetAxisParam(i, 154)
# fCLK = 16 MHz
usf = (16e6 * velocity) / (2 ** pulse_div * 2048 * 32)
speed[n] = usf * self._ustepsize[i] # m/s
# it's read-only, so we change it via _value
self.speed._value = speed
self.speed.notify(self.speed.value)
def _updateTemperatureVA(self):
"""
Update the temperature VAs, assuming that the 2 analogue inputs are
connected to a temperature sensor with mapping 10 mV <-> 1 °C. That's
conveniently what is in the Delphi.
"""
try:
# The analogue port return 0..4095 -> 0..10 V
val = self.GetIO(1, 0) # 0 = first (analogue) port
v = val * 10 / 4095 # V
t0 = v / 10e-3 # °C
val = self.GetIO(1, 4) # 4 = second (analogue) port
v = val * 10 / 4095 # V
t1 = v / 10e-3 # °C
except Exception:
logging.exception("Failed to read the temperature")
return
logging.info("Temperature 0 = %g °C, temperature 1 = %g °C", t0, t1)
self.temperature._value = t0
self.temperature.notify(t0)
self.temperature1._value = t1
self.temperature1.notify(t1)
def _createFuture(self):
"""
Return (CancellableFuture): a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event() # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
@isasync
def moveRel(self, shift):
self._checkMoveRel(shift)
shift = self._applyInversionRel(shift)
# Check if the distance is big enough to make sense
for an, v in shift.items():
aid = self._axes_names.index(an)
if abs(v) < self._ustepsize[aid]:
# TODO: store and accumulate all the small moves instead of dropping them?
del shift[an]
logging.info("Dropped too small move of %f m", abs(v))
if not shift:
return model.InstantaneousFuture()
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
@isasync
def moveAbs(self, pos):
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
pos = self._applyInversionRel(pos)
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
moveAbs.__doc__ = model.Actuator.moveAbs.__doc__
@isasync
def reference(self, axes):
if not axes:
return model.InstantaneousFuture()
self._checkReference(axes)
f = self._executor.submit(self._doReference, axes)
return f
reference.__doc__ = model.Actuator.reference.__doc__
def stop(self, axes=None):
self._executor.cancel()
def _doMoveRel(self, future, pos):
"""
Blocking and cancellable relative move
future (Future): the future it handles
pos (dict str -> float): axis name -> relative target position
"""
with future._moving_lock:
end = 0 # expected end
moving_axes = set()
for an, v in pos.items():
aid = self._axes_names.index(an)
moving_axes.add(aid)
usteps = int(round(v / self._ustepsize[aid]))
self.MoveRelPos(aid, usteps)
# compute expected end
dur = abs(usteps) * self._ustepsize[aid] / self.speed.value[an]
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
logging.debug("move successfully completed")
def _doMoveAbs(self, future, pos):
"""
Blocking and cancellable absolute move
future (Future): the future it handles
pos (dict str -> float): axis name -> absolute target position
"""
with future._moving_lock:
end = 0 # expected end
old_pos = self.position.value
moving_axes = set()
for an, v in pos.items():
aid = self._axes_names.index(an)
moving_axes.add(aid)
usteps = int(round(v / self._ustepsize[aid]))
self.MoveAbsPos(aid, usteps)
# compute expected end
dur = abs(v - old_pos[an]) / self.speed.value[an]
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
logging.debug("move successfully completed")
def _waitEndMove(self, future, axes, end=0):
"""
Wait until all the given axes are finished moving, or a request to
stop has been received.
future (Future): the future it handles
axes (set of int): the axes IDs to check
end (float): expected end time
raise:
CancelledError: if cancelled before the end of the move
"""
moving_axes = set(axes)
last_upd = time.time()
last_axes = moving_axes.copy()
try:
while not future._must_stop.is_set():
for aid in moving_axes.copy(): # need copy to remove during iteration
if self._isOnTarget(aid):
moving_axes.discard(aid)
if not moving_axes:
# no more axes to wait for
return
# Update the position from time to time (10 Hz)
if time.time() - last_upd > 0.1 or last_axes != moving_axes:
last_names = set(self._axes_names[i] for i in last_axes)
self._updatePosition(last_names)
last_upd = time.time()
last_axes = moving_axes.copy()
# Wait half of the time left (maximum 0.1 s)
left = end - time.time()
sleept = max(0, min(left / 2, 0.1))
future._must_stop.wait(sleept)
logging.debug("Move of axes %s cancelled before the end", axes)
# stop all axes still moving them
for i in moving_axes:
self.MotorStop(i)
future._was_stopped = True
raise CancelledError()
finally:
self._updatePosition() # update (all axes) with final position
def _doReference(self, axes):
"""
Actually runs the referencing code
axes (set of str)
"""
# do the referencing for each axis
for a in axes:
aid = self._axes_names.index(a)
self._startReferencing(aid)
# TODO: handle cancellation
# If not cancelled and successful, update .referenced
# We only notify after updating the position so that when a listener
# receives updates both values are already updated.
for a in axes:
self.referenced._value[a] = True
self._updatePosition(axes) # all the referenced axes should be back to 0
# read-only so manually notify
self.referenced.notify(self.referenced.value)
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set() # tell the thread taking care of the move it's over
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
@staticmethod
def _openSerialPort(port):
"""
Opens the given serial port the right way for a Thorlabs APT device.
port (string): the name of the serial port (e.g., /dev/ttyUSB0)
return (serial): the opened serial port
"""
# For debugging purpose
if port == "/dev/fake":
return TMCM3110Simulator(timeout=0.1)
ser = serial.Serial(
port=port,
baudrate=9600, # TODO: can be changed by RS485 setting p.85?
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=0.1 # s
)
return ser
@classmethod
def scan(cls):
"""
returns (list of 2-tuple): name, args (sn)
Note: it's obviously not advised to call this function if a device is already under use
"""
# TODO: use serial.tools.list_ports.comports() (but only availabe in pySerial 2.6)
if os.name == "nt":
ports = ["COM" + str(n) for n in range (0, 8)]
else:
ports = glob.glob('/dev/ttyACM?*')
logging.info("Scanning for TMCM controllers in progress...")
found = [] # (list of 2-tuple): name, args (port, axes(channel -> CL?)
for p in ports:
try:
logging.debug("Trying port %s", p)
dev = cls(None, None, p, axes=["x", "y", "z"],
ustepsize=[10e-9, 10e-9, 10e-9])
modl, vmaj, vmin = dev.GetVersion()
# TODO: based on the model name (ie, the first number) deduce
# the number of axes
except (serial.SerialException, IOError):
# not possible to use this port? next one!
continue
except Exception:
logging.exception("Error while communicating with port %s", p)
continue
found.append(("TMCM-%s" % modl,
{"port": p,
"axes": ["x", "y", "z"],
"ustepsize": [10e-9, 10e-9, 10e-9]})
)
return found
class SmarPod(model.Actuator):
def __init__(self,
name,
role,
locator,
ref_on_init=False,
actuator_speed=0.1,
axes=None,
**kwargs):
"""
A driver for a SmarAct SmarPod Actuator.
This driver uses a DLL provided by SmarAct which connects via
USB or TCP/IP using a locator string.
name: (str)
role: (str)
locator: (str) Use "fake" for a simulator.
For a real device, MCS controllers with USB interface can be addressed with the
following locator syntax:
usb:id:<id>
where <id> is the first part of a USB devices serial number which
is printed on the MCS controller.
If the controller has a TCP/IP connection, use:
network:<ip>:<port>
ref_on_init: (bool) determines if the controller should automatically reference
on initialization
actuator_speed: (double) the default speed (in m/s) of the actuators
axes: dict str (axis name) -> dict (axis parameters)
axis parameters: {
range: [float, float], default is -1 -> 1
unit: (str) default will be set to 'm'
}
"""
if not axes:
raise ValueError("Needs at least 1 axis.")
if locator != "fake":
self.core = SmarPodDLL()
else:
self.core = FakeSmarPodDLL()
# Not to be mistaken with axes which is a simple public view
self._axis_map = {} # axis name -> axis number used by controller
axes_def = {} # axis name -> Axis object
self._locator = c_char_p(locator.encode("ascii"))
self._options = c_char_p("".encode(
"ascii")) # In the current version, this must be an empty string.
for axis_name, axis_par in axes.items():
try:
axis_range = axis_par['range']
except KeyError:
logging.info("Axis %s has no range. Assuming (-1, 1)",
axis_name)
axis_range = (-1, 1)
try:
axis_unit = axis_par['unit']
except KeyError:
logging.info("Axis %s has no unit. Assuming m", axis_name)
axis_unit = "m"
ad = model.Axis(canAbs=True, unit=axis_unit, range=axis_range)
axes_def[axis_name] = ad
# Connect to the device
self._id = c_uint()
self.core.Smarpod_Open(byref(self._id), SmarPodDLL.hwModel,
self._locator, self._options)
logging.debug("Successfully connected to SmarPod Controller ID %d",
self._id.value)
self.core.Smarpod_SetSensorMode(self._id,
SmarPodDLL.SMARPOD_SENSORS_ENABLED)
model.Actuator.__init__(self, name, role, axes=axes_def, **kwargs)
# Add metadata
self._swVersion = self.GetSwVersion()
self._metadata[model.MD_SW_VERSION] = self._swVersion
logging.debug("Using SmarPod library version %s", self._swVersion)
self.position = model.VigilantAttribute({}, readonly=True)
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(1) # one task at a time
referenced = c_int()
self.core.Smarpod_IsReferenced(self._id, byref(referenced))
# define the referenced VA from the query
axes_ref = {a: referenced.value for a, i in self.axes.items()}
# VA dict str(axis) -> bool
self.referenced = model.VigilantAttribute(axes_ref, readonly=True)
# If ref_on_init, referenced immediately.
if referenced.value:
logging.debug("SmarPod is referenced")
else:
logging.warning(
"SmarPod is not referenced. The device will not function until referencing occurs."
)
if ref_on_init:
self.reference().result()
# Use a default actuator speed
self.SetSpeed(actuator_speed)
self._speed = self.GetSpeed()
self._accel = self.GetAcceleration()
self._updatePosition()
def terminate(self):
# should be safe to close the device multiple times if terminate is called more than once.
self.core.Smarpod_Close(self._id)
super(SmarPod, self).terminate()
def GetSwVersion(self):
"""
Request the software version from the DLL file
"""
major = c_uint()
minor = c_uint()
update = c_uint()
self.core.Smarpod_GetDLLVersion(byref(major), byref(minor),
byref(update))
ver = "%u.%u.%u" % (major.value, minor.value, update.value)
return ver
def IsReferenced(self):
"""
Ask the controller if it is referenced
"""
referenced = c_int()
self.core.Smarpod_IsReferenced(self._id, byref(referenced))
return bool(referenced.value)
def GetMoveStatus(self):
"""
Gets the move status from the controller.
Returns:
SmarPodDLL.SMARPOD_MOVING is returned if moving
SmarPodDLL.SMARPOD_STOPPED when stopped
SmarPodDLL.SMARPOD_HOLDING when holding between moves
SmarPodDLL.SMARPOD_CALIBRATING when calibrating
SmarPodDLL.SMARPOD_REFERENCING when referencing
SmarPodDLL.SMARPOD_STANDBY
"""
status = c_uint()
self.core.Smarpod_GetMoveStatus(self._id, byref(status))
return status
def Move(self, pos, hold_time=0, block=False):
"""
Move to pose command.
pos: (dict str -> float) axis name -> position
This is converted to the pose C-struct which is sent to the SmarPod DLL
hold_time: (float) specify in seconds how long to hold after the move.
If set to float(inf), will hold forever until a stop command is issued.
block: (bool) Set to True if the function should block until the move completes
Raises: SmarPodError if a problem occurs
"""
# convert into a smartpad pose
newPose = dict_to_pose(pos)
if hold_time == float("inf"):
ht = SmarPodDLL.SMARPOD_HOLDTIME_INFINITE
else:
ht = c_uint(int(hold_time * 1000.0))
# Use an infiinite holdtime and non-blocking (final argument)
self.core.Smarpod_Move(self._id, byref(newPose), ht, c_int(block))
def GetPose(self):
"""
Get the current pose of the SmarPod
returns: (dict str -> float): axis name -> position
"""
pose = Pose()
self.core.Smarpod_GetPose(self._id, byref(pose))
position = pose_to_dict(pose)
logging.info("Current position: %s", position)
return position
def Stop(self):
"""
Stop command sent to the SmarPod
"""
logging.debug("Stopping...")
self.core.Smarpod_Stop(self._id)
def SetSpeed(self, value):
"""
Set the speed of the SmarPod motion
value: (double) indicating speed for all axes
"""
logging.debug("Setting speed to %f", value)
# the second argument (1) turns on speed control.
self.core.Smarpod_SetSpeed(self._id, c_int(1), c_double(value))
def GetSpeed(self):
"""
Returns (double) the speed of the SmarPod motion
"""
speed_control = c_int()
speed = c_double()
self.core.Smarpod_GetSpeed(self._id, byref(speed_control),
byref(speed))
return speed.value
def SetAcceleration(self, value):
"""
Set the acceleration of the SmarPod motion
value: (double) indicating acceleration for all axes
"""
logging.debug("Setting acceleration to %f", value)
# Passing 1 enables acceleration control.
self.core.Smarpod_SetAcceleration(self._id, c_int(1), c_double(value))
def GetAcceleration(self):
"""
Returns (double) the acceleration of the SmarPod motion
"""
acceleration_control = c_int()
acceleration = c_double()
self.core.Smarpod_GetAcceleration(self._id,
byref(acceleration_control),
byref(acceleration))
return acceleration.value
def IsPoseReachable(self, pos):
"""
Ask the controller if a pose is reachable
pos: (dict of str -> float): a coordinate dictionary of axis name to value
returns: true if the pose is reachable - false otherwise.
"""
reachable = c_int()
self.core.Smarpod_IsPoseReachable(self._id, byref(dict_to_pose(pos)),
byref(reachable))
return bool(reachable.value)
def stop(self, axes=None):
"""
Stop the SmarPod controller and update position
"""
self.Stop()
self._updatePosition()
def _updatePosition(self):
"""
update the position VA
"""
try:
p = self.GetPose()
except SmarPodError as ex:
if ex.errno == SmarPodDLL.SMARPOD_NOT_REFERENCED_ERROR:
logging.warning(
"Position unknown because SmarPod is not referenced")
p = {'x': 0, 'y': 0, 'z': 0, 'rx': 0, 'ry': 0, 'rz': 0}
else:
raise
p = self._applyInversion(p)
logging.debug("Updated position to %s", p)
self.position._set_value(p, force_write=True)
def _createMoveFuture(self, ref=False):
"""
ref: if true, will use a different canceller
Return (CancellableFuture): a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event(
) # cancel of the current future requested
f._was_stopped = False # if cancel was successful
if not ref:
f.task_canceller = self._cancelCurrentMove
else:
f.task_canceller = self._cancelReference
return f
@isasync
def reference(self, _=None):
"""
reference usually takes axes as an argument. However, the SmarPod references all
axes together so this argument is extraneous.
"""
f = self._createMoveFuture(ref=True)
self._executor.submitf(f, self._doReference, f)
return f
def _doReference(self, future):
"""
Actually runs the referencing code
future (Future): the future it handles
raise:
IOError: if referencing failed due to hardware
CancelledError if was cancelled
"""
# Reset reference so that if it fails, it states the axes are not
# referenced (anymore)
with future._moving_lock:
try:
# set the referencing for all axes to fals
self.referenced._value = {a: False for a in self.axes.keys()}
# The SmarPod references all axes at once. This function blocks
self.core.Smarpod_FindReferenceMarks(self._id)
if self.IsReferenced():
self.referenced._value = {
a: True
for a in self.axes.keys()
}
self._updatePosition()
logging.info("Referencing successful.")
except SmarPodError as ex:
future._was_stopped = True
# This occurs if a stop command interrupts referencing
if ex.errno == SmarPodDLL.SMARPOD_STOPPED_ERROR:
logging.info("Referencing stopped: %s", ex)
raise CancelledError()
else:
raise
except Exception:
logging.exception("Referencing failure")
raise
finally:
# We only notify after updating the position so that when a listener
# receives updates both values are already updated.
# read-only so manually notify
self.referenced.notify(self.referenced.value)
@isasync
def moveAbs(self, pos):
"""
API call to absolute move
"""
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
if not self.IsPoseReachable(pos):
raise ValueError(
"Pose %s is not reachable by the SmarPod controller" % (pos, ))
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
def _doMoveAbs(self, future, pos):
"""
Blocking and cancellable absolute move
future (Future): the future it handles
_pos (dict str -> float): axis name -> absolute target position
raise:
SmarPodError: if the controller reported an error
CancelledError: if cancelled before the end of the move
"""
last_upd = time.time()
dur = 30 # TODO: Calculate an estimated move duration
end = time.time() + dur
max_dur = dur * 2 + 1
logging.debug("Expecting a move of %g s, will wait up to %g s", dur,
max_dur)
timeout = last_upd + max_dur
with future._moving_lock:
self.Move(pos)
while not future._must_stop.is_set():
status = self.GetMoveStatus()
# check if move is done
if status.value == SmarPodDLL.SMARPOD_STOPPED.value:
break
now = time.time()
if now > timeout:
logging.warning("Stopping move due to timeout after %g s.",
max_dur)
self.stop()
raise TimeoutError("Move is not over after %g s, while "
"expected it takes only %g s" %
(max_dur, dur))
# Update the position from time to time (10 Hz)
if now - last_upd > 0.1:
self._updatePosition()
last_upd = time.time()
# Wait half of the time left (maximum 0.1 s)
left = end - time.time()
sleept = max(0.001, min(left / 2, 0.1))
future._must_stop.wait(sleept)
else:
self.stop()
future._was_stopped = True
raise CancelledError()
self._updatePosition()
logging.debug("move successfully completed")
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Canceling current move")
future._must_stop.set(
) # tell the thread taking care of the move it's over
with future._moving_lock:
if not future._was_stopped:
logging.debug("Canceling failed")
self._updatePosition()
return future._was_stopped
def _cancelReference(self, future):
# The difficulty is to synchronize correctly when:
# * the task is just starting (about to request axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Canceling current referencing")
self.Stop()
future._must_stop.set(
) # tell the thread taking care of the referencing it's over
# Synchronise with the ending of the future
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
@isasync
def moveRel(self, shift):
"""
API call for relative move
"""
if not shift:
return model.InstantaneousFuture()
self._checkMoveRel(shift)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
def _doMoveRel(self, future, shift):
"""
Do a relative move by converting it into an absolute move
"""
pos = add_coord(self.position.value, shift)
self._doMoveAbs(future, pos)
class Stage(model.Actuator):
"""
This is an extension of the model.Actuator class. It provides functions for
moving the Zeiss stage and updating the position.
"""
def __init__(self, name, role, parent, rng=None, **kwargs):
"""
inverted (set of str): names of the axes which are inverted
rng (dict str -> (float,float)): axis name -> min/max of the position on
this axis. Note: if the axis is inverted, the range passed will be
inverted. Also, if the hardware reports position outside of the range,
move might fail, as it is considered outside of the range.
"""
if rng is None:
rng = {}
if "x" not in rng:
rng["x"] = (5e-3, 152e-3)
if "y" not in rng:
rng["y"] = (5e-3, 152e-3)
if "z" not in rng:
rng["z"] = (5e-3, 40e-3)
axes_def = {
# Ranges are from the documentation
"x": model.Axis(unit="m", range=(rng["x"][0], rng["x"][1])),
"y": model.Axis(unit="m", range=(rng["y"][0], rng["y"][1])),
"z": model.Axis(unit="m", range=(rng["z"][0], rng["z"][1])),
}
model.Actuator.__init__(self, name, role, parent=parent, axes=axes_def, **kwargs)
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(max_workers=1) # one task at a time
# RO, as to modify it the client must use .moveRel() or .moveAbs()
self.position = model.VigilantAttribute({}, unit="m", readonly=True)
self._updatePosition()
# Refresh regularly the position
self._pos_poll = util.RepeatingTimer(5, self._refreshPosition, "Position polling")
self._pos_poll.start()
def _updatePosition(self, raw_pos=None):
"""
update the position VA
raw_pos (dict str -> float): the position in mm (as received from the SEM)
"""
if raw_pos is None:
x, y, z, _ = self.parent.GetStagePosition()
else:
x, y, z = raw_pos["x"], raw_pos["y"], raw_pos["z"]
pos = {"x": x * 1e-3,
"y": y * 1e-3,
"z": z * 1e-3,
}
self.position._set_value(self._applyInversion(pos), force_write=True)
def _refreshPosition(self):
"""
Called regularly to update the current position
"""
# We don't use the VA setters, to avoid sending back to the hardware a
# set request
logging.debug("Updating SEM stage position")
try:
self._updatePosition()
except Exception:
logging.exception("Unexpected failure when updating position")
def _doMoveRel(self, future, shift):
"""
move by the shift
shift (float): unit m
"""
x, y, z, _ = self.parent.GetStagePosition()
if "x" in shift:
x += shift["x"] * 1e3
if "y" in shift:
y += shift["y"] * 1e3
if "z" in shift:
z += shift["z"] * 1e3
target_pos = self._applyInversion({"x": x * 1e-3, "y": y * 1e-3, "z": z * 1e-3})
# Check range (for the axes we are moving)
for an in shift.keys():
rng = self.axes[an].range
p = target_pos[an]
if not rng[0] <= p <= rng[1]:
raise ValueError("Relative move would cause axis %s out of bound (%g m)" % (an, p))
self._moveTo(future, x, y, z)
def _doMoveAbs(self, future, pos):
"""
move to position pos
pos (float): unit m
"""
# Don't change position for unspecified coordinates
x, y, z, _ = self.parent.GetStagePosition()
# Convert to mm
if "x" in pos:
x = pos["x"] * 1e3
if "y" in pos:
y = pos["y"] * 1e3
if "z" in pos:
z = pos["z"] * 1e3
self._moveTo(future, x, y, z)
def _moveTo(self, future, x, y, z, timeout=60):
with future._moving_lock:
try:
if future._must_stop.is_set():
raise CancelledError()
logging.debug("Moving to position (%s, %s, %s)", x, y, z)
self.parent.MoveStage(x, y, z)
# documentation suggests to wait 1s before calling
# GetStagePosition() after MoveStage()
time.sleep(1)
# Wait until the move is over
# Don't check for future._must_stop because anyway the stage will
# stop moving, and so it's nice to wait until we know the stage is
# not moving.
moving = True
tstart = time.time()
while moving:
x, y, z, moving = self.parent.GetStagePosition()
# Take the opportunity to update .position
self._updatePosition({"x": x, "y": y, "z": z})
if time.time() > tstart + timeout:
self.parent.Abort()
logging.error("Timeout after submitting stage move. Aborting move.")
break
# 50 ms is about the time it takes to read the stage status
time.sleep(50e-3)
# If it was cancelled, Abort() has stopped the stage before, and
# we still have waited until the stage stopped moving. Now let
# know the user that the move is not complete.
if future._must_stop.is_set():
raise CancelledError()
except RemconError:
if future._must_stop.is_set():
raise CancelledError()
raise
finally:
future._was_stopped = True
# Update the position, even if the move didn't entirely succeed
self._updatePosition()
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set() # tell the thread taking care of the move it's over
self.parent.Abort()
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
def _createFuture(self):
"""
Return (CancellableFuture): a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event() # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
@isasync
def moveRel(self, shift):
"""
shift (dict): shift in m
"""
if not shift:
return model.InstantaneousFuture()
self._checkMoveRel(shift)
shift = self._applyInversion(shift)
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
@isasync
def moveAbs(self, pos):
"""
pos (dict): position in m
"""
if not pos:
return model.InstantaneousFuture()
self._checkMoveAbs(pos)
pos = self._applyInversion(pos)
f = self._createFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
def stop(self, axes=None):
# Empty the queue (and already stop the stage if a future is running)
self._executor.cancel()
# Try to stop the stage, even if no future is running, for safety
logging.warning("Stopping all axes: %s", ", ".join(self.axes))
self.parent.Abort()
try:
self._updatePosition()
except Exception:
logging.exception("Unexpected failure when updating position")
class ESP(model.Actuator):
def __init__(self, name, role, port, axes=None, **kwargs):
"""
A driver for a Newport ESP 301 Stage Actuator.
This driver supports a serial connection. Note that as of the Linux
kernel 4.13, the USB connection is known to _not_ work, as the TI 3410
chipset apparently behind is not handled properly. Use a of the
RS-232 port is required (via a USB adapter if necessary).
name: (str)
role: (str)
port: (str) port name. Can be a pattern, in which case all the ports
fitting the pattern will be tried.
Use /dev/fake for a simulator
axes: dict str (axis name) -> dict (axis parameters)
axis parameters: {
number (1 <= int <= 3): axis number on the hardware
range: [float, float], default is -1 -> 1
unit (str): the external unit of the axis (internal is mm),
default is "m".
conv_factor (float): a conversion factor that converts to the
device internal unit (mm), default is 1000.
}
inverted: (bool) defines if the axes are inverted
The offset can be specified by setting MD_POS_COR as a coordinate dictionary
"""
if len(axes) == 0:
raise ValueError("Needs at least 1 axis.")
# Connect to serial port
self._ser_access = threading.Lock()
self._serial = None
self._file = None
self._port, self._version = self._findDevice(port) # sets ._serial and ._file
logging.info("Found Newport ESP301 device on port %s, Ver: %s",
self._port, self._version)
self._offset = {}
self._axis_conv_factor = {}
# Not to be mistaken with axes which is a simple public view
self._axis_map = {} # axis name -> axis number used by controller
axes_def = {} # axis name -> Axis object
speed = {}
accel = {}
decel = {}
self._id = {}
for axis_name, axis_par in axes.items():
# Unpack axis parameters from the definitions in the YAML
try:
axis_num = axis_par['number']
except KeyError:
raise ValueError("Axis %s must have a number to identify it. " % (axis_name,))
try:
axis_range = axis_par['range']
except KeyError:
logging.info("Axis %s has no range. Assuming (-1, 1)", axis_name)
axis_range = (-1, 1)
try:
axis_unit = axis_par['unit']
except KeyError:
logging.info("Axis %s has no unit. Assuming m", axis_name)
axis_unit = "m"
try:
conv_factor = float(axis_par['conv_factor'])
except KeyError:
logging.info("Axis %s has no conversion factor. Assuming 1000 (m to mm)", axis_name)
conv_factor = 1000.0
self._axis_map[axis_name] = axis_num
self._axis_conv_factor[axis_num] = conv_factor
self._id[axis_num] = self.GetIdentification(axis_num)
speed[axis_name] = self.GetSpeed(axis_num)
accel[axis_name] = self.GetAcceleration(axis_num)
decel[axis_name] = self.GetDeceleration(axis_num)
# Force millimetres for consistency as the internal unit.
self.SetAxisUnit(axis_num, "mm")
# initialize each motor
self.MotorOn(axis_num)
ad = model.Axis(canAbs=True, unit=axis_unit, range=axis_range)
axes_def[axis_name] = ad
model.Actuator.__init__(self, name, role, axes=axes_def, **kwargs)
self._hwVersion = str(self._id)
self._swversion = self._version
# Get the position in object coord with the offset applied.
# RO, as to modify it the client must use .moveRel() or .moveAbs()
self.position = model.VigilantAttribute({}, readonly=True)
self._updatePosition()
self._speed = speed
self._accel = accel
self._decel = decel
# set offset due to mounting of components (float)
self._metadata[model.MD_POS_COR] = {}
# will take care of executing axis move asynchronously
self._executor = CancellableThreadPoolExecutor(1) # one task at a time
# Check the error state
self.checkError()
def terminate(self):
with self._ser_access:
self._serial.close()
model.Actuator.terminate(self)
def updateMetadata(self, md):
super(ESP, self).updateMetadata(md)
try:
value = md[model.MD_POS_COR]
except KeyError:
# there is no offset set.
return
if not isinstance(value, dict):
raise ValueError("Invalid metadata, should be a coordinate dictionary but got %s." % (value,))
# update all axes
for n in self._axis_map.keys():
if n in value:
self._offset[n] = value[n]
logging.debug("Updating offset to %s.", value)
self._updatePosition()
# Connection methods
@staticmethod
def _openSerialPort(port, baudrate):
"""
Opens the given serial port the right way for a Power control device.
port (string): the name of the serial port (e.g., /dev/ttyUSB0)
baudrate (int)
return (serial): the opened serial port
"""
ser = serial.Serial(
port=port,
baudrate=baudrate,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
rtscts=True,
timeout=1 # s
)
# Purge
ser.flush()
ser.flushInput()
# Try to read until timeout to be extra safe that we properly flushed
ser.timeout = 0
while True:
char = ser.read()
if char == '':
break
ser.timeout = 1
return ser
def _findDevice(self, ports, baudrate=19200):
"""
Look for a compatible device
ports (str): pattern for the port name
baudrate (0<int)
return:
(str): the name of the port used
(str): the hardware version string
Note: will also update ._file and ._serial
raises:
IOError: if no devices are found
"""
# For debugging purpose
if ports == "/dev/fake":
self._serial = ESPSimulator(timeout=1)
self._file = None
ve = self.GetVersion()
return ports, ve
if os.name == "nt":
raise NotImplementedError("Windows not supported")
else:
names = glob.glob(ports)
for n in names:
try:
# Ensure no one will talk to it simultaneously, and we don't talk to devices already in use
self._file = open(n) # Open in RO, just to check for lock
try:
fcntl.flock(self._file.fileno(), fcntl.LOCK_EX | fcntl.LOCK_NB) # Raises IOError if cannot lock
except IOError:
logging.info("Port %s is busy, will not use", n)
continue
self._serial = self._openSerialPort(n, baudrate)
try:
ve = self.GetVersion()
except ESPError as e:
# Can happen if the device has received some weird characters
# => try again (now that it's flushed)
logging.info("Device answered by an error %s, will try again", e)
ve = self.GetVersion()
return n, ve
except (IOError, ESPError) as e:
logging.debug(e)
logging.info("Skipping device on port %s, which didn't seem to be compatible", n)
# not possible to use this port? next one!
continue
else:
raise HwError("Failed to find a device on ports '%s'. "
"Check that the device is turned on and connected to "
"the computer." % (ports,))
def _sendOrder(self, cmd):
"""
cmd (str): command to be sent to device (without the CR)
"""
cmd = cmd + "\r"
with self._ser_access:
logging.debug("Sending command %s", cmd.encode('string_escape'))
self._serial.write(cmd.encode('ascii'))
def _sendQuery(self, cmd):
"""
cmd (str): command to be sent to device (without the CR, but with the ?)
returns (str): answer received from the device (without \n or \r)
raise:
IOError if no answer is returned in time
"""
cmd = cmd + "\r"
with self._ser_access:
logging.debug("Sending command %s", cmd.encode('string_escape'))
self._serial.write(cmd.encode('ascii'))
self._serial.timeout = 1
ans = ''
while ans[-1:] != '\r':
char = self._serial.read()
if not char:
raise IOError("Timeout after receiving %s" % ans.encode('string_escape'))
ans += char
logging.debug("Received answer %s", ans.encode('string_escape'))
return ans.strip()
# Low level serial commands.
# Note: These all convert to internal units of the controller
def GetErrorCode(self):
# Checks the device error register
return int(self._sendQuery("TE?"))
def checkError(self):
# Checks if an error occurred and raises an exception accordingly.
err_q = []
# Get all of the errors in the error FIFO stack
while True:
errcode = self.GetErrorCode()
if errcode == 0: # No error
break
else:
err_q.append(errcode)
# After errors are collected
if len(err_q) > 0:
raise ESPError("Error code(s) %s" % (err_q))
def SetAxisUnit(self, axis_num, unit):
# Set the internal unit used by the controller
if not unit in UNIT_DEF:
raise ValueError("Unknown unit name %s" % (unit,))
self._sendOrder("%d SN %d" % (axis_num, UNIT_DEF[unit]))
def MoveAbsPos(self, axis_num, pos):
"""
Requests a move to an absolute position. This is non-blocking.
Converts to internal unit of the controller
"""
self._sendOrder("%d PA %f" % (axis_num, pos))
def MoveRelPos(self, axis_num, rel):
"""
Requests a move to a relative position. This is non-blocking.
"""
self._sendOrder("%d PR %f" % (axis_num, rel)) # 0 = absolute
def GetDesiredPos(self, axis_num):
# Get the target position programmed into the controller
return float(self._sendQuery("%d DP?" % (axis_num,)))
def StopMotion(self, axis):
# Stop the motion on the specified axis
self._sendOrder("%d ST" % (axis,))
def MotorOn(self, axis):
# Start the motor
self._sendOrder("%d MO" % (axis,))
def MotorOff(self, axis):
# Stop the motor
self._sendOrder("%d MF" % (axis,))
def GetMotionDone(self, axis_n):
# Return true or false based on if the axis is still moving.
done = int(self._sendQuery("%d MD?" % axis_n))
logging.debug("Motion done: %d", done)
return bool(done)
def GetPosition(self, axis_n):
# Get the position of the axis
return float(self._sendQuery("%d TP?" % axis_n))
def GetSpeed(self, axis_n):
# Get the speed of the axis
return float(self._sendQuery("%d VA?" % axis_n))
def SetSpeed(self, axis_n, speed):
# Set the axis speed
self._sendOrder("%d VA %f" % (axis_n, speed,))
def GetAcceleration(self, axis_n):
# Get axis accel
return float(self._sendQuery("%d AC?" % axis_n))
def SetAcceleration(self, axis_n, ac):
# Set axis accel
self._sendOrder("%d AC %f" % (axis_n, ac,))
def GetDeceleration(self, axis_n):
return float(self._sendQuery("%d AG?" % axis_n))
def SetDeceleration(self, axis_n, dc):
self._sendOrder("%d AG %f" % (axis_n, dc,))
def GetIdentification(self, axis):
"""
return (str): the identification string as-is for the first axis
"""
return self._sendQuery("%d ID?" % (axis,))
def GetVersion(self):
"""
return (str): the version string as-is
"""
return self._sendQuery("VE?")
def SaveMem(self):
"""
Instruct the controller to save the current settings to non-volatile memory
"""
self._sendOrder("SM")
"""
High level commands (ie, Odemis Actuator API)
"""
def _applyOffset(self, pos):
"""
Apply the offset to the position and return it
"""
ret = dict(pos)
for axis in self._offset:
if axis in ret:
ret[axis] -= self._offset[axis]
return ret
def _removeOffset(self, pos):
"""
Remove the offset from the position and return it
"""
ret = dict(pos)
for axis in self._offset:
if axis in ret:
ret[axis] += self._offset[axis]
return ret
@isasync
def moveAbs(self, pos):
if not pos:
return model.InstantaneousFuture()
pos = self._removeOffset(pos) # Get the position in controller coord.
self._checkMoveAbs(pos)
pos = self._applyInversion(pos)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveAbs, f, pos)
return f
@isasync
def moveRel(self, shift):
self._checkMoveRel(shift)
shift = self._applyInversion(shift)
f = self._createMoveFuture()
f = self._executor.submitf(f, self._doMoveRel, f, shift)
return f
def _doMoveRel(self, future, pos):
"""
Blocking and cancellable relative move
future (Future): the future it handles
_pos (dict str -> float): axis name -> relative target position
raise:
ValueError: if the target position is
TMCLError: if the controller reported an error
CancelledError: if cancelled before the end of the move
"""
with future._moving_lock:
end = 0 # expected end
moving_axes = set()
for an, v in pos.items():
aid = self._axis_map[an]
moving_axes.add(aid)
self.MoveRelPos(aid, v * self._axis_conv_factor[aid])
# compute expected end
# convert to mm units
dur = driver.estimateMoveDuration(abs(v) * self._axis_conv_factor[aid],
self._speed[an],
self._accel[an])
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
self.checkError()
logging.debug("move successfully completed")
def _doMoveAbs(self, future, pos):
"""
Blocking and cancellable absolute move
future (Future): the future it handles
_pos (dict str -> float): axis name -> absolute target position
raise:
TMCLError: if the controller reported an error
CancelledError: if cancelled before the end of the move
"""
with future._moving_lock:
end = 0 # expected end
old_pos = self._applyInversion(self.position.value)
moving_axes = set()
for an, v in pos.items():
aid = self._axis_map[an]
moving_axes.add(aid)
self.MoveAbsPos(aid, v * self._axis_conv_factor[aid])
d = abs(v - old_pos[an])
# convert displacement unit to mm
dur = driver.estimateMoveDuration(d * self._axis_conv_factor[aid],
self._speed[an],
self._accel[an])
end = max(time.time() + dur, end)
self._waitEndMove(future, moving_axes, end)
self.checkError()
logging.debug("move successfully completed")
def _waitEndMove(self, future, axes, end=0):
"""
Wait until all the given axes are finished moving, or a request to
stop has been received.
future (Future): the future it handles
axes (set of int): the axes IDs to check
end (float): expected end time
raise:
TimeoutError: if took too long to finish the move
CancelledError: if cancelled before the end of the move
"""
moving_axes = set(axes)
last_upd = time.time()
dur = max(0.01, min(end - last_upd, 60))
max_dur = dur * 2 + 1
logging.debug("Expecting a move of %g s, will wait up to %g s", dur, max_dur)
timeout = last_upd + max_dur
last_axes = moving_axes.copy()
try:
while not future._must_stop.is_set():
for aid in moving_axes.copy(): # need copy to remove during iteration
if self.GetMotionDone(aid):
moving_axes.discard(aid)
if not moving_axes:
# no more axes to wait for
break
now = time.time()
if now > timeout:
logging.warning("Stopping move due to timeout after %g s.", max_dur)
for i in moving_axes:
self.StopMotion(i)
raise TimeoutError("Move is not over after %g s, while "
"expected it takes only %g s" %
(max_dur, dur))
# Update the position from time to time (10 Hz)
if now - last_upd > 0.1 or last_axes != moving_axes:
last_names = set(n for n, i in self._axis_map.items() if i in last_axes)
self._updatePosition(last_names)
last_upd = time.time()
last_axes = moving_axes.copy()
# Wait half of the time left (maximum 0.1 s)
left = end - time.time()
sleept = max(0.001, min(left / 2, 0.1))
future._must_stop.wait(sleept)
else:
logging.debug("Move of axes %s cancelled before the end", axes)
# stop all axes still moving them
for i in moving_axes:
self.StopMotion(i)
future._was_stopped = True
raise CancelledError()
finally:
# TODO: check if the move succeded ? (= Not failed due to stallguard/limit switch)
self._updatePosition() # update (all axes) with final position
# high-level methods (interface)
def _updatePosition(self, axes=None):
"""
update the position VA
axes (set of str): names of the axes to update or None if all should be
updated
"""
# uses the current values (converted to internal representation)
pos = self._applyInversion(self.position.value)
for n, i in self._axis_map.items():
if axes is None or n in axes:
pos[n] = self.GetPosition(i) / self._axis_conv_factor[i]
pos = self._applyInversion(pos)
pos = self._applyOffset(pos) # Appy the offset back for display
logging.debug("Updated position to %s", pos)
self.position._set_value(pos, force_write=True)
def _cancelCurrentMove(self, future):
"""
Cancels the current move (both absolute or relative). Non-blocking.
future (Future): the future to stop. Unused, only one future must be
running at a time.
return (bool): True if it successfully cancelled (stopped) the move.
"""
# The difficulty is to synchronise correctly when:
# * the task is just starting (not finished requesting axes to move)
# * the task is finishing (about to say that it finished successfully)
logging.debug("Cancelling current move")
future._must_stop.set() # tell the thread taking care of the move it's over
with future._moving_lock:
if not future._was_stopped:
logging.debug("Cancelling failed")
return future._was_stopped
def stop(self, axes=None):
self._executor.cancel()
# For safety, just force stop every axis
for an, aid in self._axis_map.items():
if axes is None or an in axes:
self.StopMotion(aid)
def _createMoveFuture(self):
"""
Return (CancellableFuture): a future that can be used to manage a move
"""
f = CancellableFuture()
f._moving_lock = threading.Lock() # taken while moving
f._must_stop = threading.Event() # cancel of the current future requested
f._was_stopped = False # if cancel was successful
f.task_canceller = self._cancelCurrentMove
return f
