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, **kwargs):
"""
port (str): port name (only if sn is not specified)
axes (list of str): names of the axes, from the 1st to the 3rd.
ustepsize (list of float): size of a microstep in m
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))
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
self._serial = self._openSerialPort(port)
self._port = port
self._ser_access = threading.Lock()
self._target = 1 # TODO: need to be selected by user? When is it not 1?
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
# 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)
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()
def terminate(self):
if self._executor:
self.stop()
self._executor.shutdown(wait=True)
self._executor = None
with self._ser_access:
if self._serial:
self._serial.close()
self._serial = None
# 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 will never 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")
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
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
npres = numpy.frombuffer(res, dtype=numpy.uint8)
good_chk = numpy.sum(npres[:-1], dtype=numpy.uint8)
if chk != good_chk:
logging.warning(
"Message checksum incorrect (%d), skipping it", chk)
continue
if not status in TMCL_OK_STATUS:
raise TMCLError(status, rval, self._instr_to_str(msg))
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 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 postion
"""
self.SendInstruction(4, 1, axis, offset) # 1 = relative
def MotorStop(self, axis):
self.SendInstruction(3, mot=axis)
def ReferenceSearch(self, axis):
self.SendInstruction(13, 0, axis) # 0 = start
def _isOnTarget(self, axis):
"""
return (bool): True if the target position is reached
"""
reached = self.GetAxisParam(axis, 8)
return (reached != 0)
# high-level methods (interface)
def _updatePosition(self):
"""
update the position VA
"""
# TODO: allow to specify which axes to update (and other axes keep the current position)
pos = {}
for i, n in enumerate(self._axes_names):
# 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 _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 = threading.Event() # 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__
def stop(self, axes=None):
self._executor.cancel()
def _doMoveRel(self, future, pos):
"""
Blocking and cancellable relative move
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
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.
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()
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:
self._updatePosition(
) # TODO: only update the axes which moved since last time
last_upd = time.time()
# 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.set()
raise CancelledError()
finally:
self._updatePosition() # update 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 future._was_stopped.is_set():
return True
else:
logging.debug("Cancelling failed")
return False
@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=[1e-6, 1e-6, 1e-6])
modl, vmaj, vmin = dev.GetVersion()
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": [1e-6, 1e-6, 1e-6]
}))
return found
class TestExecutor(unittest.TestCase):
def setUp(self):
self.executor = None
def tearDown(self):
if self.executor:
self.executor.shutdown(wait=True)
def test_one_cancellable(self):
"""
Test cancelling multiple cancellable futures running one at a time
"""
self.executor = CancellableThreadPoolExecutor(max_workers=1)
self.called = 0
# Put several long task, and cancel all of them
fs = []
for i in range(20):
f = CancellableFuture()
f.task_canceller = self._canceller
f._must_stop = threading.Event()
f = self.executor.submitf(f, self._cancellable_task, f, 3 + i)
f.add_done_callback(self._on_end_task)
fs.append(f)
time.sleep(0.1)
self.executor.cancel()
self.assertEquals(self.called, 20)
for f in fs:
self.assertTrue(f.cancelled())
self.assertRaises(CancelledError, f.result)
def test_multiple_simple(self):
"""
Try to cancel multiple running simple futures
"""
self.executor = CancellableThreadPoolExecutor(max_workers=10)
# Put several long task, and cancel all of them
fs = []
for i in range(20):
f = self.executor.submit(self._task, 3 + i)
fs.append(f)
time.sleep(0.1)
self.executor.cancel()
cancelled = 0
for f in fs:
if f.cancelled():
cancelled += 1
self.assertRaises(CancelledError, f.result)
else:
self.assertGreaterEqual(f.result(), 1) # should be a number
self.assertGreaterEqual(cancelled, 10)
def _task(self, dur):
time.sleep(dur)
return dur
def test_multiple_cancellable(self):
"""
Try to cancel multiple running cancellable futures
"""
self.executor = CancellableThreadPoolExecutor(max_workers=10)
# Put several long task, and cancel all of them
fs = []
for i in range(20):
f = CancellableFuture()
f.task_canceller = self._canceller
f._must_stop = threading.Event()
f = self.executor.submitf(f, self._cancellable_task, f, 3 + i)
fs.append(f)
time.sleep(0.1)
self.executor.cancel()
for f in fs:
self.assertTrue(f.cancelled())
self.assertRaises(CancelledError, f.result)
def _cancellable_task(self, future, dur=0):
"""
Fake task
future
dur (float): time to wait
return (float): dur
"""
now = time.time()
end = now + dur
while now < end:
left = end - now
ms = future._must_stop.wait(max(0, left))
if ms:
raise CancelledError()
now = time.time()
return dur
def _canceller(self, future):
future._must_stop.set()
# for now we assume cancel is always successful
return True
def _on_end_task(self, future):
self.called += 1
class TestExecutor(unittest.TestCase):
def setUp(self):
self.executor = None
def tearDown(self):
if self.executor:
self.executor.shutdown(wait=True)
def test_one_cancellable(self):
"""
Test cancelling multiple cancellable futures running one at a time
"""
self.executor = CancellableThreadPoolExecutor(max_workers=1)
self.called = 0
# Put several long task, and cancel all of them
fs = []
for i in range(20):
f = CancellableFuture()
f.task_canceller = self._canceller
f._must_stop = threading.Event()
f = self.executor.submitf(f, self._cancellable_task, f, 3 + i)
f.add_done_callback(self._on_end_task)
fs.append(f)
time.sleep(0.1)
self.executor.cancel()
self.assertEquals(self.called, 20)
for f in fs:
self.assertTrue(f.cancelled())
self.assertRaises(CancelledError, f.result)
def test_multiple_simple(self):
"""
Try to cancel multiple running simple futures
"""
self.executor = CancellableThreadPoolExecutor(max_workers=10)
# Put several long task, and cancel all of them
fs = []
for i in range(20):
f = self.executor.submit(self._task, 3 + i)
fs.append(f)
time.sleep(0.1)
self.executor.cancel()
cancelled = 0
for f in fs:
if f.cancelled():
cancelled += 1
self.assertRaises(CancelledError, f.result)
else:
self.assertGreaterEqual(f.result(), 1) # should be a number
self.assertGreaterEqual(cancelled, 10)
def _task(self, dur):
time.sleep(dur)
return dur
def test_multiple_cancellable(self):
"""
Try to cancel multiple running cancellable futures
"""
self.executor = CancellableThreadPoolExecutor(max_workers=10)
# Put several long task, and cancel all of them
fs = []
for i in range(20):
f = CancellableFuture()
f.task_canceller = self._canceller
f._must_stop = threading.Event()
f = self.executor.submitf(f, self._cancellable_task, f, 3 + i)
fs.append(f)
time.sleep(0.1)
self.executor.cancel()
for f in fs:
self.assertTrue(f.cancelled())
self.assertRaises(CancelledError, f.result)
def _cancellable_task(self, future, dur=0):
"""
Fake task
future
dur (float): time to wait
return (float): dur
"""
now = time.time()
end = now + dur
while now < end:
left = end - now
ms = future._must_stop.wait(max(0, left))
if ms:
raise CancelledError()
now = time.time()
return dur
def _canceller(self, future):
future._must_stop.set()
# for now we assume cancel is always successful
return True
def _on_end_task(self, future):
self.called += 1