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