diff --git a/src-ODrive/docs/commands.rst b/src-ODrive/docs/commands.rst index 3c9e69ca..61ee8151 100644 --- a/src-ODrive/docs/commands.rst +++ b/src-ODrive/docs/commands.rst @@ -15,6 +15,8 @@ Per-Axis Commands ------------------------------------------------------------------------------- For the most part, both axes on the ODrive can be controlled independently. +Axis 0 corresponds to connector M0 and is accessed via :code:`odrv0.axis0`; +axis 1 corresponds to M1 and is accessed via :code:`odrv0.axis1`. State Machine ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -102,6 +104,8 @@ Diagnostics * :code:`.serial_number`: A number that uniquely identifies your device. When printed in upper case hexadecimal (:code:`hex(.serial_number).upper()`), this is identical to the serial number indicated by the USB descriptor. * :code:`.fw_version_major`, :code:`.fw_version_minor`, :code:`.fw_version_revision`: The firmware version that is currently running. * :code:`.hw_version_major`, :code:`.hw_version_minor`, :code:`.hw_version_revision`: The hardware version of your ODrive. + * :code:`.vbus_voltage`: The current DC bus voltage [V]. Useful for monitoring the power supply during operation. + * :code:`.ibus`: The current drawn from the DC bus [A]. Positive values indicate current flowing from the supply into the ODrive. .. _sensorless-setup: diff --git a/src-ODrive/docs/getting-started.rst b/src-ODrive/docs/getting-started.rst index 02f375fe..264e9668 100644 --- a/src-ODrive/docs/getting-started.rst +++ b/src-ODrive/docs/getting-started.rst @@ -71,7 +71,7 @@ Safety & Power UP .. _install-odrivetool: -Downloading and Installing :code:`odriveool` +Downloading and Installing :code:`odrivetool` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Most instructions in this guide refer to a utility called `odrivetool`, so you should install that first. @@ -222,7 +222,7 @@ Start :code:`odrivetool` -------------------------------------------------------------------------------- * To launch the main interactive ODrive tool, type :code:`odrivetool` and :kbd:`Enter`. -* Connect your ODrive and wait for the tool to find it. If it dos not connect after a few seconds refer to the :ref:`troubleshooting guide `. +* Connect your ODrive and wait for the tool to find it. If it does not connect after a few seconds refer to the :ref:`troubleshooting guide `. * Now you can, for instance type :code:`odrv0.vbus_voltage` :kbd:`Enter` to inspect the boards main supply voltage. It should look something like this: diff --git a/src-ODrive/docs/troubleshooting.rst b/src-ODrive/docs/troubleshooting.rst index a7a6101d..ff0735e8 100644 --- a/src-ODrive/docs/troubleshooting.rst +++ b/src-ODrive/docs/troubleshooting.rst @@ -163,4 +163,38 @@ Index search never completes * There are some GPIO pin interrupt collisions on ODrive 3.6 that could cause index search to fail in certain conditions. If you are using both step/dir and index pins, we recommend disabling :code:`step_dir_always_on`. - For more information, `see this thread `_. \ No newline at end of file + For more information, `see this thread `_. + +Motor does not respond to commands +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +* Verify the axis is in :code:`AXIS_STATE_CLOSED_LOOP_CONTROL` by checking :code:`.current_state`. +* Run :code:`dump_errors(odrv0)` to check for any active errors that may be preventing motor movement. +* Confirm that calibration completed successfully. Re-run :code:`AXIS_STATE_FULL_CALIBRATION_SEQUENCE` if necessary. +* Check that the motor phase resistance and inductance measurements look reasonable. + A phase resistance of 0 or very high values usually indicates a wiring problem. + +ODrive does not appear on USB / cannot connect +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +* Make sure the ODrive has a valid 12–56V DC power supply connected; it does not power itself over USB. +* On **Linux**, verify the udev rules are installed (see :ref:`Getting Started `). + Try running :code:`lsusb` to confirm the device is enumerated. +* On **Windows**, use `Zadig `_ to ensure the driver for :code:`ODrive 3.x Native Interface` is set to :code:`WinUSB`. +* Try a different USB cable and a different USB port (preferably a USB 2.0 port directly on the motherboard). +* Ensure no other program (another :code:`odrivetool` instance, a serial monitor, etc.) is already connected to the ODrive. + +Calibration fails immediately with MOTOR_ERROR_PHASE_RESISTANCE_OUT_OF_RANGE +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +* Check the motor wiring. Loose or missing phase connections will cause this error. +* Verify the connector on the ODrive screw terminal is tight and making good contact. +* Make sure :code:`.motor.config.motor_type` is set correctly for your motor (high-current vs. gimbal). +* If the resistance is too high, try increasing :code:`.motor.config.calibration_current` slightly. + +Encoder CPR is wrong / position drifts +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +* Double-check the encoder CPR value. The :code:`cpr` setting must be **4 × PPR** (pulses per revolution). +* Verify the encoder A and B channels are not swapped. Swapped channels will cause the encoder to count in the wrong direction. +* For SPI encoders, confirm the SPI mode and clock settings match the encoder's datasheet. \ No newline at end of file diff --git a/src-ODrive/tools/odrive/configuration.py b/src-ODrive/tools/odrive/configuration.py index afc18adc..5eaf554d 100644 --- a/src-ODrive/tools/odrive/configuration.py +++ b/src-ODrive/tools/odrive/configuration.py @@ -7,6 +7,19 @@ from odrive.utils import OperationAbortedException, yes_no_prompt def obj_to_path(root, obj): + """ + Returns the dot-separated attribute path of obj relative to root. + + Recursively searches the attributes of root to find the object and builds + a string path such as "axis0.controller.config". + + Args: + root: The root ODrive object to search from. + obj: The target object to find within root. + + Returns: + str or None: The dot-separated path string if found, or None if not found. + """ for k in dir(root): v = getattr(root, k) if not k.startswith('_') and isinstance(v, fibre.libfibre.RemoteObject): @@ -18,6 +31,21 @@ def obj_to_path(root, obj): return None def get_dict(root, obj, is_config_object): + """ + Recursively builds a dictionary representation of an ODrive object's configuration. + + Traverses the object tree and collects all config properties into a nested dict + suitable for JSON serialization. + + Args: + root: The root ODrive object (used for resolving object references to paths). + obj: The current object to serialize. + is_config_object (bool): If True, scalar properties on this object are included. + Automatically set to True for sub-objects named 'config'. + + Returns: + dict: A nested dictionary of configuration values. + """ result = {} for k in dir(obj): @@ -35,6 +63,21 @@ def get_dict(root, obj, is_config_object): return result def set_dict(obj, path, config_dict): + """ + Applies a configuration dictionary to an ODrive object. + + Recursively writes values from config_dict to the matching properties on obj. + Collects and returns a list of error messages for any properties that could + not be restored (e.g., renamed or removed in a newer firmware version). + + Args: + obj: The ODrive object to apply configuration to. + path (str): Dot-separated path string for error reporting (use "" for the root). + config_dict (dict): Dictionary of configuration values to apply. + + Returns: + list[str]: A list of error message strings for any properties that failed to restore. + """ errors = [] for (k,v) in config_dict.items(): name = path + ("." if path != "" else "") + k @@ -53,6 +96,18 @@ def set_dict(obj, path, config_dict): return errors def get_temp_config_filename(device): + """ + Returns the path to a temporary configuration file for the given ODrive device. + + The filename is derived from the device's serial number, ensuring that each + device has a unique temporary config file in the system's temp directory. + + Args: + device: The ODrive device object. + + Returns: + str: The full path to the temporary configuration JSON file. + """ serial_number = odrive.get_serial_number_str_sync(device) safe_serial_number = ''.join(filter(str.isalnum, serial_number)) return os.path.join(tempfile.gettempdir(), 'odrive-config-{}.json'.format(safe_serial_number)) diff --git a/src-ODrive/tools/odrive/shell.py b/src-ODrive/tools/odrive/shell.py index 55370d97..5822322a 100644 --- a/src-ODrive/tools/odrive/shell.py +++ b/src-ODrive/tools/odrive/shell.py @@ -8,6 +8,9 @@ from odrive.utils import * def print_banner(): + """ + Prints the ODrive welcome banner with links to documentation and community resources. + """ print("Website: https://odriverobotics.com/") print("Docs: https://docs.odriverobotics.com/") print("Forums: https://discourse.odriverobotics.com/") @@ -19,6 +22,17 @@ def print_banner(): print('You can also type help() or quit().') def print_help(args, have_devices): + """ + Prints usage help for the interactive odrivetool shell. + + Provides guidance on connecting to an ODrive and navigating its property tree + using tab completion. + + Args: + args: Parsed command-line arguments (used to display the connection path). + have_devices (bool): If True, instructs the user to connect a device first; + otherwise shows usage examples assuming a device is already connected. + """ print('') if have_devices: print('Connect your ODrive to {} and power it up.'.format(args.path)) @@ -42,6 +56,15 @@ def print_help(args, have_devices): discovered_devices = [] def benchmark(odrv): + """ + Measures the asynchronous read throughput for the ODrive's vbus_voltage property. + + Queues 1000 asynchronous reads and reports the time taken. Used to evaluate + the USB communication performance. + + Args: + odrv: The ODrive object (e.g., odrv0). + """ import asyncio import time diff --git a/src-ODrive/tools/odrive/utils.py b/src-ODrive/tools/odrive/utils.py index 1b2ff781..6f3df23e 100644 --- a/src-ODrive/tools/odrive/utils.py +++ b/src-ODrive/tools/odrive/utils.py @@ -33,18 +33,58 @@ } async def get_serial_number_str(device): + """ + Asynchronously retrieves the serial number of an ODrive device as an uppercase hex string. + + Args: + device: The ODrive device object. + + Returns: + str: The serial number as an uppercase hexadecimal string (via .upper()), + or "[unknown serial number]" if the property is unavailable. + """ if hasattr(device, '_serial_number_property'): return format(await device._serial_number_property.read(), 'x').upper() else: return "[unknown serial number]" def get_serial_number_str_sync(device): + """ + Synchronously retrieves the serial number of an ODrive device as an uppercase hex string. + + Args: + device: The ODrive device object. + + Returns: + str: The serial number as an uppercase hexadecimal string (via .upper()), + or "[unknown serial number]" if the property is unavailable. + """ if hasattr(device, '_serial_number_property'): return format(device._serial_number_property.read(), 'x').upper() else: return "[unknown serial number]" def calculate_thermistor_coeffs(degree, Rload, R_25, Beta, Tmin, Tmax, thermistor_bottom = False, plot = False): + """ + Calculates polynomial coefficients for a thermistor's voltage-to-temperature conversion. + + Uses the Beta parameter equation to model the thermistor's resistance vs. temperature curve, + then fits a polynomial of the specified degree to map normalized voltage to temperature. + + Args: + degree (int): Degree of the polynomial fit (e.g., 3 for a cubic fit). + Rload (float): Load resistance in Ohms in the voltage divider circuit. + R_25 (float): Thermistor resistance at 25°C in Ohms. + Beta (float): Beta parameter of the thermistor in Kelvin. + Tmin (float): Minimum temperature of interest in °C. + Tmax (float): Maximum temperature of interest in °C. + thermistor_bottom (bool): If True, the thermistor is on the low side (bottom) of the + voltage divider; otherwise it is on the high side (top). Default is False. + plot (bool): If True, plots the actual vs. fitted temperature curves. Default is False. + + Returns: + numpy.poly1d: A polynomial object that maps normalized voltage to temperature in °C. + """ import numpy as np T_25 = 25 + 273.15 #Kelvin temps = np.linspace(Tmin, Tmax, 1000) @@ -78,6 +118,22 @@ class OperationAbortedException(Exception): pass def set_motor_thermistor_coeffs(axis, Rload, R_25, Beta, Tmin, Tmax, thermistor_bottom = False): + """ + Configures the motor thermistor polynomial coefficients on the specified axis. + + Calculates a 3rd-degree polynomial fit for the given thermistor parameters and writes + the coefficients directly to the ODrive axis's motor thermistor configuration. + + Args: + axis: The ODrive axis object (e.g., odrv0.axis0). + Rload (float): Load resistance in Ohms in the voltage divider circuit. + R_25 (float): Thermistor resistance at 25°C in Ohms. + Beta (float): Beta parameter of the thermistor in Kelvin. + Tmin (float): Minimum temperature of interest in °C. + Tmax (float): Maximum temperature of interest in °C. + thermistor_bottom (bool): If True, the thermistor is on the low side of the + voltage divider. Default is False. + """ coeffs = calculate_thermistor_coeffs(3, Rload, R_25, Beta, Tmin, Tmax, thermistor_bottom) axis.motor.motor_thermistor.config.poly_coefficient_0 = float(coeffs[3]) axis.motor.motor_thermistor.config.poly_coefficient_1 = float(coeffs[2]) @@ -85,6 +141,23 @@ def set_motor_thermistor_coeffs(axis, Rload, R_25, Beta, Tmin, Tmax, thermistor_ axis.motor.motor_thermistor.config.poly_coefficient_3 = float(coeffs[0]) def dump_errors(odrv, clear=False, printfunc = print): + """ + Prints all active errors for each axis and submodule of an ODrive device. + + Iterates over all axes and their submodules (motor, encoder, controller, etc.), + decoding and displaying any active error flags. Useful for diagnosing issues + with the ODrive during development or operation. + + Args: + odrv: The ODrive object (e.g., odrv0). + clear (bool): If True, clears all errors on the device after printing them. + Default is False. + printfunc (callable): Function used for output. Defaults to the built-in print. + + Example: + >>> dump_errors(odrv0) + >>> dump_errors(odrv0, clear=True) + """ axes = [(name, getattr(odrv, name)) for name in dir(odrv) if name.startswith('axis')] axes.sort() @@ -127,6 +200,17 @@ def dump_errors_for_module(indent, name, obj, path, errorcodes): odrv.clear_errors() def oscilloscope_dump(odrv, num_vals, filename='oscilloscope.csv'): + """ + Dumps oscilloscope data from the ODrive to a CSV file. + + Reads a sequence of values from the ODrive's built-in oscilloscope and writes + them to the specified file, one value per line. + + Args: + odrv: The ODrive object (e.g., odrv0). + num_vals (int): Number of oscilloscope values to read and dump. + filename (str): Path to the output CSV file. Defaults to 'oscilloscope.csv'. + """ with open(filename, 'w') as f: for x in range(num_vals): f.write(str(odrv.oscilloscope.get_val(x))) @@ -263,6 +347,23 @@ def step_and_plot( axis, settle_time=0.5, data_rate=500.0, ctrl_mode=CONTROL_MODE_POSITION_CONTROL): + """ + Sends a step command to an axis and captures/plots the system response. + + Switches the axis to the specified control mode, applies a step input, + captures the response data using BulkCapture, then returns the axis to idle + and plots the results. Useful for tuning controller gains. + + Args: + axis: The ODrive axis object to step (e.g., odrv0.axis0). + step_size (float): Magnitude of the step input in the relevant units + (turns for position control, turns/s for velocity control). Default is 100.0. + settle_time (float): Time in seconds to allow the system to settle after the step. + Default is 0.5. + data_rate (float): Data sampling rate in Hz. Default is 500.0. + ctrl_mode: Control mode to use. Must be CONTROL_MODE_POSITION_CONTROL or + CONTROL_MODE_VELOCITY_CONTROL. Default is CONTROL_MODE_POSITION_CONTROL. + """ if ctrl_mode is CONTROL_MODE_POSITION_CONTROL: get_var_callback = lambda :[axis.encoder.pos_estimate, axis.controller.pos_setpoint] @@ -316,6 +417,12 @@ def print_drv_regs(name, motor): print("Control Reg 2: " + str(ctrl_reg_2) + " (" + format(ctrl_reg_2, '#09b') + ")") def show_oscilloscope(odrv): + """ + Reads and plots 18000 oscilloscope samples from the ODrive. + + Args: + odrv: The ODrive object (e.g., odrv0). + """ size = 18000 values = [] for i in range(size): @@ -369,6 +476,17 @@ def fetch_data(): threading.Thread(target=fetch_data, daemon=True).start() def yes_no_prompt(question, default=None): + """ + Prompts the user with a yes/no question and returns their answer. + + Args: + question (str): The question to display to the user. + default (bool or None): The default answer if the user presses Enter without typing. + True means yes, False means no, None means no default (answer is required). + + Returns: + bool: True if the user answered yes, False if no. + """ if default is None: question += " [y/n] " elif default == True: @@ -388,6 +506,15 @@ def yes_no_prompt(question, default=None): return default def dump_interrupts(odrv): + """ + Prints a table of active interrupt statuses for the ODrive's STM32 microcontroller. + + Iterates over all known interrupt request numbers (IRQn) and prints the priority, + enabled state, and invocation count for each interrupt that is currently active. + + Args: + odrv: The ODrive object (e.g., odrv0). + """ interrupts = [ (-12, "MemoryManagement_IRQn"), (-11, "BusFault_IRQn"), @@ -504,6 +631,15 @@ def dump_interrupts(odrv): str((status >> 8) & 0x7fffff).rjust(7))) def dump_threads(odrv): + """ + Prints a table of RTOS thread statistics for the ODrive firmware. + + Displays each thread's stack size, maximum stack usage, and priority. + Useful for diagnosing stack overflow issues or understanding CPU usage. + + Args: + odrv: The ODrive object (e.g., odrv0). + """ prefixes = ["max_stack_usage_", "stack_size_", "prio_"] keys = [k[len(prefix):] for k in dir(odrv.system_stats) for prefix in prefixes if k.startswith(prefix)] good_keys = set([k for k in set(keys) if keys.count(k) == len(prefixes)]) @@ -524,6 +660,15 @@ def dump_threads(odrv): def dump_dma(odrv): + """ + Prints a table of active DMA stream configurations for the ODrive's STM32 microcontroller. + + Shows which DMA streams are active, their channel assignments, and the mapped + peripheral functions. Supports ODrive hardware versions 3 and 4. + + Args: + odrv: The ODrive object (e.g., odrv0). + """ if odrv.hw_version_major == 3: dma_functions = [[ # https://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf Table 42 @@ -589,6 +734,19 @@ def dump_dma(odrv): "*" if (status & 0x80000000) else " ")) def dump_timing(odrv, n_samples=100, path='/tmp/timings.png'): + """ + Captures and plots timing information for ODrive firmware tasks. + + Samples the start time and duration of each firmware task multiple times, + then generates a horizontal bar chart saved as an image file. Useful for + identifying CPU bottlenecks in the firmware control loop. + + Args: + odrv: The ODrive object (e.g., odrv0). + n_samples (int): Number of timing samples to collect. Default is 100. + path (str): File path where the timing plot image will be saved. + Default is '/tmp/timings.png'. + """ import matplotlib.pyplot as plt import re import numpy as np