From 82313713d8753e228f7dcb36a1b280ed6a36d765 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sun, 11 Mar 2018 19:59:23 -0700 Subject: [PATCH 01/11] [python tools] use IPython.embed() by default exmplore_odrive.py no longer needs to be executed with "ipython -i explore_odrive.py" but can launch an embedded IPython command prompt by itself. If IPython is not available, the script falls back to checking for normal python interactive mode. --- tools/explore_odrive.py | 49 +++++++++++++++++++++++++++-------------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py index 59492a30..9efaebb7 100755 --- a/tools/explore_odrive.py +++ b/tools/explore_odrive.py @@ -6,6 +6,35 @@ Load an odrive object to play with in the IPython interactive shell. import odrive.core import argparse import sys +import platform + +# Check if IPython is installed +try: + import IPython + embed_ipython = True +except: + embed_ipython = False + + print("Warning: you don't have IPython installed.") + print("If you want to have an improved interactive console with pretty colors,") + print("you should install IPython\n") + + # Ensure interactive mode + if not bool(getattr(sys, 'ps1', sys.flags.interactive)): + print("You're not running in interactive mode. Run python -i explore_odrive.py") + print('') + sys.exit(1) + + # Enable tab complete if possible + try: + import readline + readline.parse_and_bind("tab: complete") + except: + sudo_prefix = "" if platform.system() == "Windows" else "sudo " + print("Warning: could not enable tab-complete. User experience will suffer.\n" + "Run `{}pip install readline` and then restart this script to fix this." + .format(sudo_prefix)) + # some enums described in the README # TODO: transmit as part of the JSON @@ -80,20 +109,6 @@ print('and "my_odrive.motor0.pos_setpoint = 10000"') print('will send motor0 to 10000') print('') -try: - # If this assignment works, we are already in interactive mode. - # so just drop out of script to existing shell - interpreter = sys.ps1 -except AttributeError: - # We are not in interactive mode, so let's fire one up - # Though let's be real, IPython is the way to go - print('If you want to have an improved interactive console with pretty colors,') - print('you can run this script in interactive mode with IPython with this command:') - print('ipython -i explore_odrive.py') - print('') - # Enter interactive python shell with tab complete enabled - import code - import rlcompleter - import readline - readline.parse_and_bind("tab: complete") - code.interact(local=locals(), banner='') +# If IPython is installed, embed shell, otherwise drop into interactive stock python shell +if embed_ipython: + IPython.embed() From eab7cda53ea096e7c5ac81d923e9c3346e2921ff Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 12 Mar 2018 17:51:34 -0700 Subject: [PATCH 02/11] make liveplotter not steal focus and terminate as expected --- tools/liveplotter.py | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) mode change 100644 => 100755 tools/liveplotter.py diff --git a/tools/liveplotter.py b/tools/liveplotter.py old mode 100644 new mode 100755 index d432062b..1c4ba849 --- a/tools/liveplotter.py +++ b/tools/liveplotter.py @@ -19,20 +19,32 @@ plt.ion() global vals vals = [] +# Make sure the script terminates when the user closes the plotter +cancellation_token = threading.Event() +def handle_close(evt): + cancellation_token.set() +fig = plt.figure() +fig.canvas.mpl_connect('close_event', handle_close) + def fetch_data(): global vals - while True: - vals.append(my_odrive.motor0.encoder.pll_pos) + global cancellation_token + while not cancellation_token.is_set(): + vals.append(my_odrive.motor0.timing_log.TIMING_LOG_FOC_CURRENT) if len(vals) > num_samples: vals = vals[-num_samples:] time.sleep(1/data_rate) +# TODO: use animation for better UI performance, see: +# https://matplotlib.org/examples/animation/simple_anim.html def plot_data(): global vals - while True: + global cancellation_token + while not cancellation_token.is_set(): plt.clf() plt.plot(vals) - plt.pause(1/plot_rate) + #time.sleep(1/plot_rate) + fig.canvas.flush_events() fetch_thread = threading.Thread(target=fetch_data, daemon=True) fetch_thread.start() From f1d047958f8b81403cb84b7dbd4926bdd3af429f Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Thu, 15 Mar 2018 17:05:58 -0700 Subject: [PATCH 03/11] relax tup environment check When tup sees a change in environment variables, it needs to rebuild everything because it doesn't know if a process accessed the changed environment variable. However these changes are frequent when changing between VSCode and terminal. By disabling this check we trade a tiny bit of correctness for convenience. --- Firmware/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/Makefile b/Firmware/Makefile index 40bbba59..0c6316d6 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -7,7 +7,7 @@ FIRMWARE = $(BUILD_DIR)/ODriveFirmware.elf FIRMWARE_HEX = $(BUILD_DIR)/ODriveFirmware.hex all: - @tup --quiet + @tup --quiet --no-environ-check flash: all openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ write_image\ erase\ $(FIRMWARE) -c reset\ run -c exit From 3ea4b731b31d2bc25ed89805de93a85b0c8feebf Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 16 Mar 2018 19:47:51 -0700 Subject: [PATCH 04/11] rename USBHaltException to ChannelDamagedException --- tools/odrive/protocol.py | 21 ++++++++++++------- tools/odrive/usbbulk_transport.py | 35 +++++++++++++++++++------------ 2 files changed, 36 insertions(+), 20 deletions(-) diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index 18f44a41..4878fadc 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -66,14 +66,21 @@ def calc_crc16(remainder, value): class TimeoutException(Exception): pass -class ChannelBrokenException(Exception): - pass - class DeviceInitException(Exception): pass -class USBHaltException(Exception): - pass +class ChannelDamagedException(Exception): + """ + Raised when the channel is temporarily broken and a + resend of the message might be successful + """ + pass + +class ChannelBrokenException(Exception): + """ + Raised when the channel is permanently broken + """ + pass class StreamSource(ABC): @@ -243,7 +250,7 @@ class Channel(PacketSink): while (attempt < self._send_attempts): try: self._output.process_packet(packet) - except USBHaltException: + except ChannelDamagedException: attempt += 1 continue # resend deadline = time.monotonic() + self._resend_timeout @@ -254,7 +261,7 @@ class Channel(PacketSink): response = self._input.get_packet(deadline) except TimeoutException: break # resend - except USBHaltException: + except ChannelDamagedException: break # resend # process response, which is hopefully our ACK self.process_packet(response) diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index e0bb5a58..aba9a770 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -16,6 +16,7 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) self._printer = printer self.dev = dev self._name = "USB device {}:{}".format(dev.idVendor, dev.idProduct) + self._was_damaged = False ## # information about the connected device @@ -75,36 +76,44 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) def process_packet(self, usbBuffer): try: ret = self.epw.write(usbBuffer, 0) + if self._was_damaged: + self._printer("Recovered from USB halt/stall condition") + self._was_damaged = False return ret except usb.core.USBError as ex: if ex.errno == 19: # "no such device" raise odrive.protocol.ChannelBrokenException() else: # Try resetting halt/stall condition - self.epw.clear_halt() - # Resend - ret = self.epw.write(usbBuffer, 0) - self._printer("Recovered from USB halt/stall condition on write") - return ret - # Signal to retry transfer - # raise odrive.protocol.USBHaltException() + try: + self.epw.clear_halt() + except usb.core.USBError: + raise odrive.protocol.ChannelBrokenException() + # Retry transfer + self._was_damaged = True + raise odrive.protocol.ChannelDamagedException() def get_packet(self, deadline): try: bufferLen = self.epr.wMaxPacketSize timeout = max(int((deadline - time.monotonic()) * 1000), 0) ret = self.epr.read(bufferLen, timeout) + if self._was_damaged: + self._printer("Recovered from USB halt/stall condition") + self._was_damaged = False return bytearray(ret) except usb.core.USBError as ex: if ex.errno == 19: # "no such device" raise odrive.protocol.ChannelBrokenException() else: - # Try resetting halt/stall condition and flush buffer - self.epr.clear_halt() - ret = self.epr.read(bufferLen, timeout) - self._printer("Recovered from USB halt/stall condition on read") - # Signal to retry transfer - raise odrive.protocol.USBHaltException() + # Try resetting halt/stall condition + try: + self.epw.clear_halt() + except usb.core.USBError: + raise odrive.protocol.ChannelBrokenException() + # Retry transfer + self._was_damaged = True + raise odrive.protocol.ChannelDamagedException() def send_max(self): return 64 From 1c2cfd2923a84a986984f5f6986a4f4e1c38fa08 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 16 Mar 2018 22:43:21 -0700 Subject: [PATCH 05/11] [python tools] refactor device discovery and object creation --- tools/demo.py | 7 +- tools/dfu.py | 76 ++----- tools/drv_status.py | 6 +- tools/explore_odrive.py | 176 +++++++++++----- tools/liveplotter.py | 6 +- tools/odrive/core.py | 334 ------------------------------ tools/odrive/discovery.py | 88 ++++++++ tools/odrive/remote_object.py | 212 +++++++++++++++++++ tools/odrive/serial_transport.py | 53 ++++- tools/odrive/usbbulk_transport.py | 68 +++++- tools/odrive/util.py | 23 -- tools/rate_test.py | 9 +- 12 files changed, 578 insertions(+), 480 deletions(-) mode change 100644 => 100755 tools/drv_status.py delete mode 100644 tools/odrive/core.py create mode 100644 tools/odrive/discovery.py create mode 100644 tools/odrive/remote_object.py delete mode 100644 tools/odrive/util.py mode change 100644 => 100755 tools/rate_test.py diff --git a/tools/demo.py b/tools/demo.py index 4a06c260..41360333 100755 --- a/tools/demo.py +++ b/tools/demo.py @@ -5,12 +5,15 @@ Example usage of the ODrive python library to monitor and control ODrive devices from __future__ import print_function -import odrive.core +import odrive.discovery import time import math # Find a connected ODrive (this will block until you connect one) -my_drive = odrive.core.find_any(consider_usb=True, consider_serial=False, printer=print) +my_drive = odrive.discovery.find_any() + +# Find an ODrive that is connected on the serial port /dev/ttyUSB0 +#my_drive = odrive.discovery.find_any("serial:/dev/ttyUSB0") # The above call returns a python object with a dynamically generated type. The # type hierarchy will correspond to the endpoint list in `MotorControl/protocol.cpp`. diff --git a/tools/dfu.py b/tools/dfu.py index 7ad27bf8..cd8553c1 100755 --- a/tools/dfu.py +++ b/tools/dfu.py @@ -12,7 +12,7 @@ import struct import dfuse import usb.core import usb.util -import odrive.core +import odrive.discovery # We are interactively printing status messages, so flush by default import functools @@ -142,18 +142,6 @@ def jump_to_application(dfudev, address): if status[1] != dfuse.DfuState.DFU_MANIFEST: raise RuntimeError("An error occured. Device Status: {}".format(status[1])) -def str_to_uuid(uuid): - uuid = bytearray.fromhex(uuid.replace('-', '')) - return struct.unpack('>I', uuid[0:4]), struct.unpack('>I', uuid[4:8]), struct.unpack('>I', uuid[8:12]) - -def uuid_to_str(uuid0, uuid1, uuid2): - return "{:08X}-{:08X}-{:08X}".format(struct.pack('>I', uuid0), struct.pack('>I', uuid1), struct.pack('>I', uuid2)) - -def uuid_to_serial(uuid0, uuid1, uuid2): - return (struct.pack('>I', uuid0 + uuid2) + struct.pack('>I', uuid1)[0:2]).hex().upper() - - -### THREADS ### def show_deferred_message(message, cancellation_token): """ @@ -170,43 +158,25 @@ def show_deferred_message(message, cancellation_token): t.daemon = True t.start() -def put_odrive_into_dfu_mode_thread(cancellation_token): +def put_odrive_into_dfu_mode(my_drive): """ - Waits for an ODrive with a matching serial number and puts - it into DFU mode once it's found. The thread continues to put - matching devices into DFU mode until cancellation_token - is set. + Puts the specified device into DFU mode """ - global app_cancellation_token - while not cancellation_token.is_set(): - constraints = {} if serial_number == None else {'serial_number': serial_number} - my_drive = odrive.core.find_any(consider_usb=True, consider_serial=False, - cancellation_token=cancellation_token, - **constraints) - if cancellation_token.is_set(): - return - if not hasattr(my_drive, "enter_dfu_mode"): - print("The firmware on device {} does not support DFU. You need to \n" - "flash the firmware once using STLink (`make flash`), after that \n" - "DFU with this script should work fine." - .format(my_drive.__channel__.usb_device.serial_number)) - # Terminate script, otherwise it would try to reconnect to the same - # incompatible device - app_cancellation_token.set() # TODO: implement a more sensible discorvery mechanism to fix this - return - print("Putting device {} into DFU mode...".format(my_drive.__channel__.usb_device.serial_number)) - try: - my_drive.enter_dfu_mode() - except usb.core.USBError as ex: - pass # this is expected because the device reboots - if platform.system() == "Windows": - show_deferred_message("Still waiting for the device to reappear.\n" - "Use the Zadig utility to set the driver of 'STM32 BOOTLOADER' to libusb-win32.", - cancellation_token) - # If we immediately continue we might still pick up the device that was - # just rebooted. This isn't an issue but will display a distracting - # error message. - time.sleep(1) + if not hasattr(my_drive, "enter_dfu_mode"): + print("The firmware on device {} does not support DFU. You need to \n" + "flash the firmware once using STLink (`make flash`), after that \n" + "DFU with this script should work fine." + .format(my_drive.__channel__.usb_device.serial_number)) + return + print("Putting device {} into DFU mode...".format(my_drive.__channel__.usb_device.serial_number)) + try: + my_drive.enter_dfu_mode() + except odrive.protocol.ChannelBrokenException as ex: + pass # this is expected because the device reboots + if platform.system() == "Windows": + show_deferred_message("Still waiting for the device to reappear.\n" + "Use the Zadig utility to set the driver of 'STM32 BOOTLOADER' to libusb-win32.", + find_odrive_cancellation_token) ### BEGINNING OF APPLICATION ### @@ -230,12 +200,7 @@ hexfile = IntelHex(args.file) #for start, end in hexfile.segments(): # print(" {:08X} to {:08X}".format(start, end - 1)) -if args.uuid != None: - serial_number = uuid_to_serial(*str_to_uuid(args.uuid)) -elif args.serial_number != None: - serial_number = args.serial_number -else: - serial_number = None +serial_number = args.serial_number app_cancellation_token = threading.Event() @@ -244,7 +209,8 @@ try: print("Waiting for ODrive...") # Scan for ODrives not in DFU mode and put them into DFU mode once they appear - threading.Thread(target=put_odrive_into_dfu_mode_thread, args=(find_odrive_cancellation_token,)).start() + # We only scan on USB because DFU is only possible over USB + odrive.discovery.find_all("usb", serial_number, put_odrive_into_dfu_mode, find_odrive_cancellation_token) # Poll libUSB until a device in DFU mode is found while not app_cancellation_token.is_set(): diff --git a/tools/drv_status.py b/tools/drv_status.py old mode 100644 new mode 100755 index fda6c292..bc62c3ed --- a/tools/drv_status.py +++ b/tools/drv_status.py @@ -5,12 +5,14 @@ Example usage of the ODrive python library to monitor and control ODrive devices from __future__ import print_function -import odrive.core +import odrive.discovery import time import math # Find a connected ODrive (this will block until you connect one) -my_drive = odrive.core.find_any(consider_usb=True, consider_serial=False, printer=print) +print("Waiting for ODrive...") +my_drive = odrive.discovery.find_any() +print("connected") # Print DRV device regs for Motor 0 fault = my_drive.motor0.gate_driver.drv_fault diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py index 9efaebb7..b6120a5c 100755 --- a/tools/explore_odrive.py +++ b/tools/explore_odrive.py @@ -3,10 +3,15 @@ Load an odrive object to play with in the IPython interactive shell. """ -import odrive.core import argparse import sys import platform +import threading +import odrive.discovery + +# Flush stdout by default +import functools +print = functools.partial(print, flush=True) # Check if IPython is installed try: @@ -49,24 +54,27 @@ CTRL_MODE_POSITION_CONTROL = 3 # Parse arguments -parser = argparse.ArgumentParser(description='Load an odrive object to play with in the IPython interactive shell.') +parser = argparse.ArgumentParser(description='Load an odrive object to play with in the IPython interactive shell.', + formatter_class=argparse.RawTextHelpFormatter) parser.add_argument("-v", "--verbose", action="store_true", help="print debug information") -group = parser.add_mutually_exclusive_group() -group.add_argument("-d", "--discover", metavar="CHANNELS", action="store", - help="Automatically discover ODrives. Takes a comma-separated list (without spaces) " - "to indicate which connection types should be considered. Possible values are " - "usb and serial. For example \"--discover usb,serial\" indicates " - "that USB and serial ports should be scanned for ODrives. " - "If none of the below options are specified, --discover usb is assumed.") -group.add_argument("-u", "--usb", metavar="BUS:DEVICE", action="store", - help="Specifies the USB port on which the device is connected. " - "For example \"001:014\" means bus 001, device 014. The numbers can be obtained " - "using `lsusb`.") -group.add_argument("-s", "--serial", metavar="PORT", action="store", - help="Specifies the serial port on which the device is connected. " - "For example \"/dev/ttyUSB0\". Use `ls /dev/tty*` to find your port name.") -parser.set_defaults(discover="usb") +parser.add_argument("-p", "--path", metavar="PATH", action="store", + help="The path(s) where ODrive(s) should be discovered.\n" + "By default the script will connect to any ODrive on USB.\n\n" + "To select a specific USB device:\n" + " --path usb:BUS:DEVICE\n" + "usbwhere BUS and DEVICE are the bus and device numbers as shown in `lsusb`.\n\n" + "To select a specific serial port:\n" + " --path serial:PATH\n" + "where PATH is the path of the serial port. For example \"/dev/ttyUSB0\".\n" + "You can use `ls /dev/tty*` to find the correct port.\n\n" + "You can combine USB and serial specs by separating them with a comma (no space!)\n" + "Example:\n" + " --path usb,serial:/dev/ttyUSB0\n" + "means \"discover any USB device or a serial device on /dev/ttyUSB0\"") +parser.add_argument("-s", "--serial-number", action="store", + help="The serial number of the device. If omitted, any device is accepted.\n") +parser.set_defaults(path="usb") args = parser.parse_args() if (args.verbose): @@ -74,41 +82,109 @@ if (args.verbose): else: printer = lambda x: None +COLOR_RED = '\x1b[91;1m' +COLOR_CYAN = '\x1b[96;1m' +COLOR_RESET = '\x1b[0m' + +interactive_variables = {} +discovered_devices = [] + +def did_discover_device(odrive): + """ + Handles the discovery of new devices by displaying a + message and making the device available to the interactive + console + """ + serial_number = odrive.serial_number if hasattr(odrive, 'serial_number') else "[unknown serial number]" + if serial_number in discovered_devices: + verb = "Reconnected" + index = discovered_devices.index(serial_number) + else: + verb = "Connected" + discovered_devices.append(serial_number) + index = len(discovered_devices) - 1 + interactive_name = "odrv" + str(index) + + # Subscribe to disappearance of the device + odrive.__dict__["__sealed__"] = False + odrive._did_disappear_callback = lambda: did_lose_device(interactive_name) + odrive.__sealed__ = True + + # Publish new ODrive to interactive console + interactive_variables[interactive_name] = odrive + globals()[interactive_name] = odrive # Add to globals so tab complete works + print_on_second_last_line(COLOR_CYAN + "{} to ODrive {:012X} as {}".format(verb, serial_number, interactive_name) + COLOR_RESET) + +def did_lose_device(interactive_name): + """ + Handles the disappearance of a device by displaying + a message. + """ + print_on_second_last_line(COLOR_RED + "Oh no {} disappeared".format(interactive_name) + COLOR_RESET) + # Connect to device -if not args.usb is None: - try: - bus = int(args.usb.split(":")[0]) - address = int(args.usb.split(":")[1]) - except (ValueError, IndexError): - print("the --usb argument must look something like this: \"001:014\"") - sys.exit(1) - try: - my_odrive = odrive.core.open_usb(bus, address, printer=printer) - except odrive.protocol.DeviceInitException as ex: - print(str(ex)) - sys.exit(1) -elif not args.serial is None: - my_odrive = odrive.core.open_serial(args.serial, printer=printer) -else: - print("Waiting for device...") - consider_usb = 'usb' in args.discover.split(',') - consider_serial = 'serial' in args.discover.split(',') - my_odrive = odrive.core.find_any(consider_usb, consider_serial, printer=printer) -print("Connected!") +printer("Waiting for device...") +app_shutdown_token = threading.Event() +odrive.discovery.find_all(args.path, args.serial_number, + did_discover_device, app_shutdown_token, + printer=printer) -print('') -print('ODRIVE EXPLORER') -print('') -print('You can now type "my_odrive." and press ') -print('This will present you with all the properties that you can reference') -print('') -print('For example: "my_odrive.motor0.encoder.pll_pos"') -print('will print the current encoder position on motor 0') -print('and "my_odrive.motor0.pos_setpoint = 10000"') -print('will send motor0 to 10000') -print('') +def print_help(): + print('') + print('ODRIVE EXPLORER') + print('') + print('Type "odrv0." and press ') + print('This will present you with all the properties that you can reference') + print('') + print('For example: "odrv0.motor0.encoder.pll_pos"') + print('will print the current encoder position on motor 0') + print('and "odrv0.motor0.pos_setpoint = 10000"') + print('will send motor0 to 10000') + print('') -# If IPython is installed, embed shell, otherwise drop into interactive stock python shell +def print_on_second_last_line(text, **kwargs): + """ + Prints a text on the second last line. + This can be used to print a message above the command + prompt. If the command prompt spans multiple lines, + there will be glitches. + """ + # Escape character sequence: + # ESC 7: store cursor position + # ESC 1A: move cursor up by one + # ESC 1S: scroll entire viewport by one + # ESC 1L: insert 1 line at cursor position + # (print text) + # ESC 8: restore old cursor position + kwargs['end'] = '' + kwargs['flush'] = True + print('\x1b7\x1b[1A\x1b[1S\x1b[1L' + text + '\x1b8', **kwargs) + + +embed_ipython = False + +# If IPython is installed, embed IPython shell, otherwise embed regular shell if embed_ipython: - IPython.embed() + IPython.embed() +else: + import code + import rlcompleter + import readline + readline.parse_and_bind("tab: complete") + console = code.InteractiveConsole(locals=interactive_variables) + console.locals["help"] = print_help + console.runcode('import sys') + console.runcode('superexcepthook = sys.excepthook') + console.runcode('def newexcepthook(ex_class,ex,trace):\n' + ' if ex_class.__module__ == "odrive.protocol" and ex_class.__name__ == "ChannelBrokenException":\n' + ' pass\n' + ' else:\n' + ' superexcepthook(ex_class,ex,trace)') + console.runcode('sys.excepthook=newexcepthook') + #print = print_on_second_last_line + console.interact(banner='ODrive control utility v0.4\n' + 'Please connect your ODrive.\n' + 'Type help() for help.') + +app_shutdown_token.set() diff --git a/tools/liveplotter.py b/tools/liveplotter.py index 1c4ba849..e67ca8cf 100755 --- a/tools/liveplotter.py +++ b/tools/liveplotter.py @@ -4,7 +4,7 @@ Liveplotter """ import time -import odrive.core +import odrive.discovery import matplotlib.pyplot as plt import numpy as np import threading @@ -13,7 +13,7 @@ data_rate = 100 plot_rate = 10 num_samples = 1000 -my_odrive = odrive.core.find_any() +my_odrive = odrive.discovery.find_any() plt.ion() global vals @@ -30,7 +30,7 @@ def fetch_data(): global vals global cancellation_token while not cancellation_token.is_set(): - vals.append(my_odrive.motor0.timing_log.TIMING_LOG_FOC_CURRENT) + vals.append(my_odrive.motor0.encoder.pll_pos) if len(vals) > num_samples: vals = vals[-num_samples:] time.sleep(1/data_rate) diff --git a/tools/odrive/core.py b/tools/odrive/core.py deleted file mode 100644 index bb2bd8e1..00000000 --- a/tools/odrive/core.py +++ /dev/null @@ -1,334 +0,0 @@ -""" -Provides functions for the discovery of ODrive devices -""" - -import sys -import time -import json -import usb.core -import usb.util -import serial -import serial.tools.list_ports -import odrive.util -import odrive.usbbulk_transport -import odrive.serial_transport -import re -import time -import os -import odrive.protocol -import itertools -import struct -import functools - -def noprint(x): - pass - - -class SimpleDeviceProperty(property): - """ - Used internally by dynamically created objects to translate - property assignments and fetches into endpoint operations on the - object's associated channel - """ - def __init__(self, channel, id, type, struct_format, can_read, can_write): - self._channel = channel - self._id = id - self._type = type - self._struct_format = struct_format - property.__init__(self, - self.fget if can_read else None, - self.fset if can_write else None) - - def fget(self, obj): - size = struct.calcsize(self._struct_format) - buffer = self._channel.remote_endpoint_operation(self._id, None, True, size) - return struct.unpack(self._struct_format, buffer)[0] - - def fset(self, obj, value): - value = self._type(value) - buffer = struct.pack(self._struct_format, value) - # TODO: Currenly we wait for an ack here. Settle on the default guarantee. - self._channel.remote_endpoint_operation(self._id, buffer, True, 0) - -def call_remote_function(channel, trigger_id, arg_properties, *args): - """ - Used internally by the dynamically created objects to translate - function calls into endpoint operations on the associated channel - """ - if (len(arg_properties) != len(args)): - raise TypeError("expected {} arguments but have {}".format(len(arg_properties), len(args))) - for i in range(len(args)): - arg_properties[i].fset(None, args[i]) - channel.remote_endpoint_operation(trigger_id, None, True, 0) - -def setattr_or_raise_if_undefined(self, name, value): - """ - If employed as an object's __setattr__ function, this function - makes sure that an assignment to an undefined attribute doesn't - create a new attribute but instead raises an exception - """ - # We can't use hasattr here because internally it fetches the property - # value, creating unnecessary bus traffic - if name in dir(self): - object.__setattr__(self, name, value) - else: - raise TypeError('Cannot set name %r on object of type %s' % ( - name, self.__class__.__name__)) - -def create_property(name, json_data, channel, printer): - """ - Dynamically creates a property based on a JSON definition - """ - name = name or "[anonymous]" - - type_str = json_data.get("type", None) - if type_str is None: - printer("property {} has no specified type".format(name)) - return None - - if type_str == "float": - property_type = float - struct_format = " Date: Sat, 17 Mar 2018 15:50:31 -0700 Subject: [PATCH 06/11] move liveplotter to odrive.utils, make explore_odrive work with IPython --- tools/explore_odrive.py | 181 ++++++++++++++++++++++------------------ tools/liveplotter.py | 43 ++-------- tools/odrive/utils.py | 142 +++++++++++++++++++++++++++++++ 3 files changed, 249 insertions(+), 117 deletions(-) create mode 100755 tools/odrive/utils.py diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py index b6120a5c..fcf3b7d0 100755 --- a/tools/explore_odrive.py +++ b/tools/explore_odrive.py @@ -8,38 +8,12 @@ import sys import platform import threading import odrive.discovery +from odrive.utils import start_liveplotter # Flush stdout by default import functools print = functools.partial(print, flush=True) -# Check if IPython is installed -try: - import IPython - embed_ipython = True -except: - embed_ipython = False - - print("Warning: you don't have IPython installed.") - print("If you want to have an improved interactive console with pretty colors,") - print("you should install IPython\n") - - # Ensure interactive mode - if not bool(getattr(sys, 'ps1', sys.flags.interactive)): - print("You're not running in interactive mode. Run python -i explore_odrive.py") - print('') - sys.exit(1) - - # Enable tab complete if possible - try: - import readline - readline.parse_and_bind("tab: complete") - except: - sudo_prefix = "" if platform.system() == "Windows" else "sudo " - print("Warning: could not enable tab-complete. User experience will suffer.\n" - "Run `{}pip install readline` and then restart this script to fix this." - .format(sudo_prefix)) - # some enums described in the README # TODO: transmit as part of the JSON @@ -53,7 +27,7 @@ CTRL_MODE_VELOCITY_CONTROL = 2, CTRL_MODE_POSITION_CONTROL = 3 -# Parse arguments +## Parse arguments ## parser = argparse.ArgumentParser(description='Load an odrive object to play with in the IPython interactive shell.', formatter_class=argparse.RawTextHelpFormatter) parser.add_argument("-v", "--verbose", action="store_true", @@ -72,6 +46,10 @@ parser.add_argument("-p", "--path", metavar="PATH", action="store", "Example:\n" " --path usb,serial:/dev/ttyUSB0\n" "means \"discover any USB device or a serial device on /dev/ttyUSB0\"") +parser.add_argument("--no-ipython", action="store_true", + help="Use the regular Python shell\n" + "instead of the IPython shell,\n" + "even if IPython is installed\n") parser.add_argument("-s", "--serial-number", action="store", help="The serial number of the device. If omitted, any device is accepted.\n") parser.set_defaults(path="usb") @@ -82,11 +60,60 @@ if (args.verbose): else: printer = lambda x: None + +## Interactive console utils ## + COLOR_RED = '\x1b[91;1m' COLOR_CYAN = '\x1b[96;1m' COLOR_RESET = '\x1b[0m' +def print_on_second_last_line(text, **kwargs): + """ + Prints a text on the second last line. + This can be used to print a message above the command + prompt. If the command prompt spans multiple lines, + there will be glitches. + """ + # Escape character sequence: + # ESC 7: store cursor position + # ESC 1A: move cursor up by one + # ESC 1S: scroll entire viewport by one + # ESC 1L: insert 1 line at cursor position + # (print text) + # ESC 8: restore old cursor position + kwargs['end'] = '' + kwargs['flush'] = True + print('\x1b7\x1b[1A\x1b[1S\x1b[1L' + text + '\x1b8', **kwargs) + +def print_banner(): + print('ODrive control utility v0.4') + print('Please connect your ODrive.') + print('Type help() for help.') + +def print_help(): + print('') + if len(discovered_devices) == 0: + print('Connect your ODrive to {} and power it up.'.format(args.path)) + print('After that, the following message should appear:') + print(' "Connected to ODrive [serial number] as odrv0"') + print('') + print('Once the ODrive is connected, type "odrv0." and press ') + else: + print('Type "odrv0." and press ') + print('This will present you with all the properties that you can reference') + print('') + print('For example: "odrv0.motor0.encoder.pll_pos"') + print('will print the current encoder position on motor 0') + print('and "odrv0.motor0.pos_setpoint = 10000"') + print('will send motor0 to 10000') + print('') + interactive_variables = {} +interactive_variables["help"] = print_help + + +## Device discovery ## + discovered_devices = [] def did_discover_device(odrive): @@ -130,61 +157,55 @@ odrive.discovery.find_all(args.path, args.serial_number, printer=printer) -def print_help(): - print('') - print('ODRIVE EXPLORER') - print('') - print('Type "odrv0." and press ') - print('This will present you with all the properties that you can reference') - print('') - print('For example: "odrv0.motor0.encoder.pll_pos"') - print('will print the current encoder position on motor 0') - print('and "odrv0.motor0.pos_setpoint = 10000"') - print('will send motor0 to 10000') - print('') +## Launch interactive shell ## -def print_on_second_last_line(text, **kwargs): - """ - Prints a text on the second last line. - This can be used to print a message above the command - prompt. If the command prompt spans multiple lines, - there will be glitches. - """ - # Escape character sequence: - # ESC 7: store cursor position - # ESC 1A: move cursor up by one - # ESC 1S: scroll entire viewport by one - # ESC 1L: insert 1 line at cursor position - # (print text) - # ESC 8: restore old cursor position - kwargs['end'] = '' - kwargs['flush'] = True - print('\x1b7\x1b[1A\x1b[1S\x1b[1L' + text + '\x1b8', **kwargs) - - -embed_ipython = False +# Check if IPython is installed +if args.no_ipython: + use_ipython = False +else: + try: + import IPython + use_ipython = True + except: + print("Warning: you don't have IPython installed.") + print("If you want to have an improved interactive console with pretty colors,") + print("you should install IPython\n") + use_ipython = False # If IPython is installed, embed IPython shell, otherwise embed regular shell -if embed_ipython: - IPython.embed() +if use_ipython: + #interactive_variables["lalala"] = print_help + help = print_help # Override help function + console = IPython.terminal.embed.InteractiveShellEmbed(local_ns=interactive_variables, banner1='') + console.runcode = console.run_code # hack to make IPython look like the regular console + interact = console else: - import code - import rlcompleter - import readline - readline.parse_and_bind("tab: complete") - console = code.InteractiveConsole(locals=interactive_variables) - console.locals["help"] = print_help - console.runcode('import sys') - console.runcode('superexcepthook = sys.excepthook') - console.runcode('def newexcepthook(ex_class,ex,trace):\n' - ' if ex_class.__module__ == "odrive.protocol" and ex_class.__name__ == "ChannelBrokenException":\n' - ' pass\n' - ' else:\n' - ' superexcepthook(ex_class,ex,trace)') - console.runcode('sys.excepthook=newexcepthook') - #print = print_on_second_last_line - console.interact(banner='ODrive control utility v0.4\n' - 'Please connect your ODrive.\n' - 'Type help() for help.') + # Enable tab complete if possible + try: + import rlcompleter + import readline + readline.parse_and_bind("tab: complete") + except: + sudo_prefix = "" if platform.system() == "Windows" else "sudo " + print("Warning: could not enable tab-complete. User experience will suffer.\n" + "Run `{}pip install readline` and then restart this script to fix this." + .format(sudo_prefix)) + + import code + console = code.InteractiveConsole(locals=interactive_variables) + interact = lambda: console.interact(banner='') + +# install hook to hide ChannelBrokenException +console.runcode('import sys') +console.runcode('superexcepthook = sys.excepthook') +console.runcode('def newexcepthook(ex_class,ex,trace):\n' + ' if ex_class.__module__ + "." + ex_class.__name__ != "odrive.protocol.ChannelBrokenException":\n' + ' superexcepthook(ex_class,ex,trace)') +console.runcode('sys.excepthook=newexcepthook') + + +# Launch shell +print_banner() +interact() app_shutdown_token.set() diff --git a/tools/liveplotter.py b/tools/liveplotter.py index e67ca8cf..95c50c24 100755 --- a/tools/liveplotter.py +++ b/tools/liveplotter.py @@ -4,10 +4,9 @@ Liveplotter """ import time -import odrive.discovery -import matplotlib.pyplot as plt -import numpy as np import threading +import odrive.discovery +from odrive.utils import start_liveplotter data_rate = 100 plot_rate = 10 @@ -15,38 +14,8 @@ num_samples = 1000 my_odrive = odrive.discovery.find_any() -plt.ion() -global vals -vals = [] +# If you want to plot different values, change them here. +# You can plot any number of values concurrently. +start_liveplotter(lambda: [my_odrive.motor0.encoder.pll_pos, + my_odrive.motor1.encoder.pll_pos]) -# Make sure the script terminates when the user closes the plotter -cancellation_token = threading.Event() -def handle_close(evt): - cancellation_token.set() -fig = plt.figure() -fig.canvas.mpl_connect('close_event', handle_close) - -def fetch_data(): - global vals - global cancellation_token - while not cancellation_token.is_set(): - vals.append(my_odrive.motor0.encoder.pll_pos) - if len(vals) > num_samples: - vals = vals[-num_samples:] - time.sleep(1/data_rate) - -# TODO: use animation for better UI performance, see: -# https://matplotlib.org/examples/animation/simple_anim.html -def plot_data(): - global vals - global cancellation_token - while not cancellation_token.is_set(): - plt.clf() - plt.plot(vals) - #time.sleep(1/plot_rate) - fig.canvas.flush_events() - -fetch_thread = threading.Thread(target=fetch_data, daemon=True) -fetch_thread.start() - -plot_data() diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py new file mode 100755 index 00000000..f1fb779b --- /dev/null +++ b/tools/odrive/utils.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python3 +""" +Liveplotter +""" + +import sys +import time +import threading + +data_rate = 100 +plot_rate = 10 +num_samples = 1000 + +def start_liveplotter(get_var_callback): + """ + Starts a liveplotter. + The variable that is plotted is retrieved from get_var_callback. + This function returns immediately and the liveplotter quits when + the user closes it. + """ + + import matplotlib.pyplot as plt + import numpy as np + + cancellation_token = threading.Event() + + global vals + vals = [] + def fetch_data(): + global vals + while not cancellation_token.is_set(): + try: + data = get_var_callback() + except Exception as ex: + print(str(ex)) + time.sleep(1) + continue + vals.append(data) + if len(vals) > num_samples: + vals = vals[-num_samples:] + time.sleep(1/data_rate) + + # TODO: use animation for better UI performance, see: + # https://matplotlib.org/examples/animation/simple_anim.html + def plot_data(): + global vals + + plt.ion() + + # Make sure the script terminates when the user closes the plotter + def did_close(evt): + cancellation_token.set() + fig = plt.figure() + fig.canvas.mpl_connect('close_event', did_close) + + while not cancellation_token.is_set(): + plt.clf() + plt.plot(vals) + #time.sleep(1/plot_rate) + fig.canvas.flush_events() + + threading.Thread(target=fetch_data).start() + threading.Thread(target=plot_data).start() + #plot_data() + +## Exceptions ## + +class TimeoutException(Exception): + pass + +## Threading utils ## + +class Event(): + """ + Alternative to threading.Event(), enhanced by the subscribe() function + that the original fails to provide. + """ + def __init__(self): + self._evt = threading.Event() + self._subscribers = [] + self._mutex = threading.Lock() + + def is_set(self): + return self._evt.is_set() + + def set(self): + """ + Sets the event and invokes all subscribers if the event was + not already set + """ + self._mutex.acquire() + try: + if not self._evt.is_set(): + self._evt.set() + for s in self._subscribers: + s() + finally: + self._mutex.release() + + def subscribe(self, handler): + """ + Invokes the specified handler exactly once as soon as the + specified event is set. If the event is already set, the + handler is invoked immediately. + Returns a function that can be invoked to unsubscribe. + """ + self._mutex.acquire() + try: + self._subscribers.append(handler) + if self._evt.is_set(): + handler() + finally: + self._mutex.release() + return lambda: self.unsubscribe(handler) + + def unsubscribe(self, handler): + self._mutex.acquire() + try: + self._subscribers.pop(self._subscribers.index(handler)) + finally: + self._mutex.release() + + def wait(self, timeout=None): + return self._evt.wait(timeout=timeout) + +def wait_any(*events, timeout=None): + """ + Blocks until any of the specified events are triggered. + Returns the number of the event that was triggerd or raises + a TimeoutException + """ + or_event = threading.Event() + unsubscribe_functions = [] + for event in events: + unsubscribe_functions.append(event.subscribe(lambda: or_event.set())) + or_event.wait(timeout=timeout) + for unsubscribe_function in unsubscribe_functions: + unsubscribe_function() + for i in range(len(events)): + if events[i].is_set(): + return i + raise TimeoutException() From f4bded078fd5a3772a8d575db5191a9187945bf8 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sat, 17 Mar 2018 18:27:24 -0700 Subject: [PATCH 07/11] make remote_endpoint_operation thread-safe --- tools/odrive/discovery.py | 51 +++++++++------- tools/odrive/protocol.py | 102 +++++++++++++++++++++---------- tools/odrive/serial_transport.py | 3 +- 3 files changed, 102 insertions(+), 54 deletions(-) diff --git a/tools/odrive/discovery.py b/tools/odrive/discovery.py index 745a9e92..7822950a 100644 --- a/tools/odrive/discovery.py +++ b/tools/odrive/discovery.py @@ -7,6 +7,7 @@ import json import time import threading import odrive.protocol +import odrive.utils import odrive.remote_object import odrive.usbbulk_transport import odrive.serial_transport @@ -35,29 +36,35 @@ def find_all(path, serial_number, This queries the endpoint 0 on that channel to gain information about the interface, which is then used to init the corresponding object. """ - printer("Connecting to device on " + channel._name) try: - json_bytes = channel.remote_endpoint_read_buffer(0) - except (odrive.protocol.TimeoutException, odrive.protocol.ChannelBrokenException): - raise odrive.protocol.DeviceInitException("no response - probably incompatible") - json_crc16 = odrive.protocol.calc_crc16(odrive.protocol.PROTOCOL_VERSION, json_bytes) - channel._interface_definition_crc = json_crc16 - try: - json_string = json_bytes.decode("ascii") - except UnicodeDecodeError: - raise odrive.protocol.DeviceInitException("device responded on endpoint 0 with something that is not ASCII") - printer("JSON: " + json_string) - try: - json_data = json.loads(json_string) - except json.decoder.JSONDecodeError as error: - raise odrive.protocol.DeviceInitException("device responded on endpoint 0 with something that is not JSON: " + str(error)) - json_data = {"name": "odrive", "members": json_data} - obj = odrive.remote_object.RemoteObject(json_data, None, channel, None, printer) - device_serial_number = serial_number if hasattr(obj, 'serial_number') else "[unknown serial number]" - if serial_number != None and device_serial_number != serial_number: - printer("Ignoring device with serial number {}".format(device_serial_number)) - return - did_discover_object_callback(obj) + printer("Connecting to device on " + channel._name) + try: + json_bytes = channel.remote_endpoint_read_buffer(0) + except (odrive.utils.TimeoutException, odrive.protocol.ChannelBrokenException): + printer("no response - probably incompatible") + return + json_crc16 = odrive.protocol.calc_crc16(odrive.protocol.PROTOCOL_VERSION, json_bytes) + channel._interface_definition_crc = json_crc16 + try: + json_string = json_bytes.decode("ascii") + except UnicodeDecodeError: + printer("device responded on endpoint 0 with something that is not ASCII") + return + printer("JSON: " + json_string) + try: + json_data = json.loads(json_string) + except json.decoder.JSONDecodeError as error: + printer("device responded on endpoint 0 with something that is not JSON: " + str(error)) + return + json_data = {"name": "odrive", "members": json_data} + obj = odrive.remote_object.RemoteObject(json_data, None, channel, None, printer) + device_serial_number = serial_number if hasattr(obj, 'serial_number') else "[unknown serial number]" + if serial_number != None and device_serial_number != serial_number: + printer("Ignoring device with serial number {}".format(device_serial_number)) + return + did_discover_object_callback(obj) + except Exception as ex: + printer("Unexpected exception after discovering channel: " + str(ex)) # For each connection type, kick off an appropriate discovery loop for search_spec in path.split(','): diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index 4878fadc..a6c326eb 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -3,6 +3,10 @@ import time import struct import sys +import threading +import odrive.utils +from odrive.utils import wait_any +from odrive.utils import Event import abc @@ -63,9 +67,6 @@ def calc_crc16(remainder, value): #print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37]))) -class TimeoutException(Exception): - pass - class DeviceInitException(Exception): pass @@ -204,11 +205,14 @@ class Channel(PacketSink): _outbound_seq_no = 0 _interface_definition_crc = 0 _expected_acks = {} + _responses = {} # Choose these parameters to be sensible for a specific transport layer _resend_timeout = 0.1 # [s] _send_attempts = 5 + _channel_broken = Event() + def __init__(self, name, input, output): """ Params: @@ -220,6 +224,33 @@ class Channel(PacketSink): self._name = name self._input = input self._output = output + self._my_lock = threading.Lock() + self.start_receiver_thread(Event()) # TODO: use app_shutdown_token + + def start_receiver_thread(self, cancellation_token): + """ + Starts the receiver thread that processes incoming messages. + The thread quits as soon as the channel enters a broken state. + """ + def receiver_thread(): + try: + while (not cancellation_token.is_set()) and (not self._channel_broken.is_set()): + # Set an arbitrary deadline because the get_packet function + # currently doesn't support a cancellation_token + deadline = time.monotonic() + 1.0 + try: + response = self._input.get_packet(deadline) + except odrive.utils.TimeoutException: + continue # try again + except ChannelDamagedException: + continue # try again + # Process response + # This should not throw an exception, otherwise the channel breaks + self.process_packet(response) + print("receiver thread is exiting") + finally: + self._channel_broken.set() + threading.Thread(target=receiver_thread, daemon=True).start() def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length): if input is None: @@ -230,9 +261,13 @@ class Channel(PacketSink): if (expect_ack): endpoint_id |= 0x8000 - self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff) - self._outbound_seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol - seq_no = self._outbound_seq_no + self._my_lock.acquire() + try: + self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff) + seq_no = self._outbound_seq_no + finally: + self._my_lock.release() + seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol packet = struct.pack(' Date: Sat, 17 Mar 2018 19:24:08 -0700 Subject: [PATCH 08/11] robustify connect and disconnect logic --- tools/explore_odrive.py | 11 ++++---- tools/odrive/discovery.py | 7 +++-- tools/odrive/protocol.py | 25 ++++++++-------- tools/odrive/remote_object.py | 47 +++++++++---------------------- tools/odrive/serial_transport.py | 2 +- tools/odrive/usbbulk_transport.py | 18 ++++++------ tools/odrive/utils.py | 1 - 7 files changed, 47 insertions(+), 64 deletions(-) diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py index fcf3b7d0..ccc252d4 100755 --- a/tools/explore_odrive.py +++ b/tools/explore_odrive.py @@ -132,22 +132,21 @@ def did_discover_device(odrive): index = len(discovered_devices) - 1 interactive_name = "odrv" + str(index) - # Subscribe to disappearance of the device - odrive.__dict__["__sealed__"] = False - odrive._did_disappear_callback = lambda: did_lose_device(interactive_name) - odrive.__sealed__ = True - # Publish new ODrive to interactive console interactive_variables[interactive_name] = odrive globals()[interactive_name] = odrive # Add to globals so tab complete works print_on_second_last_line(COLOR_CYAN + "{} to ODrive {:012X} as {}".format(verb, serial_number, interactive_name) + COLOR_RESET) + # Subscribe to disappearance of the device + odrive.__channel__._channel_broken.subscribe(lambda: did_lose_device(interactive_name)) + def did_lose_device(interactive_name): """ Handles the disappearance of a device by displaying a message. """ - print_on_second_last_line(COLOR_RED + "Oh no {} disappeared".format(interactive_name) + COLOR_RESET) + if not app_shutdown_token.is_set(): + print_on_second_last_line(COLOR_RED + "Oh no {} disappeared".format(interactive_name) + COLOR_RESET) # Connect to device printer("Waiting for device...") diff --git a/tools/odrive/discovery.py b/tools/odrive/discovery.py index 7822950a..f31f0a2d 100644 --- a/tools/odrive/discovery.py +++ b/tools/odrive/discovery.py @@ -6,6 +6,7 @@ import sys import json import time import threading +import traceback import odrive.protocol import odrive.utils import odrive.remote_object @@ -57,14 +58,14 @@ def find_all(path, serial_number, printer("device responded on endpoint 0 with something that is not JSON: " + str(error)) return json_data = {"name": "odrive", "members": json_data} - obj = odrive.remote_object.RemoteObject(json_data, None, channel, None, printer) + obj = odrive.remote_object.RemoteObject(json_data, None, channel, printer) device_serial_number = serial_number if hasattr(obj, 'serial_number') else "[unknown serial number]" if serial_number != None and device_serial_number != serial_number: printer("Ignoring device with serial number {}".format(device_serial_number)) return did_discover_object_callback(obj) - except Exception as ex: - printer("Unexpected exception after discovering channel: " + str(ex)) + except Exception: + printer("Unexpected exception after discovering channel: " + traceback.format_exc()) # For each connection type, kick off an appropriate discovery loop for search_spec in path.split(','): diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index a6c326eb..413fca58 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -4,6 +4,7 @@ import time import struct import sys import threading +import traceback import odrive.utils from odrive.utils import wait_any from odrive.utils import Event @@ -106,11 +107,10 @@ class PacketSink(ABC): class StreamToPacketConverter(StreamSink): - _header = [] - _packet = [] - _packet_length = 0 - def __init__(self, output): + self._header = [] + self._packet = [] + self._packet_length = 0 self._output = output def process_bytes(self, bytes): @@ -202,18 +202,11 @@ class PacketFromStreamConverter(PacketSource): class Channel(PacketSink): - _outbound_seq_no = 0 - _interface_definition_crc = 0 - _expected_acks = {} - _responses = {} - # Choose these parameters to be sensible for a specific transport layer _resend_timeout = 0.1 # [s] _send_attempts = 5 - _channel_broken = Event() - - def __init__(self, name, input, output): + def __init__(self, name, input, output, printer): """ Params: input: A PacketSource where this channel will source packets from on @@ -224,7 +217,13 @@ class Channel(PacketSink): self._name = name self._input = input self._output = output + self._printer = printer + self._outbound_seq_no = 0 + self._interface_definition_crc = 0 + self._expected_acks = {} + self._responses = {} self._my_lock = threading.Lock() + self._channel_broken = Event() self.start_receiver_thread(Event()) # TODO: use app_shutdown_token def start_receiver_thread(self, cancellation_token): @@ -248,6 +247,8 @@ class Channel(PacketSink): # This should not throw an exception, otherwise the channel breaks self.process_packet(response) print("receiver thread is exiting") + except Exception: + self._printer("receiver thread is exiting: " + traceback.format_exc()) finally: self._channel_broken.set() threading.Thread(target=receiver_thread, daemon=True).start() diff --git a/tools/odrive/remote_object.py b/tools/odrive/remote_object.py index 3c051620..58da44ab 100644 --- a/tools/odrive/remote_object.py +++ b/tools/odrive/remote_object.py @@ -16,17 +16,13 @@ import odrive.protocol class ObjectDefinitionError(Exception): pass -class RemoteProperty(property): +class RemoteProperty(): """ Used internally by dynamically created objects to translate property assignments and fetches into endpoint operations on the object's associated channel """ def __init__(self, json_data, parent): - property.__init__(self, - lambda prop, obj: self.get_value(prop), - lambda prop, obj, val: self.set_value(prop, val)) - self._parent = parent id_str = json_data.get("id", None) if id_str is None: @@ -39,7 +35,7 @@ class RemoteProperty(property): type_str = json_data.get("type", None) if type_str is None: - raise ObjectDefinitionError("unspecified type".format(name)) + raise ObjectDefinitionError("unspecified type") if type_str == "float": self._property_type = float @@ -116,31 +112,32 @@ class RemoteObject(object): """ Object with functions and properties that map to remote endpoints """ - _remote_attributes = {} - __sealed__ = False - - def __init__(self, json_data, parent, channel, did_disappear_callback, printer): + def __init__(self, json_data, parent, channel, printer): """ Creates an object that implements the specified JSON type description by communicating over the provided channel """ + # Directly write to __dict__ to avoid calling __setattr__ too early + object.__getattribute__(self, "__dict__")["_remote_attributes"] = {} + object.__getattribute__(self, "__dict__")["__sealed__"] = False + # Assign once more to make linter happy self._remote_attributes = {} self.__sealed__ = False + self.__channel__ = channel self.__parent__ = parent - self._did_disappear_callback = did_disappear_callback # Build attribute list from JSON for member_json in json_data.get("members", []): member_name = member_json.get("name", None) if member_name is None: - printer("ignoring unnamed attribute in {}".format(namespace)) + printer("ignoring unnamed attribute") continue try: type_str = member_json.get("type", None) if type_str == "object": - attribute = RemoteObject(member_json, self, channel, self._did_disappear, printer) + attribute = RemoteObject(member_json, self, channel, printer) elif type_str == "function": attribute = RemoteFunction(member_json, self) elif type_str != None: @@ -157,6 +154,7 @@ class RemoteObject(object): # Ensure that from here on out assignments to undefined attributes # raise an exception self.__sealed__ = True + channel._channel_broken.subscribe(self._tear_down) def __str__(self): return str(dir(self)) # TODO: improve print output @@ -164,17 +162,10 @@ class RemoteObject(object): return self.__str__() def __getattribute__(self, name): - #print("get attr " + name) - #d = object.__getattribute__(self, "__dict__") - #attr = d.get("_remote_attributes", {}).get(name, None) attr = object.__getattribute__(self, "_remote_attributes").get(name, None) if isinstance(attr, RemoteProperty): if attr._can_read: - try: - return attr.get_value() - except odrive.protocol.ChannelBrokenException: - self._did_disappear() - raise + return attr.get_value() else: raise Exception("Cannot read from property {}".format(name)) elif attr != None: @@ -184,15 +175,10 @@ class RemoteObject(object): #raise AttributeError("Attribute {} not found".format(name)) def __setattr__(self, name, value): - #print("set attr " + name) attr = object.__getattribute__(self, "_remote_attributes").get(name, None) if isinstance(attr, RemoteProperty): if attr._can_write: - try: - attr.set_value(value) - except odrive.protocol.ChannelBrokenException: - self._did_disappear() - raise + attr.set_value(value) else: raise Exception("Cannot write to property {}".format(name)) elif not object.__getattribute__(self, "__sealed__") or name in object.__getattribute__(self, "__dict__"): @@ -200,13 +186,8 @@ class RemoteObject(object): else: raise AttributeError("Attribute {} not found".format(name)) - def _did_disappear(self): + def _tear_down(self): # Clear all remote members for k in self._remote_attributes.keys(): self.__dict__.pop(k) self._remote_attributes = {} - - # Call hook - if self._did_disappear_callback: - self._did_disappear_callback() - diff --git a/tools/odrive/serial_transport.py b/tools/odrive/serial_transport.py index e4621f9e..cfb9168e 100644 --- a/tools/odrive/serial_transport.py +++ b/tools/odrive/serial_transport.py @@ -77,7 +77,7 @@ def discover_channels(path, serial_number, callback, cancellation_token, printer output_stream = odrive.protocol.PacketToStreamConverter(serial_device) channel = odrive.protocol.Channel( "serial port {}@{}".format(port_name, ODRIVE_BAUDRATE), - input_stream, output_stream) + input_stream, output_stream, printer) channel.serial_device = serial_device callback(channel) known_devices.append(port_name) diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index 8d0f1a19..2a7075b8 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -29,16 +29,12 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) for cfg in self.dev: string += "ConfigurationValue {0}\n".format(cfg.bConfigurationValue) for intf in cfg: - string += "\tInterfaceNumber {0},{0}\n".format(intf.bInterfaceNumber, intf.bAlternateSetting) + string += "\tInterfaceNumber {0},{1}\n".format(intf.bInterfaceNumber, intf.bAlternateSetting) for ep in intf: string += "\t\tEndpointAddress {0}\n".format(ep.bEndpointAddress) return string def init(self): - # Resetting device to start init from a known state - # self.dev.reset() - # time.sleep(1) - # detach kernel driver try: if self.dev.is_kernel_driver_active(1): self.dev.detach_kernel_driver(1) @@ -166,13 +162,19 @@ def discover_channels(path, serial_number, callback, cancellation_token, printer bulk_device.init() channel = odrive.protocol.Channel( "USB device bus {} device {}".format(usb_device.bus, usb_device.address), - bulk_device, bulk_device) + bulk_device, bulk_device, printer) channel.usb_device = usb_device # for debugging only except usb.core.USBError as ex: if ex.errno == 13: printer("USB device access denied. Did you set up your udev rules correctly?") continue - raise - callback(channel) + elif ex.errno == 16: + printer("USB device busy. I'll reset it and try again.") + usb_device.reset() + continue + else: + printer("USB device init failed. Ignoring this device") + else: + callback(channel) known_devices.append((usb_device.bus, usb_device.address)) time.sleep(1) diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index f1fb779b..be19adcd 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -20,7 +20,6 @@ def start_liveplotter(get_var_callback): """ import matplotlib.pyplot as plt - import numpy as np cancellation_token = threading.Event() From 1386ff21202ca1a7f093233421840ebf8fc96317 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sat, 17 Mar 2018 19:50:27 -0700 Subject: [PATCH 09/11] update changelog --- Firmware/CHANGELOG.md | 7 +++++++ Firmware/README.md | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index b78e3f21..8e46007c 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -11,6 +11,13 @@ Please add a note of your changes below this heading if you make a Pull Request. * Update CubeMX generated STM platform code to version 1.19.0 * Remove `UUID_0`, `UUID_1` and `UUID_2` from USB protocol. Use `serial_number` instead. * Freertos memory pool (task stacks, etc) now uses Core Coupled Memory. +* Refactor python tools + * `explore_odrive.py` now supports controlling multiple ODrives concurrently (`odrv0`, `odrv1`, ...) + * No need to restart `explore_odrive.py` when devices get disconnected and reconnected + * The command line arguments of `explore_odrive.py` have changed. See `explore_odrive.py --help` for more details. + * ODrive accesses from within python tools are now thread-safe + * Liveplotter does no longer steal focus and closes as expected + * (experimental: start liveplotter from `explore_odrive.py` by typing `start_liveplotter(lambda: odrv0.motor0.encoder.encoder_state)`) ### Fixed * malloc now fails if we run out of memory (before it would always succeed even if we are out of ram...) diff --git a/Firmware/README.md b/Firmware/README.md index 540265d6..11f9445f 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -167,7 +167,7 @@ pip install pyusb pyserial 6. Open the bash prompt in the `ODrive/tools/` folder. 7. Run `python3 demo.py` or `python3 explore_odrive.py`. - `demo.py` is a very simple script which will make motor 0 turn back and forth. Use this as an example if you want to control the ODrive yourself programatically. -- `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --discover serial`. +- `explore_odrive.py` drops you into an interactive python shell where you can explore and edit the parameters that are available on your device. For instance `my_odrive.motor0.pos_setpoint = 10000` makes motor0 move to position 10000. To connect over serial instead of USB run `./tools/explore_odrive.py --path serial`. ### From Arduino [See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) From a2413e3469b3af1fd74522ac465d448ac83700c7 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sat, 17 Mar 2018 20:29:57 -0700 Subject: [PATCH 10/11] robustify connect/reconnect via serial connection --- tools/odrive/serial_transport.py | 32 +++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/tools/odrive/serial_transport.py b/tools/odrive/serial_transport.py index cfb9168e..53ca6850 100644 --- a/tools/odrive/serial_transport.py +++ b/tools/odrive/serial_transport.py @@ -39,6 +39,9 @@ class SerialStreamTransport(odrive.protocol.StreamSource, odrive.protocol.Stream raise odrive.utils.TimeoutException("expected {} bytes but got only {}", n_bytes, len(result)) return result + def close(self): + self._dev.close() + def find_dev_serial_ports(): try: @@ -68,17 +71,28 @@ def discover_channels(path, serial_number, callback, cancellation_token, printer return False return bool(re.match(regex, port_name)) + def did_disconnect(port_name, device): + device.close() + # TODO: yes there is a race condition here in case you wonder. + known_devices.pop(known_devices.index(port_name)) + while not cancellation_token.is_set(): all_ports = find_pyserial_ports() + find_dev_serial_ports() new_ports = filter(device_matcher, all_ports) for port_name in new_ports: - serial_device = SerialStreamTransport(port_name, ODRIVE_BAUDRATE) - input_stream = odrive.protocol.PacketFromStreamConverter(serial_device) - output_stream = odrive.protocol.PacketToStreamConverter(serial_device) - channel = odrive.protocol.Channel( - "serial port {}@{}".format(port_name, ODRIVE_BAUDRATE), - input_stream, output_stream, printer) - channel.serial_device = serial_device - callback(channel) - known_devices.append(port_name) + try: + serial_device = SerialStreamTransport(port_name, ODRIVE_BAUDRATE) + input_stream = odrive.protocol.PacketFromStreamConverter(serial_device) + output_stream = odrive.protocol.PacketToStreamConverter(serial_device) + channel = odrive.protocol.Channel( + "serial port {}@{}".format(port_name, ODRIVE_BAUDRATE), + input_stream, output_stream, printer) + channel.serial_device = serial_device + except serial.serialutil.SerialException: + printer("Serial device init failed. Ignoring this port") + known_devices.append(port_name) + else: + known_devices.append(port_name) + channel._channel_broken.subscribe(lambda: did_disconnect(port_name, serial_device)) + callback(channel) time.sleep(1) From 4404e0aa74449d6a6910077f0340c194cc002aa1 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sat, 17 Mar 2018 21:14:22 -0700 Subject: [PATCH 11/11] increase readability --- tools/odrive/usbbulk_transport.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index 2a7075b8..f513789f 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -174,7 +174,8 @@ def discover_channels(path, serial_number, callback, cancellation_token, printer continue else: printer("USB device init failed. Ignoring this device") + known_devices.append((usb_device.bus, usb_device.address)) else: + known_devices.append((usb_device.bus, usb_device.address)) callback(channel) - known_devices.append((usb_device.bus, usb_device.address)) time.sleep(1)