make tests work on back-to-back rig, check test preconditions

2026-08-18 09:29:03 +08:00 · 2018-04-06 13:03:35 -07:00
parent 1aba0cb513
commit 359f0c656a
4 changed files with 374 additions and 138 deletions
@@ -13,15 +13,15 @@ AXIS_STATE_CLOSED_LOOP_CONTROL = 8

 AXIS_ERROR_NO_ERROR = 0
 AXIS_ERROR_INVALID_STATE = 1
-AXIS_ERROR_DC_BUS_UNDER_VOLTAGE = 2
-AXIS_ERROR_DC_BUS_OVER_VOLTAGE = 3
-AXIS_ERROR_CURRENT_MEASUREMENT_TIMEOUT = 4
-AXIS_ERROR_CONTROL_LOOP_TIMEOUT = 5
-AXIS_ERROR_MOTOR_FAILED = 6
-AXIS_ERROR_SENSORLESS_ESTIMATOR_FAILED = 7
-AXIS_ERROR_ENCODER_FAILED = 8
-AXIS_ERROR_CONTROLLER_FAILED = 9
-AXIS_ERROR_POS_CTRL_DURING_SENSORLESS = 10
+#AXIS_ERROR_DC_BUS_UNDER_VOLTAGE = 2
+#AXIS_ERROR_DC_BUS_OVER_VOLTAGE = 3
+#AXIS_ERROR_CURRENT_MEASUREMENT_TIMEOUT = 4
+#AXIS_ERROR_CONTROL_LOOP_TIMEOUT = 5
+#AXIS_ERROR_MOTOR_FAILED = 6
+#AXIS_ERROR_SENSORLESS_ESTIMATOR_FAILED = 7
+#AXIS_ERROR_ENCODER_FAILED = 8
+#AXIS_ERROR_CONTROLLER_FAILED = 9
+#AXIS_ERROR_POS_CTRL_DURING_SENSORLESS = 10

 MOTOR_TYPE_HIGH_CURRENT = 0
 #MOTOR_TYPE_LOW_CURRENT = 1
@@ -4,8 +4,10 @@ import shlex
 import math
 import time
 import sys
+import threading
 import odrive.discovery
 from odrive.enums import *
+import odrive.utils

 import abc
 ABC = abc.ABC
@@ -14,6 +16,36 @@ class TestFailed(Exception):
    def __init__(self, message):
        Exception.__init__(self, message)

+class PreconditionsNotMet(Exception):
+    pass
+
+class ODriveTestContext():
+    def __init__(self, name: str, yaml: dict):
+        self.handle = None
+        self.yaml = yaml
+        self.name = name
+        self.axes = []
+        for axis_idx, axis_yaml in enumerate(yaml['axes']):
+            axis_name = axis_yaml['name'] if 'name' in axis_yaml else '{}.axis{}'.format(name, axis_idx)
+            self.axes.append(AxisTestContext(axis_name, axis_yaml, self))
+
+    def rediscover(self):
+        """
+        Reconnects to the ODrive
+        """
+        self.handle = odrive.discovery.find_any(
+            path="usb", serial_number=self.yaml['serial-number'], timeout=15)
+        for axis_idx, axis_ctx in enumerate(self.axes):
+            axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)]
+
+class AxisTestContext():
+    def __init__(self, name: str, yaml: dict, odrv_ctx: ODriveTestContext):
+        self.handle = None
+        self.yaml = yaml
+        self.name = name
+        self.lock = threading.Lock()
+        self.odrv_ctx = odrv_ctx
+
 def test_assert_eq(observed, expected, range=None, accuracy=None):
    if range is None and accuracy is None and observed != expected:
        raise TestFailed("value mismatch: expected {} but observed {}".format(expected, observed))
@@ -22,6 +54,19 @@ def test_assert_eq(observed, expected, range=None, accuracy=None):
    elif not accuracy is None and ((observed < expected * (1 - accuracy)) or (observed > expected * (1 + accuracy))):
        raise TestFailed("value out of range: expected {}+-{}% but observed {}".format(expected, accuracy*100.0, observed))

+def test_assert_no_error(axis_ctx: AxisTestContext):
+    errors = []
+    if axis_ctx.handle.motor.error != 0:
+        errors.append("motor failed with error {:04X}".format(axis_ctx.handle.motor.error))
+    if axis_ctx.handle.encoder.error != 0:
+        errors.append("encoder failed with error {:04X}".format(axis_ctx.handle.encoder.error))
+    if axis_ctx.handle.sensorless_estimator.error != 0:
+        errors.append("sensorless_estimator failed with error {:04X}".format(axis_ctx.handle.sensorless_estimator.error))
+    if axis_ctx.handle.error != 0:
+        errors.append("axis failed with error {:04X}".format(axis_ctx.handle.error))
+    if len(errors) > 0:
+        raise TestFailed("\n".join(errors))
+
 def run(command_line, logger, timeout=None):
    """
    Runs a shell command in the current directory
@@ -35,24 +80,51 @@ def run(command_line, logger, timeout=None):
        logger.error(result.stdout.decode(sys.stdout.encoding))
        raise TestFailed("command {} failed".format(command_line))

-def rediscover(odrv_yaml):
+def request_state(axis_ctx: AxisTestContext, state, expect_success=True):
+    axis_ctx.handle.requested_state = state
+    time.sleep(0.001)
+    if expect_success:
+        test_assert_eq(axis_ctx.handle.current_state, state)
+    else:
+        test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
+        test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_INVALID_STATE)
+        axis_ctx.handle.error = AXIS_ERROR_NO_ERROR # reset error
+
+def set_limits(axis_ctx: AxisTestContext, logger, vel_limit=20000, current_limit=10):
    """
-    Connects to the ODrive indicated by odrv_yaml
+    Sets the velocity and current limits for the axis, subject to the following constraints:
+      - the arguments given to this function are not exceeded
+      - max motor current is not exceeded
+      - max brake resistor power divided by two is not exceeded (here velocity takes precedence over current)
    """
-    odrv = odrive.discovery.find_any(path="usb", serial_number=odrv_yaml['serial-number'], timeout=10)
-    odrv_yaml['odrv'] = odrv
-    for axis_idx, axis_yaml in enumerate(odrv_yaml['axes']):
-        axis_yaml['axis'] = odrv.__dict__['axis{}'.format(axis_idx)]
-    return odrv
+    max_rpm = vel_limit / axis_ctx.yaml['encoder-cpr'] * 60
+    max_emf_voltage = max_rpm / axis_ctx.yaml['motor-kv']
+    max_brake_power = axis_ctx.odrv_ctx.yaml['max-brake-power'] / 2 * 0.8 # 20% safety margin
+    max_motor_current = max_brake_power / max_emf_voltage
+    logger.debug("velocity limit = {} => V_emf = {:.3}V, I_lim = {:.3}A".format(vel_limit, max_emf_voltage, max_motor_current))
+
+    # Bound current limit based on the motor's current limit and the brake resistor current limit
+    current_limit = min(current_limit, axis_ctx.yaml['motor-max-current'], max_motor_current)
+    # TODO: set as an atomic operation
+    axis_ctx.handle.motor.config.current_lim = current_limit
+    axis_ctx.handle.controller.config.vel_limit = vel_limit
+

 class ODriveTest(ABC):
    """
    Tests inheriting from this class get full ownership of the ODrive
    being tested. However no guarantees are made for the mechanical
    state of the axes.
+    The test can demand exclusive run time which means that the host will
+    not run any other test at the same time. This can be used if the test
+    invokes a command that's so lame that it can't run twice concurrently.
    """
+    def __init__(self, exclusive=False):
+        self._exclusive = exclusive
+    def check_preconditions(self, odrv_ctx: ODriveTestContext, logger):
+        pass
    @abc.abstractmethod
-    def run_test(self, odrv, odrv_config, logger):
+    def run_test(self, odrv_ctx: ODriveTestContext, logger):
        pass

 class AxisTest(ABC):
@@ -62,8 +134,16 @@ class AxisTest(ABC):
    axis, the other axis is guaranteed to be disabled (high impedance)
    during this test.
    """
+    def check_preconditions(self, axis_ctx: AxisTestContext, logger):
+        test_assert_no_error(axis_ctx)
+        test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
+        if (abs(axis_ctx.handle.encoder.pll_vel) > 500):
+            logger.warn("axis still in motion, delaying 2 sec...")
+            time.sleep(2)
+        test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=500)
+
    @abc.abstractmethod
-    def run_test(self, axis, axis_config, logger):
+    def run_test(self, axis_ctx: AxisTestContext, logger):
        pass

 class DualAxisTest(ABC):
@@ -71,30 +151,48 @@ class DualAxisTest(ABC):
    Tests using this scope get ownership of two axes that are mechanically
    coupled.
    """
+    def check_preconditions(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger):
+        test_assert_no_error(axis0_ctx)
+        test_assert_no_error(axis1_ctx)
+        test_assert_eq(axis0_ctx.handle.current_state, AXIS_STATE_IDLE)
+        test_assert_eq(axis1_ctx.handle.current_state, AXIS_STATE_IDLE)
+        test_assert_eq(axis0_ctx.handle.encoder.pll_vel, 0, range=1000)
+        test_assert_eq(axis1_ctx.handle.encoder.pll_vel, 0, range=1000)
+
    @abc.abstractmethod
-    def run_test(self, axis0, axis0_config, axis1, axis1_config, logger):
+    def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger):
        pass

+class TestDiscoverAndGotoIdle(ODriveTest):
+    def run_test(self, odrv_ctx: ODriveTestContext, logger):
+        odrv_ctx.rediscover()
+        odrv_ctx.axes[0].handle.error = 0
+        odrv_ctx.axes[1].handle.error = 0
+        request_state(odrv_ctx.axes[0], AXIS_STATE_IDLE)
+        request_state(odrv_ctx.axes[1], AXIS_STATE_IDLE)
+
 class TestFlashAndErase(ODriveTest):
-    def run_test(self, odrv, odrv_config, logger):
-        run("make flash PROGRAMMER='" + odrv_config['programmer'] + "'", logger, timeout=20)
+    def __init__(self):
+        ODriveTest.__init__(self, exclusive=True)
+    def run_test(self, odrv_ctx: ODriveTestContext, logger):
+        run("make flash PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=20)
        # FIXME: device does not reboot correctly after erasing config this way
        #run("make erase_config PROGRAMMER='" + test_rig.programmer + "'", timeout=10)

        logger.debug("waiting for ODrive...")
-        odrv = rediscover(odrv_config)
+        odrv_ctx.rediscover()
        # ensure the correct odrive is returned
-        test_assert_eq(format(odrv.serial_number, 'x').upper(), odrv_config['serial-number'])
+        test_assert_eq(format(odrv_ctx.handle.serial_number, 'x').upper(), odrv_ctx.yaml['serial-number'])

        # erase configuration and reboot
        logger.debug("erasing old configuration...")
-        odrv.erase_configuration()
+        odrv_ctx.handle.erase_configuration()
        #time.sleep(0.1)
        try:
            # FIXME: sometimes the device does not reappear after this ("no response - probably incompatible")
            # this is a firmware issue since it persists when unplugging/replugging
            # but goes away when power cycling the device
-            odrv.reboot()
+            odrv_ctx.handle.reboot()
        except odrive.protocol.ChannelBrokenException:
            pass # this is expected
        time.sleep(0.5)
@@ -103,36 +201,26 @@ class TestSetup(ODriveTest):
    """
    Preconditions: ODrive is unconfigured and just rebooted
    """
-    def run_test(self, odrv, odrv_config, logger):
-        odrv = rediscover(odrv_config)
+    def run_test(self, odrv_ctx: ODriveTestContext, logger):
+        odrv_ctx.rediscover()

        # initial protocol tests and setup
        logger.debug("setting up ODrive...")
-        odrv.config.enable_uart = True
-        test_assert_eq(odrv.config.enable_uart, True)
-        odrv.config.enable_uart = False
-        test_assert_eq(odrv.config.enable_uart, False)
-        odrv.config.brake_resistance = 1.0
-        test_assert_eq(odrv.config.brake_resistance, 1.0)
-        odrv.config.brake_resistance = odrv_config['brake-resistance']
-        test_assert_eq(odrv.config.brake_resistance, odrv_config['brake-resistance'], accuracy=0.01)
+        odrv_ctx.handle.config.enable_uart = True
+        test_assert_eq(odrv_ctx.handle.config.enable_uart, True)
+        odrv_ctx.handle.config.enable_uart = False
+        test_assert_eq(odrv_ctx.handle.config.enable_uart, False)
+        odrv_ctx.handle.config.brake_resistance = 1.0
+        test_assert_eq(odrv_ctx.handle.config.brake_resistance, 1.0)
+        odrv_ctx.handle.config.brake_resistance = odrv_ctx.yaml['brake-resistance']
+        test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01)

        # firmware has 1500ms startup delay
        time.sleep(2)

        logger.debug("ensure we're in idle state")
-        test_assert_eq(odrv.axis0.current_state, AXIS_STATE_IDLE)
-        test_assert_eq(odrv.axis1.current_state, AXIS_STATE_IDLE)
-
-def request_state(axis, state, expect_success=True):
-    axis.requested_state = state
-    time.sleep(0.001)
-    if expect_success:
-        test_assert_eq(axis.current_state, state)
-    else:
-        test_assert_eq(axis.current_state, AXIS_STATE_IDLE)
-        test_assert_eq(axis.error, AXIS_ERROR_INVALID_STATE)
-        axis.error = AXIS_ERROR_NO_ERROR # reset error
+        test_assert_eq(odrv_ctx.handle.axis0.current_state, AXIS_STATE_IDLE)
+        test_assert_eq(odrv_ctx.handle.axis1.current_state, AXIS_STATE_IDLE)

 class TestMotorCalibration(AxisTest):
    """
@@ -141,25 +229,29 @@ class TestMotorCalibration(AxisTest):
    Preconditions: The motor must be uncalibrated.
    Postconditions: The motor will be calibrated after this test.
    """
-    def run_test(self, axis, axis_config, logger):
+    def check_preconditions(self, axis_ctx: AxisTestContext, logger):
+        super(TestMotorCalibration, self).check_preconditions(axis_ctx, logger)
+        test_assert_eq(axis_ctx.handle.motor.is_calibrated, False)
+
+    def run_test(self, axis_ctx: AxisTestContext, logger):
        logger.debug("try to enter closed loop control (should be rejected)")
-        request_state(axis, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False)
+        request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False)

        logger.debug("try to start encoder index search (should be rejected)")
-        request_state(axis, AXIS_STATE_ENCODER_INDEX_SEARCH, expect_success=False)
+        request_state(axis_ctx, AXIS_STATE_ENCODER_INDEX_SEARCH, expect_success=False)

        logger.debug("try to start encoder offset calibration (should be rejected)")
-        request_state(axis, AXIS_STATE_ENCODER_OFFSET_CALIBRATION, expect_success=False)
+        request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION, expect_success=False)

        logger.debug("motor calibration (takes about 4.5 seconds)")
-        axis.motor.config.pole_pairs = axis_config['motor-pole-pairs']
-        request_state(axis, AXIS_STATE_MOTOR_CALIBRATION)
+        axis_ctx.handle.motor.config.pole_pairs = axis_ctx.yaml['motor-pole-pairs']
+        request_state(axis_ctx, AXIS_STATE_MOTOR_CALIBRATION)
        time.sleep(6)
-        test_assert_eq(axis.current_state, AXIS_STATE_IDLE)
-        test_assert_eq(axis.error, AXIS_ERROR_NO_ERROR)
-        test_assert_eq(axis.motor.config.phase_resistance, axis_config['motor-phase-resistance'], accuracy=0.1)
-        test_assert_eq(axis.motor.config.phase_inductance, axis_config['motor-phase-inductance'], accuracy=0.5)
-        axis.motor.config.pre_calibrated = True
+        test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
+        test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_NO_ERROR)
+        test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.2)
+        test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5)
+        axis_ctx.handle.motor.config.pre_calibrated = True

 class TestEncoderOffsetCalibration(AxisTest):
    """
@@ -167,19 +259,32 @@ class TestEncoderOffsetCalibration(AxisTest):
    Preconditions: The encoder must be non-ready.
    Postconditions: The encoder will be ready after this test.
    """
-    def run_test(self, axis, axis_config, logger):
+    def __init__(self, pass_if_ready=False):
+        AxisTest.__init__(self)
+        self._pass_if_ready = pass_if_ready
+
+    def check_preconditions(self, axis_ctx: AxisTestContext, logger):
+        super(TestEncoderOffsetCalibration, self).check_preconditions(axis_ctx, logger)
+        if not self._pass_if_ready:
+            test_assert_eq(axis_ctx.handle.encoder.is_ready, False)
+
+    def run_test(self, axis_ctx: AxisTestContext, logger):
+        if (self._pass_if_ready and axis_ctx.handle.encoder.is_ready):
+            logger.debug("encoder already ready, skipping this test")
+            return
+
        logger.debug("try to enter closed loop control (should be rejected)")
-        request_state(axis, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False)
+        request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False)

        logger.debug("encoder offset calibration (takes about 9.5 seconds)")
-        axis.encoder.config.cpr = axis_config['encoder-cpr'] # TODO: test setting a wrong CPR
-        request_state(axis, AXIS_STATE_ENCODER_OFFSET_CALIBRATION)
+        axis_ctx.handle.encoder.config.cpr = axis_ctx.yaml['encoder-cpr'] # TODO: test setting a wrong CPR
+        request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION)
        # TODO: ensure the encoder calibration doesn't do crap
        time.sleep(11)
-        test_assert_eq(axis.current_state, AXIS_STATE_IDLE)
-        test_assert_eq(axis.error, AXIS_ERROR_NO_ERROR)
-        test_assert_eq(axis.motor.config.direction, axis_config['motor-direction'])
-        axis.encoder.config.pre_calibrated = True
+        test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
+        test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_NO_ERROR)
+        test_assert_eq(axis_ctx.handle.motor.config.direction, axis_ctx.yaml['motor-direction'])
+        axis_ctx.handle.encoder.config.pre_calibrated = True

 class TestClosedLoopControl(AxisTest):
    """
@@ -187,49 +292,108 @@ class TestClosedLoopControl(AxisTest):
    and verifies that the sensorless estimator works
    Precondition: The axis is calibrated and ready for closed loop control
    """
-    def run_test(self, axis, axis_config, logger):
+    def check_preconditions(self, axis_ctx: AxisTestContext, logger):
+        super(TestClosedLoopControl, self).check_preconditions(axis_ctx, logger)
+        test_assert_eq(axis_ctx.handle.motor.is_calibrated, True)
+        test_assert_eq(axis_ctx.handle.encoder.is_ready, True)
+
+    def run_test(self, axis_ctx: AxisTestContext, logger):
        logger.debug("closed loop control: test tiny position changes")
-        axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        axis_ctx.handle.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
        time.sleep(0.001)
-        test_assert_eq(axis.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL)
+        test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL)
        time.sleep(0.1) # give the PLL some time to settle
-        test_assert_eq(axis.encoder.pll_pos, 0, range=300)
-        axis.controller.set_pos_setpoint(1000, 0, 0)
+        init_pos = axis_ctx.handle.encoder.pll_pos
+        axis_ctx.handle.controller.set_pos_setpoint(init_pos+1000, 0, 0)
        time.sleep(0.5)
-        test_assert_eq(axis.encoder.pll_pos, 1000, range=200)
-        axis.controller.set_pos_setpoint(-1000, 0, 0)
+        test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos+1000, range=200)
+        axis_ctx.handle.controller.set_pos_setpoint(init_pos-1000, 0, 0)
        time.sleep(0.5)
-        test_assert_eq(axis.encoder.pll_pos, -1000, range=200)
+        test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos-1000, range=400)

        logger.debug("closed loop control: test vel_limit")
-        axis.controller.set_pos_setpoint(50000, 0, 0)
-        axis.controller.config.vel_limit = 40000
+        axis_ctx.handle.controller.set_pos_setpoint(50000, 0, 0)
+        axis_ctx.handle.controller.config.vel_limit = 40000
        time.sleep(0.3)
-        test_assert_eq(axis.encoder.pll_vel, 40000, range=4000)
-        expected_sensorless_estimation = 40000 * 2 * math.pi / axis_config['encoder-cpr'] * axis_config['motor-pole-pairs']
-        test_assert_eq(axis.sensorless_estimator.pll_vel, expected_sensorless_estimation, range=50)
+        test_assert_eq(axis_ctx.handle.encoder.pll_vel, 40000, range=4000)
+        expected_sensorless_estimation = 40000 * 2 * math.pi / axis_ctx.yaml['encoder-cpr'] * axis_ctx.yaml['motor-pole-pairs']
+        test_assert_eq(axis_ctx.handle.sensorless_estimator.pll_vel, expected_sensorless_estimation, range=50)
        time.sleep(3)
-        test_assert_eq(axis.encoder.pll_vel, 0, range=1000)
+        test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=1000)
+        time.sleep(0.5)
+        request_state(axis_ctx, AXIS_STATE_IDLE)

 class TestStoreAndReboot(ODriveTest):
    """
    Stores the current configuration to NVM and reboots.
    """
-    def run_test(self, odrv, odrv_config, logger):
+    def run_test(self, odrv_ctx: ODriveTestContext, logger):
        logger.debug("storing configuration and rebooting...")
-        odrv.save_configuration()
+        odrv_ctx.handle.save_configuration()
        try:
-            odrv.reboot()
+            odrv_ctx.handle.reboot()
        except odrive.protocol.ChannelBrokenException:
            pass # this is expected
        time.sleep(2)

-        odrv = rediscover(odrv_config)
+        odrv_ctx.rediscover()

        logger.debug("verifying configuration after reboot...")
-        test_assert_eq(odrv.config.brake_resistance, odrv_config['brake-resistance'], accuracy=0.01)
-        for axis_config in odrv_config['axes']:
-            axis = axis_config['axis']
-            test_assert_eq(axis.encoder.config.cpr, axis_config['encoder-cpr'])
-            test_assert_eq(axis.motor.config.phase_resistance, axis_config['motor-phase-resistance'], accuracy=0.1)
-            test_assert_eq(axis.motor.config.phase_inductance, axis_config['motor-phase-inductance'], accuracy=0.5)
+        test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01)
+        for axis_ctx in odrv_ctx.axes:
+            test_assert_eq(axis_ctx.handle.encoder.config.cpr, axis_ctx.yaml['encoder-cpr'])
+            test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.15)
+            test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5)
+
+class TestVelCtrlVsPosCtrl(DualAxisTest):
+    """
+    Uses one ODrive as a load operating in velocity control mode.
+    The other ODrive tries to "fight" against the load in position mode.
+    """
+    def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger):
+        load_ctx = axis0_ctx
+        driver_ctx = axis1_ctx
+
+        # Set up viscous fluid load
+        logger.debug("activating load on {}...".format(load_ctx.name))
+        load_ctx.handle.controller.config.vel_integrator_gain = 0
+        load_ctx.handle.controller.vel_integrator_current = 0
+        set_limits(load_ctx, logger, vel_limit=100000, current_limit=50)
+        load_ctx.handle.controller.set_vel_setpoint(0, 0)
+        request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
+
+        # Turn to some position
+        logger.debug("using {} as driver against load, vel=100000...".format(driver_ctx.name))
+        set_limits(driver_ctx, logger, vel_limit=100000, current_limit=50)
+        init_pos = driver_ctx.handle.encoder.pll_pos
+        driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0)
+        request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
+        for _ in range(int(4000/5)):
+            logger.debug(str(driver_ctx.handle.motor.current_control.Iq_setpoint))
+            time.sleep(0.005)
+
+        test_assert_no_error(load_ctx)
+        test_assert_no_error(driver_ctx)
+
+        logger.debug("using {} as driver against load, vel=20000...".format(driver_ctx.name))
+        set_limits(driver_ctx, logger, vel_limit=20000, current_limit=50)
+        init_pos = driver_ctx.handle.encoder.pll_pos
+        driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0)
+        request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
+        #for _ in range(int(5*4000/5)):
+        #    logger.debug(str(driver_ctx.handle.motor.current_control.Iq_setpoint))
+        #    time.sleep(0.005)
+        time.sleep(7)
+
+        odrive.utils.print_drv_regs("load motor ({})".format(load_ctx.name), load_ctx.handle.motor)
+        odrive.utils.print_drv_regs("driver motor ({})".format(driver_ctx.name), driver_ctx.handle.motor)
+
+        test_assert_no_error(load_ctx)
+        test_assert_no_error(driver_ctx)
+
+        ## Turn to another position
+        #logger.debug("controlling against load, vel=40000...")
+        #set_limits(axis1_ctx, logger, vel_limit=40000, current_limit=20)
+        #init_pos = axis1_ctx.handle.encoder.pll_pos
+        #axis1_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0)
+        #request_state(axis1_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
@@ -17,15 +17,19 @@ from odrive.utils import Logger, for_all_parallel


 all_tests = [
-    TestFlashAndErase(),
-    TestSetup(),
-    TestMotorCalibration(),
-    # TODO: test encoder index search
-    TestEncoderOffsetCalibration(),
-    TestClosedLoopControl(),
-    TestStoreAndReboot(),
-    TestEncoderOffsetCalibration(), # need to find offset _or_ index after reboot
-    TestClosedLoopControl()
+#    TestFlashAndErase(),
+#    TestSetup(),
+#    TestMotorCalibration(),
+#    # TODO: test encoder index search
+#    TestEncoderOffsetCalibration(),
+#    # TODO: hold down one motor while the other one does an index search (should fail)
+#    TestClosedLoopControl(),
+#    TestStoreAndReboot(),
+#    TestEncoderOffsetCalibration(), # need to find offset _or_ index after reboot
+#    TestClosedLoopControl(),
+    TestDiscoverAndGotoIdle(), # for testing
+    TestEncoderOffsetCalibration(pass_if_ready=True),
+    TestVelCtrlVsPosCtrl()
    # TODO: test step/dir
    # TODO: test sensorless
    # TODO: test ASCII protocol
@@ -41,65 +45,101 @@ with open(script_path + '/test-rig.yaml', 'r') as file_stream:

 os.chdir(script_path + '/../Firmware')

-# Ensure every device has a name
-for idx, odrv_yaml in enumerate(test_rig_yaml['odrives']):
-    if not 'name' in odrv_yaml:
-        odrv_yaml['name'] = 'odrive{}'.format(idx)
+# Build a dictionary of odrive test contexts by name
+odrives_by_name = {}
+for odrv_idx, odrv_yaml in enumerate(test_rig_yaml['odrives']):
+    name = odrv_yaml['name'] if 'name' in odrv_yaml else 'odrive{}'.format(odrv_idx)
+    odrives_by_name[name] = ODriveTestContext(name, odrv_yaml)

-# Build a dictionary of axes by name (e.g. odrive0.axis0)
-# Also ensure every axis has a name and mutex
+# Build a dictionary of axis test contexts by name (e.g. odrive0.axis0)
 axes_by_name = {}
-for odrv_yaml in test_rig_yaml['odrives']:
-    for axis_idx, axis_yaml in enumerate(odrv_yaml['axes']):
-        if not 'name' in axis_yaml:
-            axis_yaml['name'] = '{}.axis{}'.format(odrv_yaml['name'], axis_idx)
-        axis_yaml['lock'] = threading.Lock()
-        axes_by_name[axis_yaml['name']] = axis_yaml
+for odrv_ctx in odrives_by_name.values():
+    for axis_idx, axis_ctx in enumerate(odrv_ctx.axes):
+        axes_by_name[axis_ctx.name] = axis_ctx

 # Ensure mechanical couplings are valid
+couplings = []
 if test_rig_yaml['couplings'] is None:
    test_rig_yaml['couplings'] = {}
 else:
-    for axis in sum(test_rig_yaml['couplings'], []):
-        if not axis in axes_by_name:
-            logger.error('Unknown axis {} in list of mechanical couplings'.format(axis))
+    for coupling in test_rig_yaml['couplings']:
+        couplings.append([axes_by_name[axis_name] for axis_name in coupling])


 try:
    for test in all_tests:
        if isinstance(test, ODriveTest):
-            def odrv_test_thread(odrv_yaml):
-                test_subject_name = odrv_yaml['name']
-                logger.info('● running {} on {}...'.format(type(test).__name__, test_subject_name))
-                odrv = odrv_yaml['odrv'] if 'odrv' in odrv_yaml else None
-                test.run_test(odrv, odrv_yaml,
-                              logger.indent('  {}: '.format(test_subject_name)))
+            def odrv_test_thread(odrv_name):
+                odrv_ctx = odrives_by_name[odrv_name]
+                logger.info('● running {} on {}...'.format(type(test).__name__, odrv_name))
+                try:
+                    test.check_preconditions(odrv_ctx,
+                              logger.indent('  {}: '.format(odrv_name)))
+                except:
+                    raise PreconditionsNotMet()
+                test.run_test(odrv_ctx,
+                              logger.indent('  {}: '.format(odrv_name)))

-            for_all_parallel(test_rig_yaml['odrives'], lambda x: x['name'], odrv_test_thread)
+            if test._exclusive:
+                for odrv in odrives_by_name:
+                    odrv_test_thread(odrv)
+            else:
+                for_all_parallel(odrives_by_name, lambda x: x, odrv_test_thread)

        elif isinstance(test, AxisTest):
            def axis_test_thread(axis_name):
                # Get all axes that are mechanically coupled with the axis specified by axis_name
-                conflicting_axes = sum([c for c in test_rig_yaml['couplings'] if (axis_name in c)], [])
+                conflicting_axes = sum([c for c in couplings if (axis_name in [a.name for a in c])], [])
                # Remove duplicates
                conflicting_axes = list(set(conflicting_axes))
                # Acquire lock for all conflicting axes
-                conflicting_axes.sort() # prevent deadlocks
+                conflicting_axes.sort(key=lambda x: x.name) # prevent deadlocks
+                axis_ctx = axes_by_name[axis_name]
                for conflicting_axis in conflicting_axes:
-                    axes_by_name[conflicting_axis]['lock'].acquire()
+                    conflicting_axis.lock.acquire()
                try:
                    # Run test on this axis
                    logger.info('● running {} on {}...'.format(type(test).__name__, axis_name))
-                    axis_yaml = axes_by_name[axis_name]
-                    test.run_test(axis_yaml['axis'], axis_yaml,
+                    try:
+                        test.check_preconditions(axis_ctx,
+                                    logger.indent('  {}: '.format(axis_name)))
+                    except:
+                        raise PreconditionsNotMet()
+                    test.run_test(axis_ctx,
                                  logger.indent('  {}: '.format(axis_name)))
                finally:
                    # Release all conflicting axes
                    for conflicting_axis in conflicting_axes:
-                        axes_by_name[conflicting_axis]['lock'].release()
+                        conflicting_axis.lock.release()

            for_all_parallel(axes_by_name, lambda x: x, axis_test_thread)

+        elif isinstance(test, DualAxisTest):
+            def dual_axis_test_thread(coupling):
+                coupling_name = "...".join([a.name for a in coupling])
+                # Remove duplicates
+                coupled_axes = list(set(coupling))
+                # Acquire lock for all conflicting axes
+                coupled_axes.sort(key=lambda x: x.name) # prevent deadlocks
+                for axis_ctx in coupled_axes:
+                    axis_ctx.lock.acquire()
+                try:
+                    # Run test on this axis
+                    logger.info('● running {} on {}...'.format(type(test).__name__, coupling_name))
+                    try:
+                        test.check_preconditions(coupled_axes[0], coupled_axes[1],
+                                    logger.indent('  {}: '.format(coupling_name)))
+                    except:
+                        raise PreconditionsNotMet()
+                    test.run_test(coupled_axes[0], coupled_axes[1],
+                                  logger.indent('  {}: '.format(coupling_name)))
+                finally:
+                    # Release all conflicting axes
+                    for axis_ctx in coupled_axes:
+                        axis_ctx.lock.release()
+
+            for_all_parallel(couplings, lambda x: "..".join([a.name for a in x]), dual_axis_test_thread)
+
        else:
            logger.warn("ignoring unknown test type {}".format(type(test)))

@@ -109,9 +149,10 @@ except:
    try:
        dont_secure_after_failure = True # TODO: disable
        if not dont_secure_after_failure:
-            def odrv_reset_thread(odrv_yaml):
-                run("make erase PROGRAMMER='" + odrv_yaml['programmer'] + "'", logger, timeout=30)
-            for_all_parallel(test_rig_yaml['odrives'], lambda x: x['name'], odrv_reset_thread)
+            def odrv_reset_thread(odrv_name):
+                odrv_ctx = odrives_by_name[odrv_name]
+                run("make erase PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=30)
+            for_all_parallel(odrives_by_name, lambda x: x['name'], odrv_reset_thread)
    except:
        logger.error('///////////////////////////////////////////')
        logger.error('/// CRITICAL: COULD NOT SECURE TEST RIG ///')
@@ -1,25 +1,56 @@

 # ODrives
 odrives:
-  - board-version: v3.4-24V
+  - name: top-odrive
+    board-version: v3.4-24V
    serial-number: "385F324D3037"
    brake-resistance: 0.47
-    uart: /dev/serial/by-id/...
+    uart: /dev/serial/by-id/[not-yet-used]
    usb: auto
-    programmer: /dev...
+    programmer: '533f7506493f49514454193f'
+    vbus-voltage: 12 # [V]
+    max-brake-power: 150 # [W]
    axes:
-      - motor-phase-resistance: 0.033
-        motor-phase-inductance: 1.6e-05
+      - motor-phase-resistance: 0.0245
+        motor-phase-inductance: 2.03e-05
        motor-pole-pairs: 7
-        motor-direction: 1
+        motor-direction: -1
+        motor-kv: 190
+        motor-max-current: 50
        encoder-cpr: 8192
      - motor-phase-resistance: 0.028
        motor-phase-inductance: 1.6e-05
        motor-pole-pairs: 7
        motor-direction: -1
+        motor-kv: 270
+        motor-max-current: 50
+        encoder-cpr: 8192
+  - name: bottom-odrive
+    board-version: v3.4-48V
+    serial-number: "306A396A3235"
+    brake-resistance: 0.47
+    uart: /dev/serial/by-id/[not-yet-used]
+    usb: auto
+    programmer: '493f6f06493f56540929113f'
+    vbus-voltage: 12 # [V]
+    max-brake-power: 150 # [W]
+    axes:
+      - motor-phase-resistance: 0.0253
+        motor-phase-inductance: 1.6e-05
+        motor-pole-pairs: 7
+        motor-direction: 1
+        motor-kv: 270
+        motor-max-current: 50
+        encoder-cpr: 8192
+      - motor-phase-resistance: 0.0245
+        motor-phase-inductance: 2.03e-05
+        motor-pole-pairs: 7
+        motor-direction: -1
+        motor-kv: 190
+        motor-max-current: 50
        encoder-cpr: 8192

 # Mechanical couplings
 couplings:
-  #- [ odrive0.axis0, odrive1.axis0 ]
-  #- [ odrive0.axis1, odrive1.axis1 ]
+  - [ top-odrive.axis0, bottom-odrive.axis1 ]
+  - [ top-odrive.axis1, bottom-odrive.axis0 ]