From db41fe9a2904d2309402da42661b2e89212c96b7 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Thu, 30 Jul 2020 18:16:56 -0400 Subject: [PATCH] Added closed loop test to CAN HWIL test Fixed ascii protocol docs: current -> torque --- docs/ascii-protocol.md | 8 +-- tools/odrive/tests/can_test.py | 126 ++++++++++++++++++++++++++++++++- 2 files changed, 129 insertions(+), 5 deletions(-) diff --git a/docs/ascii-protocol.md b/docs/ascii-protocol.md index 948e7307..9625ec47 100644 --- a/docs/ascii-protocol.md +++ b/docs/ascii-protocol.md @@ -56,13 +56,13 @@ q motor position velocity_lim torque_lim Example: `q 0 -2 1 0.1` ``` -p motor position velocity_ff current_ff +p motor position velocity_ff torque_ff ``` * `p` for position * `motor` is the motor number, `0` or `1`. * `position` is the desired position, in [turns]. * `velocity_ff` is the velocity feed-forward term, in [turns/s] (optional). -* `torque_ff` is the current feed-forward term, in [Nm] (optional). +* `torque_ff` is the torque feed-forward term, in [Nm] (optional). Example: `p 0 -2 0 0` @@ -89,9 +89,9 @@ This command updates the watchdog timer for the motor. ``` c motor torque ``` -* `c` for current +* `c` for torque * `motor` is the motor number, `0` or `1`. -* `torque` is the desired current in [Nm]. +* `torque` is the desired torque in [Nm]. This command updates the watchdog timer for the motor. diff --git a/tools/odrive/tests/can_test.py b/tools/odrive/tests/can_test.py index 863eaff1..c3b85223 100644 --- a/tools/odrive/tests/can_test.py +++ b/tools/odrive/tests/can_test.py @@ -231,6 +231,130 @@ class TestSimpleCAN(): time.sleep(2.0) odrive.prepare(logger) +class TestSimpleCANClosedLoop(): + def prepare(self, odrive: ODriveComponent, canbus: CanInterfaceComponent, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, node_id: int, extended_id: bool, logger: Logger): + # Make sure there are no funny configurations active + logger.debug('Setting up clean configuration...') + axis_ctx.parent.erase_config_and_reboot() + + # run calibration + axis_ctx.handle.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE + while axis_ctx.handle.current_state != AXIS_STATE_IDLE: + time.sleep(1) + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_no_error(axis_ctx) + + # Return a context that can be used in a with-statement. + class safe_terminator(): + def __enter__(self): + pass + def __exit__(self, exc_type, exc_val, exc_tb): + logger.debug('clearing config...') + axis_ctx.handle.requested_state = AXIS_STATE_IDLE + time.sleep(0.005) + axis_ctx.parent.erase_config_and_reboot() + return safe_terminator() + + + def get_test_cases(self, testrig: TestRig): + for odrive in testrig.get_components(ODriveComponent): + can_interfaces = list(testrig.get_connected_components(odrive.can, CanInterfaceComponent)) + for num in range(2): + encoders = testrig.get_connected_components({ + 'a': (odrive.encoders[num].a, False), + 'b': (odrive.encoders[num].b, False) + }, EncoderComponent) + motors = testrig.get_connected_components(odrive.axes[num], MotorComponent) + for motor, encoder in itertools.product(motors, encoders): + if encoder.impl in testrig.get_connected_components(motor): + yield (odrive, can_interfaces, odrive.axes[num], motor, encoder, 0, False) + + def run_test(self, odrive: ODriveComponent, canbus: CanInterfaceComponent, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, node_id: int, extended_id: bool, logger: Logger): + # this test is a sanity check to make sure that closed loop operation works + # actual testing of closed loop functionality should be tested using closed_loop_test.py + + with self.prepare(odrive, canbus, axis_ctx, motor_ctx, enc_ctx, node_id, extended_id, logger): + def my_cmd(cmd_name, **kwargs): command(canbus.handle, node_id, extended_id, cmd_name, **kwargs) + def my_req(cmd_name, **kwargs): return asyncio.run(request(canbus.handle, node_id, extended_id, cmd_name, **kwargs)) + def fence(): my_req('get_vbus_voltage') # fence to ensure the CAN command was sent + + # make sure no gpio input is overwriting our values + odrive.unuse_gpios() + + axis_ctx.handle.config.enable_watchdog = False + axis_ctx.handle.clear_errors() + axis_ctx.handle.config.can_node_id = node_id + axis_ctx.handle.config.can_node_id_extended = extended_id + time.sleep(0.1) + + my_cmd('set_node_id', node_id=node_id+20) + asyncio.run(request(canbus.handle, node_id+20, extended_id, 'get_vbus_voltage')) + test_assert_eq(axis_ctx.handle.config.can_node_id, node_id+20) + + # Reset node ID to default value + command(canbus.handle, node_id+20, extended_id, 'set_node_id', node_id=node_id) + fence() + test_assert_eq(axis_ctx.handle.config.can_node_id, node_id) + + vel_limit = 15.0 + nominal_vel = 10.0 + axis_ctx.handle.controller.config.vel_limit = vel_limit + axis_ctx.handle.motor.config.current_lim = 30.0 + + my_cmd('set_requested_state', requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL) + fence() + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) + test_assert_no_error(axis_ctx) + + start_pos = axis_ctx.handle.encoder.pos_estimate + + # position test + logger.debug('Position control test') + my_cmd('set_controller_modes', control_mode=CONTROL_MODE_POSITION_CONTROL, input_mode=INPUT_MODE_PASSTHROUGH) # position control, passthrough + fence() + my_cmd('set_input_pos', input_pos=1.0, vel_ff=0, torque_ff=0) + fence() + test_assert_eq(axis_ctx.handle.controller.input_pos, 1.0, range=0.1) + time.sleep(2) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, start_pos + 1.0, range=0.1) + my_cmd('set_input_pos', input_pos=0, vel_ff=0, torque_ff=0) + fence() + time.sleep(2) + + test_assert_no_error(axis_ctx) + + # velocity test + logger.debug('Velocity control test') + my_cmd('set_controller_modes', control_mode=CONTROL_MODE_VELOCITY_CONTROL, input_mode=INPUT_MODE_PASSTHROUGH) # velocity control, passthrough + fence() + my_cmd('set_input_vel', input_vel = nominal_vel, torque_ff=0) + fence() + time.sleep(5) + test_assert_eq(axis_ctx.handle.encoder.vel_estimate, nominal_vel, range=nominal_vel * 0.05) # big range here due to cogging and other issues + my_cmd('set_input_vel', input_vel = 0, torque_ff=0) + fence() + time.sleep(2) + + test_assert_no_error(axis_ctx) + + # torque test + logger.debug('Torque control test') + my_cmd('set_controller_modes', control_mode=CONTROL_MODE_TORQUE_CONTROL, input_mode=INPUT_MODE_PASSTHROUGH) # torque control, passthrough + fence() + my_cmd('set_input_torque', input_torque=0.5) + fence() + time.sleep(5) + test_assert_eq(axis_ctx.handle.controller.input_torque, 0.5, range=0.1) + my_cmd('set_input_torque', input_torque = 0) + fence() + time.sleep(2) + + test_assert_no_error(axis_ctx) + + # go back to idle + my_cmd('set_requested_state', requested_state = AXIS_STATE_IDLE) + fence() + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) if __name__ == '__main__': - test_runner.run(TestSimpleCAN()) + test_runner.run([TestSimpleCAN(), TestSimpleCANClosedLoop()])