def prepare(self, odrive: ODriveComponent, uart_num: int, tx_gpio: list,
rx_gpio: list, logger: Logger):
logger.debug('Enabling UART {}...'.format(chr(ord('A') + uart_num)))
# GPIOs might be in use by something other than UART and some components
# might be configured so that they would fail in the later test.
odrive.disable_mappings()
odrive.handle.config.enable_uart_a = False
odrive.handle.config.uart_a_baudrate = 115200
odrive.handle.config.enable_uart_b = False
odrive.handle.config.uart_b_baudrate = 115200
odrive.handle.config.enable_uart_c = False
odrive.handle.config.uart_c_baudrate = 115200
if uart_num == 0:
odrive.handle.config.enable_uart_a = True
mode = GPIO_MODE_UART_A
elif uart_num == 1:
odrive.handle.config.enable_uart_b = True
mode = GPIO_MODE_UART_B
elif uart_num == 2:
odrive.handle.config.enable_uart_c = True
mode = GPIO_MODE_UART_C
else:
raise TestFailed(f"unknown UART: {uart_num}")
setattr(odrive.handle.config, f'gpio{tx_gpio}_mode', mode)
setattr(odrive.handle.config, f'gpio{rx_gpio}_mode', mode)
odrive.save_config_and_reboot()
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()
axis_ctx.parent.handle.config.enable_brake_resistor = True
axis_ctx.parent.save_config_and_reboot()
# run calibration
axis_ctx.handle.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
while axis_ctx.handle.requested_state != AXIS_STATE_UNDEFINED or 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 run_test(self, axis0_ctx: ODriveAxisComponent,
motor0_ctx: MotorComponent, enc0_ctx: EncoderComponent,
axis1_ctx: ODriveAxisComponent, motor1_ctx: MotorComponent,
enc1_ctx: EncoderComponent, logger: Logger):
with SafeTerminator(logger, axis0_ctx, axis1_ctx):
nominal_rps = 3.0
nominal_vel = nominal_rps
logger.debug(
f'Testing closed loop velocity control at {nominal_rps} rounds/s...'
)
axis0_ctx.handle.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
axis0_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH
axis0_ctx.handle.controller.input_vel = 0
request_state(axis0_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
axis0_ctx.handle.controller.config.vel_limit = nominal_vel
axis0_ctx.handle.controller.config.vel_limit_tolerance = 2
axis0_ctx.handle.controller.input_vel = nominal_vel
data = record_log(lambda: [
axis0_ctx.handle.encoder.vel_estimate, axis1_ctx.handle.encoder
.vel_estimate, axis0_ctx.handle.encoder.pos_estimate
],
duration=5.0)
test_assert_no_error(axis0_ctx)
test_assert_eq(axis0_ctx.handle.current_state,
AXIS_STATE_CLOSED_LOOP_CONTROL)
request_state(axis0_ctx, AXIS_STATE_IDLE)
# axis0: encoder.vel_estimate
slope, offset, fitted_curve = fit_line(data[:, (0, 1)])
test_assert_eq(slope, 0.0, range=nominal_vel * 0.02)
test_assert_eq(offset, nominal_vel, accuracy=0.05)
test_curve_fit(data[:, (0, 1)],
fitted_curve,
max_mean_err=nominal_vel * 0.3,
inlier_range=nominal_vel * 0.5,
max_outliers=len(data[:, 0]) * 0.1)
# axis1: encoder.vel_estimate
slope, offset, fitted_curve = fit_line(data[:, (0, 2)])
test_assert_eq(slope, 0.0, range=nominal_vel * 0.02)
test_assert_eq(offset, -nominal_vel, accuracy=0.05)
test_curve_fit(data[:, (0, 2)],
fitted_curve,
max_mean_err=nominal_vel * 0.3,
inlier_range=nominal_vel * 0.5,
max_outliers=len(data[:, 0]) * 0.1)
# axis0: encoder.pos_estimate
slope, offset, fitted_curve = fit_line(data[:, (0, 3)])
test_assert_eq(slope, nominal_vel, accuracy=0.01)
test_curve_fit(data[:, (0, 3)],
fitted_curve,
max_mean_err=nominal_vel * 0.01,
inlier_range=nominal_vel * 0.1,
max_outliers=len(data[:, 0]) * 0.01)
def run_with_values(self, odrive: ODriveComponent, values: list,
logger: Logger):
logger.debug("storing configuration and rebooting...")
for value in values:
odrive.handle.config.brake_resistance = value
odrive.save_config_and_reboot()
odrive.prepare(logger)
logger.debug("verifying configuration after reboot...")
test_assert_eq(odrive.handle.config.brake_resistance,
values[-1],
accuracy=0.01)
def run_test(self, axis_ctx: ODriveAxisComponent,
motor_ctx: MotorComponent, enc_ctx: EncoderComponent,
logger: Logger):
with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger):
nominal_rps = 6.0
nominal_vel = float(enc_ctx.yaml['cpr']) * nominal_rps
# Accept a bit of noise on Ibus
axis_ctx.parent.handle.config.dc_max_negative_current = -0.2
logger.debug(f'Brake control test from {nominal_rps} rounds/s...')
axis_ctx.handle.controller.config.vel_limit = float(
enc_ctx.yaml['cpr']) * 10.0 # max 10 rps
axis_ctx.handle.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
# accelerate...
axis_ctx.handle.controller.input_vel = nominal_vel
time.sleep(1.0)
test_assert_no_error(axis_ctx)
# ... and brake
axis_ctx.handle.controller.input_vel = 0
time.sleep(1.0)
test_assert_no_error(axis_ctx)
# once more, but this time without brake resistor
axis_ctx.parent.handle.config.brake_resistance = 0
# accelerate...
axis_ctx.handle.controller.input_vel = nominal_vel
time.sleep(1.0)
test_assert_no_error(axis_ctx)
# ... and brake
axis_ctx.handle.controller.input_vel = 0 # this should fail almost instantaneously
time.sleep(0.1)
test_assert_eq(
axis_ctx.handle.error, errors.axis.ERROR_MOTOR_DISARMED
| errors.axis.ERROR_BRAKE_RESISTOR_DISARMED)
test_assert_eq(axis_ctx.handle.motor.error,
errors.motor.ERROR_DC_BUS_OVER_REGEN_CURRENT)
def run_test(self, enc: ODriveEncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, logger: Logger):
true_cps = 8192*0.5 # counts per second generated by the virtual encoder
code = teensy_incremental_encoder_emulation_code.replace("{enc_a}", str(teensy_gpio_a.num)).replace("{enc_b}", str(teensy_gpio_b.num))
teensy.compile_and_program(code)
if enc.handle.config.mode != ENCODER_MODE_INCREMENTAL:
enc.handle.config.mode = ENCODER_MODE_INCREMENTAL
enc.parent.save_config_and_reboot()
else:
time.sleep(1.0) # wait for PLLs to stabilize
enc.handle.config.bandwidth = 1000
logger.debug("testing with 8192 CPR...")
self.run_generic_encoder_test(enc.handle, 8192, true_cps / 8192)
logger.debug("testing with 65536 CPR...")
self.run_generic_encoder_test(enc.handle, 65536, true_cps / 65536)
enc.handle.config.cpr = 8192
def prepare(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent,
enc_ctx: EncoderComponent, logger: Logger):
# Make sure there are no funny configurations active
logger.debug('Setting up clean configuration...')
axis_ctx.parent.erase_config_and_reboot()
# Set motor calibration values
axis_ctx.handle.motor.config.phase_resistance = float(
motor_ctx.yaml['phase-resistance'])
axis_ctx.handle.motor.config.phase_inductance = float(
motor_ctx.yaml['phase-inductance'])
axis_ctx.handle.motor.config.pre_calibrated = True
# Set calibration settings
axis_ctx.handle.motor.config.direction = 0
axis_ctx.handle.encoder.config.use_index = False
axis_ctx.handle.encoder.config.calib_scan_omega = 12.566 # 2 electrical revolutions per second
axis_ctx.handle.encoder.config.calib_scan_distance = 50.265 # 8 revolutions
axis_ctx.handle.encoder.config.bandwidth = 1000
axis_ctx.handle.clear_errors()
logger.debug('Calibrating encoder offset...')
request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION)
time.sleep(9) # actual calibration takes 8 seconds
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 run_with_values(self, odrive: ODriveComponent, values: list,
logger: Logger):
logger.debug("storing configuration and rebooting...")
for value in values:
odrive.handle.config.brake_resistance = value
odrive.handle.save_configuration()
try:
odrive.handle.reboot()
except fibre.ChannelBrokenException:
pass # this is expected
odrive.handle = None
time.sleep(2)
odrive.prepare(logger)
logger.debug("verifying configuration after reboot...")
test_assert_eq(odrive.handle.config.brake_resistance,
values[-1],
accuracy=0.01)
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
def flush_rx():
while not canbus.handle.recv(timeout=0) is None:
pass
axis_ctx.handle.config.enable_watchdog = False
odrive.handle.clear_errors()
axis_ctx.handle.config.can.node_id = node_id
axis_ctx.handle.config.can.is_extended = extended_id
time.sleep(0.1)
my_cmd('set_node_id', node_id=node_id + 20)
flush_rx()
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 = 20.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)
def run_test(self, axis_ctx: ODriveAxisComponent,
motor_ctx: MotorComponent, enc_ctx: EncoderComponent,
logger: Logger):
with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger):
max_rps = 20.0
max_vel = float(enc_ctx.yaml['cpr']) * max_rps
absolute_max_vel = max_vel * 1.2
max_current = 3.0
axis_ctx.handle.controller.config.vel_gain /= 10 # reduce the slope to make it easier to see what's going on
vel_gain = axis_ctx.handle.controller.config.vel_gain
logger.debug(f'vel gain is {vel_gain}')
axis_ctx.handle.controller.config.vel_limit = max_vel
axis_ctx.handle.controller.config.vel_limit_tolerance = inf # disable hard limit on velocity
axis_ctx.handle.motor.config.current_lim = max_current
axis_ctx.handle.controller.config.control_mode = CTRL_MODE_CURRENT_CONTROL
# Returns the expected limited setpoint for a given velocity and current
def get_expected_setpoint(input_setpoint, velocity):
return clamp(
clamp(input_setpoint, (velocity + max_vel) * -vel_gain,
(velocity - max_vel) * -vel_gain), -max_current,
max_current)
def data_getter():
# sample velocity twice to avoid systematic bias
velocity0 = axis_ctx.handle.encoder.vel_estimate
current_setpoint = axis_ctx.handle.motor.current_control.Iq_setpoint
velocity1 = axis_ctx.handle.encoder.vel_estimate
velocity = ((velocity0 + velocity1) / 2)
# Abort immediately if the absolute limits are exceeded
test_assert_within(current_setpoint, -max_current, max_current)
test_assert_within(velocity, -absolute_max_vel,
absolute_max_vel)
return input_current, velocity, current_setpoint, get_expected_setpoint(
input_current, velocity)
axis_ctx.handle.controller.input_current = input_current = 0.0
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
# Move the system around its operating envelope
axis_ctx.handle.controller.input_current = input_current = 2.0
dataA = record_log(data_getter, duration=1.0)
axis_ctx.handle.controller.input_current = input_current = -2.0
dataA = np.concatenate(
[dataA, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_current = input_current = 4.0
dataA = np.concatenate(
[dataA, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_current = input_current = -4.0
dataA = np.concatenate(
[dataA, record_log(data_getter, duration=1.0)])
# Shrink the operating envelope while motor is moving faster than the envelope allows
max_rps = 5.0
max_vel = float(enc_ctx.yaml['cpr']) * max_rps
axis_ctx.handle.controller.config.vel_limit = max_vel
# Move the system around its operating envelope
axis_ctx.handle.controller.input_current = input_current = 2.0
dataB = record_log(data_getter, duration=1.0)
axis_ctx.handle.controller.input_current = input_current = -2.0
dataB = np.concatenate(
[dataB, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_current = input_current = 4.0
dataB = np.concatenate(
[dataB, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_current = input_current = -4.0
dataB = np.concatenate(
[dataB, record_log(data_getter, duration=1.0)])
# Try the shrink maneuver again at positive velocity
axis_ctx.handle.controller.config.vel_limit = 20.0 * float(
enc_ctx.yaml['cpr'])
axis_ctx.handle.controller.input_current = 4.0
time.sleep(0.5)
axis_ctx.handle.controller.config.vel_limit = max_vel
axis_ctx.handle.controller.input_current = input_current = 2.0
dataB = np.concatenate(
[dataB, record_log(data_getter, duration=1.0)])
test_assert_no_error(axis_ctx)
axis_ctx.handle.requested_state = 1
test_curve_fit(dataA[:, (0, 3)],
dataA[:, 4],
max_mean_err=0.02,
inlier_range=0.05,
max_outliers=len(dataA[:, 0] * 0.01))
test_curve_fit(dataB[:, (0, 3)],
dataB[:, 4],
max_mean_err=0.1,
inlier_range=0.2,
max_outliers=len(dataB[:, 0]) * 0.01)
def run_test(self, axis_ctx: ODriveAxisComponent,
motor_ctx: MotorComponent, enc_ctx: EncoderComponent,
logger: Logger):
with SafeTerminator(logger, axis_ctx):
nominal_rps = 15.0
max_current = 20.0
# Accept a bit of noise on Ibus
axis_ctx.parent.handle.config.dc_max_negative_current = -0.5
logger.debug(f'Brake control test from {nominal_rps} rounds/s...')
axis_ctx.handle.controller.config.vel_limit = 25.0
axis_ctx.handle.motor.config.current_lim = max_current
axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
# accelerate...
axis_ctx.handle.controller.input_vel = nominal_rps
time.sleep(1.0)
test_assert_no_error(axis_ctx)
# ... and brake
axis_ctx.handle.controller.input_vel = 0
time.sleep(1.0)
test_assert_no_error(axis_ctx)
logger.debug(f'Brake control test with brake resistor disabled')
# once more, but this time without brake resistor
axis_ctx.parent.handle.config.enable_brake_resistor = False
# accelerate...
axis_ctx.handle.controller.input_vel = nominal_rps
time.sleep(1.0)
test_assert_no_error(axis_ctx)
# ... and brake
axis_ctx.parent.handle.config.dc_max_negative_current = -0.2
axis_ctx.handle.controller.input_vel = 10 # this should fail almost instantaneously
time.sleep(0.1)
test_assert_eq(axis_ctx.parent.handle.error,
ODRIVE_ERROR_DC_BUS_OVER_REGEN_CURRENT)
test_assert_eq(
axis_ctx.handle.motor.error & MOTOR_ERROR_SYSTEM_LEVEL,
MOTOR_ERROR_SYSTEM_LEVEL)
test_assert_eq(axis_ctx.handle.error, 0)
# Do test again with wrong brake resistance setting
logger.debug(f'Brake control test with brake resistor = 100')
axis_ctx.parent.handle.clear_errors()
time.sleep(1.0)
axis_ctx.parent.handle.config.brake_resistance = 100
axis_ctx.parent.handle.config.dc_max_negative_current = -0.5
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
# accelerate...
axis_ctx.handle.controller.input_vel = nominal_rps
time.sleep(1.0)
test_assert_no_error(axis_ctx)
# ... and brake
axis_ctx.handle.controller.input_vel = 0
time.sleep(1.0) # expect DC_BUS_OVER_REGEN_CURRENT
time.sleep(0.1)
test_assert_eq(axis_ctx.parent.handle.error,
ODRIVE_ERROR_DC_BUS_OVER_REGEN_CURRENT)
test_assert_eq(
axis_ctx.handle.motor.error & MOTOR_ERROR_SYSTEM_LEVEL,
MOTOR_ERROR_SYSTEM_LEVEL)
test_assert_eq(axis_ctx.handle.error, 0)
def run_test(self, odrive: ODriveComponent, canbus: CanInterfaceComponent, node_id: int, extended_id: bool, logger: Logger):
# make sure no gpio input is overwriting our values
odrive.unuse_gpios()
axis = odrive.handle.axis0
axis.config.enable_watchdog = False
axis.clear_errors()
axis.config.can_node_id = node_id
axis.config.can_node_id_extended = extended_id
time.sleep(0.1)
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
test_assert_eq(my_req('get_vbus_voltage')['vbus_voltage'], odrive.handle.vbus_voltage, accuracy=0.01)
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.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.config.can_node_id, node_id)
# Check that extended node IDs are not carelessly projected to 6-bit IDs
extended_id = not extended_id
my_cmd('estop') # should not be accepted
extended_id = not extended_id
fence()
test_assert_eq(axis.error, AXIS_ERROR_NONE)
axis.encoder.set_linear_count(123)
test_assert_eq(my_req('get_encoder_estimates')['encoder_pos_estimate'], 123.0 / axis.encoder.config.cpr, accuracy=0.01)
test_assert_eq(my_req('get_encoder_count')['encoder_shadow_count'], 123.0, accuracy=0.01)
my_cmd('clear_errors')
fence()
test_assert_eq(axis.error, 0)
my_cmd('estop')
fence()
test_assert_eq(axis.error, AXIS_ERROR_ESTOP_REQUESTED)
my_cmd('set_requested_state', requested_state=42) # illegal state - should assert axis error
fence()
test_assert_eq(axis.current_state, 1) # idle
test_assert_eq(axis.error, AXIS_ERROR_ESTOP_REQUESTED | AXIS_ERROR_INVALID_STATE)
my_cmd('clear_errors')
fence()
test_assert_eq(axis.error, 0)
my_cmd('set_controller_modes', control_mode=1, input_mode=5) # current conrol, traprzoidal trajectory
fence()
test_assert_eq(axis.controller.config.control_mode, 1)
test_assert_eq(axis.controller.config.input_mode, 5)
# Reset to safe values
my_cmd('set_controller_modes', control_mode=3, input_mode=1) # position control, passthrough
fence()
test_assert_eq(axis.controller.config.control_mode, 3)
test_assert_eq(axis.controller.config.input_mode, 1)
axis.controller.input_pos = 1234
axis.controller.input_vel = 1234
axis.controller.input_torque = 1234
my_cmd('set_input_pos', input_pos=1.23, vel_ff=1.2, torque_ff=3.4)
fence()
test_assert_eq(axis.controller.input_pos, 1.23, range=0.1)
test_assert_eq(axis.controller.input_vel, 1.2, range=0.01)
test_assert_eq(axis.controller.input_torque, 3.4, range=0.001)
axis.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
my_cmd('set_input_vel', input_vel=-10.5, torque_ff=0.1234)
fence()
test_assert_eq(axis.controller.input_vel, -10.5, range=0.01)
test_assert_eq(axis.controller.input_torque, 0.1234, range=0.01)
axis.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL
my_cmd('set_input_torque', input_torque=0.1)
fence()
test_assert_eq(axis.controller.input_torque, 0.1, range=0.01)
my_cmd('set_velocity_limit', velocity_limit=2.345678)
fence()
test_assert_eq(axis.controller.config.vel_limit, 2.345678, range=0.001)
my_cmd('set_traj_vel_limit', traj_vel_limit=123.456)
fence()
test_assert_eq(axis.trap_traj.config.vel_limit, 123.456, range=0.0001)
my_cmd('set_traj_accel_limits', traj_accel_limit=98.231, traj_decel_limit=-12.234)
fence()
test_assert_eq(axis.trap_traj.config.accel_limit, 98.231, range=0.0001)
test_assert_eq(axis.trap_traj.config.decel_limit, -12.234, range=0.0001)
my_cmd('set_traj_inertia', inertia=55.086)
fence()
test_assert_eq(axis.controller.config.inertia, 55.086, range=0.0001)
# any CAN cmd will feed the watchdog
test_watchdog(axis, lambda: my_cmd('set_input_torque', input_torque=0.0), logger)
logger.debug('testing heartbeat...')
# note that this will include the heartbeats that were received during the
# watchdog test (which takes 4.8s).
heartbeats = asyncio.run(get_all(record_messages(canbus.handle, node_id, extended_id, 'heartbeat', timeout = 1.0)))
test_assert_eq(len(heartbeats), 5.8 / 0.1, accuracy=0.05)
test_assert_eq([msg['error'] for msg in heartbeats[0:35]], [0] * 35) # before watchdog expiry
test_assert_eq([msg['error'] for msg in heartbeats[-10:]], [AXIS_ERROR_WATCHDOG_TIMER_EXPIRED] * 10) # after watchdog expiry
test_assert_eq([msg['current_state'] for msg in heartbeats], [1] * len(heartbeats))
logger.debug('testing reboot...')
my_cmd('reboot')
time.sleep(0.5)
if len(odrive.handle._remote_attributes) != 0:
raise TestFailed("device didn't seem to reboot")
odrive.handle = None
time.sleep(2.0)
odrive.prepare(logger)
def run_test(self, enc: ODriveEncoderComponent, odrive_ncs_gpio: int, teensy: TeensyComponent, teensy_gpio_sck: TeensyGpio, teensy_gpio_miso: TeensyGpio, teensy_gpio_mosi: TeensyGpio, teensy_gpio_ncs: TeensyGpio, teensy_gpio_reset: TeensyGpio, reset_gpio: LinuxGpioComponent, logger: Logger):
true_cpr = 16384
true_rps = 1.0
reset_gpio.config(output=True) # hold encoder and disable its SPI
reset_gpio.write(True)
code = (teensy_spi_encoder_emulation_code
.replace("{sck}", str(teensy_gpio_sck.num))
.replace("{miso}", str(teensy_gpio_miso.num))
.replace("{mosi}", str(teensy_gpio_mosi.num))
.replace("{ncs}", str(teensy_gpio_ncs.num))
.replace("{reset}", str(teensy_gpio_reset.num))
.replace("{mode}", str(self.mode)))
teensy.compile_and_program(code)
logger.debug(f'Configuring absolute encoder in mode 0x{self.mode:x}...')
enc.handle.config.mode = self.mode
enc.handle.config.abs_spi_cs_gpio_pin = odrive_ncs_gpio
enc.handle.config.cpr = true_cpr
# Also put the other encoder into SPI mode to make it more interesting
other_enc = enc.parent.encoders[1 - enc.num]
other_enc.handle.config.mode = self.mode
other_enc.handle.config.abs_spi_cs_gpio_pin = odrive_ncs_gpio
other_enc.handle.config.cpr = true_cpr
enc.parent.save_config_and_reboot()
time.sleep(1.0)
logger.debug('Testing absolute readings and SPI errors...')
# Encoder is still disabled - expect recurring error
enc.handle.error = 0
time.sleep(0.002)
# This fails from time to time because the pull-up on the ODrive only manages
# to pull MISO to 1.8V, leaving it in the undefined range.
test_assert_eq(enc.handle.error, ENCODER_ERROR_ABS_SPI_COM_FAIL)
# Enable encoder and expect error to go away
reset_gpio.write(False)
release_time = time.monotonic()
enc.handle.error = 0
time.sleep(0.002)
test_assert_eq(enc.handle.error, 0)
# Check absolute position after 1.5s
time.sleep(1.5)
true_delta_t = time.monotonic() - release_time
test_assert_eq(enc.handle.pos_abs, (true_delta_t * true_rps * true_cpr) % true_cpr, range = true_cpr*0.001)
test_assert_eq(enc.handle.error, 0)
reset_gpio.write(True)
time.sleep(0.002)
test_assert_eq(enc.handle.error, ENCODER_ERROR_ABS_SPI_COM_FAIL)
reset_gpio.write(False)
release_time = time.monotonic()
enc.handle.error = 0
time.sleep(0.002)
test_assert_eq(enc.handle.error, 0)
# Check absolute position after 1.5s
time.sleep(1.5)
true_delta_t = time.monotonic() - release_time
test_assert_eq(enc.handle.pos_abs, (true_delta_t * true_rps * true_cpr) % true_cpr, range = true_cpr*0.001)
self.run_generic_encoder_test(enc.handle, true_cpr, true_rps)
enc.handle.config.cpr = 8192
def run_test(self, axis_ctx: ODriveAxisComponent,
motor_ctx: MotorComponent, enc_ctx: EncoderComponent,
logger: Logger):
logger.debug(f'Encoder {axis_ctx.num} was passed through')
except:
app_shutdown_token.set()
raise
finally:
# Release all conflicting axes
for axis_ctx in coupled_axes:
axis_ctx.lock.release()
for_all_parallel(couplings, lambda x: type(test).__name__ + " on " + "..".join([a.name for a in x]), dual_axis_test_thread)
else:
logger.warn("ignoring unknown test type {}".format(type(test)))
except:
logger.error(traceback.format_exc())
logger.debug('=> Test failed. Please wait while I secure the test rig...')
try:
dont_secure_after_failure = False # TODO: disable
if not dont_secure_after_failure:
def odrv_reset_thread(odrv_name):
odrv_ctx = odrives_by_name[odrv_name]
#run("make erase PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=30)
odrv_ctx.handle.axis0.requested_state = AXIS_STATE_IDLE
odrv_ctx.handle.axis1.requested_state = AXIS_STATE_IDLE
dump_errors(odrv_ctx.axes[0], logger)
dump_errors(odrv_ctx.axes[1], logger)
for_all_parallel(odrives_by_name, lambda x: x['name'], odrv_reset_thread)
except:
logger.error('///////////////////////////////////////////')
logger.error('/// CRITICAL: COULD NOT SECURE TEST RIG ///')
def run_test(self, odrive: ODriveComponent, port: SerialPortComponent, logger: Logger):
logger.debug('Enabling UART...')
# GPIOs might be in use by something other than UART and some components
# might be configured so that they would fail in the later test.
odrive.erase_config_and_reboot()
odrive.handle.config.enable_uart = True
with port.open(115200) as ser:
# reset port to known state
reset_state(ser)
# Read a top-level attribute
ser.write(b'r vbus_voltage\n')
response = float(ser.readline().strip())
test_assert_eq(response, odrive.handle.vbus_voltage, accuracy=0.1)
# Read an unknown attribute
ser.write(b'r blahblah\n')
response = ser.readline().strip()
test_assert_eq(response, b'invalid property')
# Send command with delays in between
for byte in b'r vbus_voltage\n':
ser.write([byte])
time.sleep(0.1)
response = float(ser.readline().strip())
test_assert_eq(response, odrive.handle.vbus_voltage, accuracy=0.1)
# Test GCode checksum and comments
ser.write(b'r vbus_voltage *12\n') # invalid checksum
test_assert_eq(ser.readline(), b'')
ser.write(append_checksum(b'r vbus_voltage ') + b' ; this is a comment\n') # valid checksum
response = float(strip_checksum(ser.readline()).strip())
test_assert_eq(response, odrive.handle.vbus_voltage, accuracy=0.1)
# Read an attribute with a long name
ser.write(b'r axis0.motor.current_control.v_current_control_integral_d\n')
response = float(ser.readline().strip())
test_assert_eq(response, odrive.handle.axis0.motor.current_control.v_current_control_integral_d, accuracy=0.1)
# Write an attribute
ser.write(b'w test_property 12345\n')
ser.write(b'r test_property\n')
response = int(ser.readline().strip())
test_assert_eq(response, 12345)
# Test 'c', 'v', 'p', 'q' and 'f' commands
odrive.handle.axis0.controller.input_current = 0
ser.write(b'c 0 12.5\n')
test_assert_eq(ser.readline(), b'')
test_assert_eq(odrive.handle.axis0.controller.input_current, 12.5, accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.control_mode, CTRL_MODE_CURRENT_CONTROL)
odrive.handle.axis0.controller.input_vel = 0
odrive.handle.axis0.controller.input_current = 0
ser.write(b'v 0 567.8 12.5\n')
test_assert_eq(ser.readline(), b'')
test_assert_eq(odrive.handle.axis0.controller.input_vel, 567.8, accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.input_current, 12.5, accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.control_mode, CTRL_MODE_VELOCITY_CONTROL)
odrive.handle.axis0.controller.input_pos = 0
odrive.handle.axis0.controller.input_vel = 0
odrive.handle.axis0.controller.input_current = 0
ser.write(b'p 0 123.4 567.8 12.5\n')
test_assert_eq(ser.readline(), b'')
test_assert_eq(odrive.handle.axis0.controller.input_pos, 123.4, accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.input_vel, 567.8, accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.input_current, 12.5, accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.control_mode, CTRL_MODE_POSITION_CONTROL)
odrive.handle.axis0.controller.input_pos = 0
odrive.handle.axis0.controller.config.vel_limit = 0
odrive.handle.axis0.motor.config.current_lim = 0
ser.write(b'q 0 123.4 567.8 12.5\n')
test_assert_eq(ser.readline(), b'')
test_assert_eq(odrive.handle.axis0.controller.input_pos, 123.4, accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.vel_limit, 567.8, accuracy=0.001)
test_assert_eq(odrive.handle.axis0.motor.config.current_lim, 12.5, accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.control_mode, CTRL_MODE_POSITION_CONTROL)
ser.write(b'f 0\n')
response = ser.readline().strip()
test_assert_eq(float(response.split()[0]), odrive.handle.axis0.encoder.pos_estimate, accuracy=0.001)
test_assert_eq(float(response.split()[1]), odrive.handle.axis0.encoder.vel_estimate, accuracy=0.001)
test_watchdog(odrive.handle.axis0, lambda: ser.write(b'u 0\n'), logger)
test_assert_eq(ser.readline(), b'') # check if the device remained silent during the test
def run_test(self, axis_ctx: ODriveAxisComponent,
motor_ctx: MotorComponent, enc_ctx: EncoderComponent,
logger: Logger):
with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger):
nominal_rps = 1.0
nominal_vel = float(enc_ctx.yaml['cpr']) * nominal_rps
logger.debug(
f'Testing closed loop velocity control at {nominal_rps} rounds/s...'
)
axis_ctx.handle.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH
axis_ctx.handle.controller.input_vel = 0
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
axis_ctx.handle.controller.input_vel = nominal_vel
data = record_log(lambda: [
axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.
pos_estimate
],
duration=5.0)
test_assert_eq(axis_ctx.handle.current_state,
AXIS_STATE_CLOSED_LOOP_CONTROL)
test_assert_no_error(axis_ctx)
request_state(axis_ctx, AXIS_STATE_IDLE)
# encoder.vel_estimate
slope, offset, fitted_curve = fit_line(data[:, (0, 1)])
test_assert_eq(slope, 0.0, range=nominal_vel * 0.02)
test_assert_eq(offset, nominal_vel, accuracy=0.05)
test_curve_fit(data[:, (0, 1)],
fitted_curve,
max_mean_err=nominal_vel * 0.3,
inlier_range=nominal_vel * 0.5,
max_outliers=len(data[:, 0]) * 0.1)
# encoder.pos_estimate
slope, offset, fitted_curve = fit_line(data[:, (0, 2)])
test_assert_eq(slope, nominal_vel, accuracy=0.01)
test_curve_fit(data[:, (0, 2)],
fitted_curve,
max_mean_err=nominal_vel * 0.01,
inlier_range=nominal_vel * 0.1,
max_outliers=len(data[:, 0]) * 0.01)
logger.debug(f'Testing closed loop position control...')
axis_ctx.handle.controller.config.control_mode = CTRL_MODE_POSITION_CONTROL
axis_ctx.handle.controller.input_pos = 0
axis_ctx.handle.controller.config.vel_limit = float(
enc_ctx.yaml['cpr']) * 5.0 # max 5 rps
axis_ctx.handle.encoder.set_linear_count(0)
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
# Test small position changes
axis_ctx.handle.controller.input_pos = 5000
time.sleep(0.3)
test_assert_no_error(axis_ctx)
test_assert_eq(
axis_ctx.handle.encoder.pos_estimate, 5000,
range=2000) # large range needed because of cogging torque
axis_ctx.handle.controller.input_pos = -5000
time.sleep(0.3)
test_assert_no_error(axis_ctx)
test_assert_eq(axis_ctx.handle.encoder.pos_estimate,
-5000,
range=2000)
axis_ctx.handle.controller.input_pos = 0
time.sleep(0.3)
nominal_vel = float(enc_ctx.yaml['cpr']) * 5.0
axis_ctx.handle.controller.input_pos = nominal_vel * 2.0 # 10 turns (takes 2 seconds)
# Test large position change with bounded velocity
data = record_log(lambda: [
axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.
pos_estimate
],
duration=4.0)
test_assert_eq(axis_ctx.handle.current_state,
AXIS_STATE_CLOSED_LOOP_CONTROL)
test_assert_no_error(axis_ctx)
request_state(axis_ctx, AXIS_STATE_IDLE)
data_motion = data[data[:, 0] < 1.9]
data_still = data[data[:, 0] > 2.1]
# encoder.vel_estimate
slope, offset, fitted_curve = fit_line(data_motion[:, (0, 1)])
test_assert_eq(slope, 0.0, range=nominal_vel * 0.05)
test_assert_eq(offset, nominal_vel, accuracy=0.05)
test_curve_fit(data_motion[:, (0, 1)],
fitted_curve,
max_mean_err=nominal_vel * 0.05,
inlier_range=nominal_vel * 0.1,
max_outliers=len(data[:, 0]) * 0.01)
# encoder.pos_estimate
slope, offset, fitted_curve = fit_line(data_motion[:, (0, 2)])
test_assert_eq(slope, nominal_vel, accuracy=0.01)
test_curve_fit(data_motion[:, (0, 2)],
fitted_curve,
max_mean_err=nominal_vel * 0.01,
inlier_range=nominal_vel * 0.1,
max_outliers=len(data[:, 0]) * 0.01)
# encoder.vel_estimate
slope, offset, fitted_curve = fit_line(data_still[:, (0, 1)])
test_assert_eq(slope, 0.0, range=nominal_vel * 0.05)
test_assert_eq(offset, 0.0, range=nominal_vel * 0.05)
test_curve_fit(data_still[:, (0, 1)],
fitted_curve,
max_mean_err=nominal_vel * 0.05,
inlier_range=nominal_vel * 0.1,
max_outliers=len(data[:, 0]) * 0.01)
# encoder.pos_estimate
slope, offset, fitted_curve = fit_line(data_still[:, (0, 2)])
test_assert_eq(slope, 0.0, range=nominal_vel * 0.05)
test_assert_eq(offset, nominal_vel * 2, range=nominal_vel * 0.02)
test_curve_fit(data_still[:, (0, 2)],
fitted_curve,
max_mean_err=nominal_vel * 0.01,
inlier_range=nominal_vel * 0.01,
max_outliers=len(data[:, 0]) * 0.01)
def run_test(self, odrive: ODriveComponent, canbus: CanInterfaceComponent,
node_id: int, extended_id: bool, logger: Logger):
odrive.disable_mappings()
if odrive.yaml['board-version'].startswith("v3."):
odrive.handle.config.gpio15_mode = GPIO_MODE_CAN_A
odrive.handle.config.gpio16_mode = GPIO_MODE_CAN_A
elif odrive.yaml['board-version'].startswith("v4."):
pass # CAN pin configuration is hardcoded
else:
raise Exception("unknown board version {}".format(
odrive.yaml['board-version']))
odrive.handle.config.enable_can_a = True
odrive.save_config_and_reboot()
axis = odrive.handle.axis0
axis.config.enable_watchdog = False
odrive.handle.clear_errors()
axis.config.can.node_id = node_id
axis.config.can.is_extended = extended_id
time.sleep(0.1)
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
def flush_rx():
while not canbus.handle.recv(timeout=0) is None:
pass
logger.debug('sending request...')
test_assert_eq(my_req('get_vbus_voltage')['vbus_voltage'],
odrive.handle.vbus_voltage,
accuracy=0.01)
my_cmd('set_node_id', node_id=node_id + 20)
time.sleep(0.1) # TODO: remove this hack (see note in firmware)
asyncio.run(
request(canbus.handle, node_id + 20, extended_id,
'get_vbus_voltage'))
test_assert_eq(axis.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.config.can.node_id, node_id)
# Check that extended node IDs are not carelessly projected to 6-bit IDs
extended_id = not extended_id
my_cmd('estop') # should not be accepted
extended_id = not extended_id
fence()
test_assert_eq(axis.error, AXIS_ERROR_NONE)
axis.encoder.set_linear_count(123)
flush_rx(
) # drop previous encoder estimates that were sent by the ODrive at a constant rate
test_assert_eq(my_req('get_encoder_estimates')['encoder_pos_estimate'],
123.0 / axis.encoder.config.cpr,
accuracy=0.01)
test_assert_eq(my_req('get_encoder_count')['encoder_shadow_count'],
123.0,
accuracy=0.01)
my_cmd('clear_errors')
fence()
test_assert_eq(axis.error, 0)
my_cmd('estop')
fence()
test_assert_eq(axis.error, AXIS_ERROR_ESTOP_REQUESTED)
my_cmd('set_requested_state',
requested_state=42) # illegal state - should assert axis error
fence()
test_assert_eq(axis.current_state, 1) # idle
test_assert_eq(axis.error,
AXIS_ERROR_ESTOP_REQUESTED | AXIS_ERROR_INVALID_STATE)
my_cmd('clear_errors')
fence()
test_assert_eq(axis.error, 0)
my_cmd('set_controller_modes', control_mode=1,
input_mode=5) # current conrol, traprzoidal trajectory
fence()
test_assert_eq(axis.controller.config.control_mode, 1)
test_assert_eq(axis.controller.config.input_mode, 5)
# Reset to safe values
my_cmd('set_controller_modes', control_mode=3,
input_mode=1) # position control, passthrough
fence()
test_assert_eq(axis.controller.config.control_mode, 3)
test_assert_eq(axis.controller.config.input_mode, 1)
axis.controller.input_pos = 1234
axis.controller.input_vel = 1234
axis.controller.input_torque = 1234
my_cmd('set_input_pos', input_pos=1.23, vel_ff=1.2, torque_ff=3.4)
fence()
test_assert_eq(axis.controller.input_pos, 1.23, range=0.1)
test_assert_eq(axis.controller.input_vel, 1.2, range=0.01)
test_assert_eq(axis.controller.input_torque, 3.4, range=0.001)
axis.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
my_cmd('set_input_vel', input_vel=-10.5, torque_ff=0.1234)
fence()
test_assert_eq(axis.controller.input_vel, -10.5, range=0.01)
test_assert_eq(axis.controller.input_torque, 0.1234, range=0.01)
axis.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL
my_cmd('set_input_torque', input_torque=0.1)
fence()
test_assert_eq(axis.controller.input_torque, 0.1, range=0.01)
my_cmd('set_velocity_limit', velocity_limit=2.345678)
fence()
test_assert_eq(axis.controller.config.vel_limit, 2.345678, range=0.001)
my_cmd('set_traj_vel_limit', traj_vel_limit=123.456)
fence()
test_assert_eq(axis.trap_traj.config.vel_limit, 123.456, range=0.0001)
my_cmd('set_traj_accel_limits',
traj_accel_limit=98.231,
traj_decel_limit=-12.234)
fence()
test_assert_eq(axis.trap_traj.config.accel_limit, 98.231, range=0.0001)
test_assert_eq(axis.trap_traj.config.decel_limit,
-12.234,
range=0.0001)
my_cmd('set_traj_inertia', inertia=55.086)
fence()
test_assert_eq(axis.controller.config.inertia, 55.086, range=0.0001)
# any CAN cmd will feed the watchdog
test_watchdog(axis,
lambda: my_cmd('set_input_torque', input_torque=0.0),
logger)
logger.debug('testing heartbeat...')
flush_rx(
) # Flush RX buffer to get a clean state for the heartbeat test
heartbeats = asyncio.run(
get_all(
record_messages(canbus.handle,
node_id,
extended_id,
'heartbeat',
timeout=2.0)))
test_assert_eq(len(heartbeats), 2.0 / 0.1, accuracy=0.05)
test_assert_eq([msg['error'] for msg in heartbeats],
[AXIS_ERROR_WATCHDOG_TIMER_EXPIRED] * len(heartbeats))
test_assert_eq([msg['current_state'] for msg in heartbeats],
[1] * len(heartbeats))
logger.debug('testing reboot...')
test_assert_eq(odrive.handle._on_lost.done(), False)
my_cmd('reboot')
time.sleep(0.5)
if not odrive.handle._on_lost is None:
raise TestFailed("device didn't seem to reboot")
odrive.handle = None
time.sleep(2.0)
odrive.prepare(logger)
def run_test(self, axis_ctx: ODriveAxisComponent,
motor_ctx: MotorComponent, enc_ctx: EncoderComponent,
logger: Logger):
with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger):
max_rps = 15.0
max_vel = max_rps
max_current = 30.0
max_torque = 0.1 # must be less than max_current * torque_constant.
torque_constant = axis_ctx.handle.motor.config.torque_constant
test_pos = 5
test_vel = 10
test_torque = 0.5
axis_ctx.handle.controller.config.vel_limit = max_vel
axis_ctx.handle.motor.config.current_lim = max_current
axis_ctx.handle.motor.config.torque_lim = inf #disable torque limit
axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
def data_getter():
current_setpoint = axis_ctx.handle.motor.current_control.Iq_setpoint
torque_setpoint = current_setpoint * torque_constant
torque_limit = axis_ctx.handle.motor.config.torque_lim
# Abort immediately if the absolute limits are exceeded
test_assert_within(current_setpoint, -max_current, max_current)
test_assert_within(torque_setpoint, -torque_limit,
torque_limit)
return max_current, current_setpoint, torque_limit, torque_setpoint
# begin test
axis_ctx.handle.motor.config.torque_lim = max_torque
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
# step input positions
logger.debug('input_pos step test')
axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL
axis_ctx.handle.controller.input_pos = test_pos
dataPos = record_log(data_getter, duration=1.0)
axis_ctx.handle.controller.input_pos = -test_pos
dataPos = np.concatenate(
[dataPos, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_pos = test_pos
dataPos = np.concatenate(
[dataPos, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_pos = -test_pos
dataPos = np.concatenate(
[dataPos, record_log(data_getter, duration=1.0)])
time.sleep(0.5)
test_assert_no_error(axis_ctx)
# step input velocities
logger.debug('input_vel step test')
axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
axis_ctx.handle.controller.input_vel = test_vel
dataVel = record_log(data_getter, duration=1.0)
axis_ctx.handle.controller.input_vel = -test_vel
dataVel = np.concatenate(
[dataVel, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_vel = test_vel
dataVel = np.concatenate(
[dataVel, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_vel = -test_vel
dataVel = np.concatenate(
[dataVel, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_vel = 0
time.sleep(0.5)
# step input torques
logger.debug('input_torque step test')
axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL
axis_ctx.handle.controller.input_torque = test_torque
dataTq = record_log(data_getter, duration=1.0)
axis_ctx.handle.controller.input_torque = -test_torque
dataTq = np.concatenate(
[dataTq, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_torque = test_torque
dataTq = np.concatenate(
[dataTq, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_torque = -test_torque
dataTq = np.concatenate(
[dataTq, record_log(data_getter, duration=1.0)])
axis_ctx.handle.controller.input_torque = 0
time.sleep(0.5)
# did we pass?
test_assert_no_error(axis_ctx)
axis_ctx.handle.requested_state = 1
def run_test(self, odrive: ODriveComponent, uart_num: int, tx_gpio: list,
rx_gpio: list, port: SerialPortComponent, logger: Logger):
self.prepare(odrive, uart_num, tx_gpio, rx_gpio, logger)
with port.open(115200) as ser:
# reset port to known state
reset_state(ser)
# Read a top-level attribute
ser.write(b'r vbus_voltage\n')
response = float(ser.readline().strip())
test_assert_eq(response, odrive.handle.vbus_voltage, accuracy=0.1)
# Read an unknown attribute
ser.write(b'r blahblah\n')
response = ser.readline().strip()
test_assert_eq(response, b'invalid property')
# Send command with delays in between
for byte in b'r vbus_voltage\n':
ser.write([byte])
time.sleep(0.1)
response = float(ser.readline().strip())
test_assert_eq(response, odrive.handle.vbus_voltage, accuracy=0.1)
# Test GCode checksum and comments
ser.write(b'r vbus_voltage *12\n') # invalid checksum
test_assert_eq(ser.readline(), b'')
cmd = append_checksum(
b'r vbus_voltage ') + b' ; this is a comment\n'
# cmd evalutates to "r vbus_voltage *93 ; this is a comment"
logger.debug(f'sending command with checksum: "{cmd}"')
ser.write(cmd) # valid checksum
response = float(strip_checksum(ser.readline()).strip())
test_assert_eq(response, odrive.handle.vbus_voltage, accuracy=0.1)
# Read an attribute with a long name
ser.write(
b'r axis0.motor.current_control.v_current_control_integral_d\n'
)
response = float(ser.readline().strip())
test_assert_eq(response,
odrive.handle.axis0.motor.current_control.
v_current_control_integral_d,
accuracy=0.1)
# Write an attribute
ser.write(b'w test_property 12345\n')
ser.write(b'r test_property\n')
response = int(ser.readline().strip())
test_assert_eq(response, 12345)
# Test custom setter (aka property write hook)
odrive.handle.axis0.motor.config.phase_resistance = 1
odrive.handle.axis0.motor.config.phase_inductance = 1
odrive.handle.axis0.motor.config.current_control_bandwidth = 1000
old_gain = odrive.handle.axis0.motor.current_control.p_gain
test_assert_eq(old_gain, 1000, accuracy=0.0001
) # must be non-zero for subsequent check to work
ser.write(
'w axis0.motor.config.current_control_bandwidth {}\n'.format(
odrive.handle.axis0.motor.config.current_control_bandwidth
/ 2).encode('ascii'))
test_assert_eq(ser.readline(), b'')
test_assert_eq(odrive.handle.axis0.motor.current_control.p_gain,
old_gain / 2,
accuracy=0.0001)
# Test 'c', 'v', 'p', 'q' and 'f' commands
odrive.handle.axis0.controller.input_torque = 0
ser.write(b'c 0 12.5\n')
test_assert_eq(ser.readline(), b'')
test_assert_eq(odrive.handle.axis0.controller.input_torque,
12.5,
accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.control_mode,
CONTROL_MODE_TORQUE_CONTROL)
odrive.handle.axis0.controller.input_vel = 0
odrive.handle.axis0.controller.input_torque = 0
ser.write(b'v 0 567.8 12.5\n')
test_assert_eq(ser.readline(), b'')
test_assert_eq(odrive.handle.axis0.controller.input_vel,
567.8,
accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.input_torque,
12.5,
accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.control_mode,
CONTROL_MODE_VELOCITY_CONTROL)
odrive.handle.axis0.controller.input_pos = 0
odrive.handle.axis0.controller.input_vel = 0
odrive.handle.axis0.controller.input_torque = 0
ser.write(b'p 0 123.4 567.8 12.5\n')
test_assert_eq(ser.readline(), b'')
test_assert_eq(odrive.handle.axis0.controller.input_pos,
123.4,
accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.input_vel,
567.8,
accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.input_torque,
12.5,
accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.control_mode,
CONTROL_MODE_POSITION_CONTROL)
odrive.handle.axis0.controller.input_pos = 0
odrive.handle.axis0.controller.config.vel_limit = 0
odrive.handle.axis0.motor.config.current_lim = 0
ser.write(b'q 0 123.4 567.8 12.5\n')
test_assert_eq(ser.readline(), b'')
test_assert_eq(odrive.handle.axis0.controller.input_pos,
123.4,
accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.vel_limit,
567.8,
accuracy=0.001)
test_assert_eq(odrive.handle.axis0.motor.config.torque_lim,
12.5,
accuracy=0.001)
test_assert_eq(odrive.handle.axis0.controller.config.control_mode,
CONTROL_MODE_POSITION_CONTROL)
ser.write(b'f 0\n')
response = ser.readline().strip()
test_assert_eq(float(response.split()[0]),
odrive.handle.axis0.encoder.pos_estimate,
accuracy=0.001)
test_assert_eq(float(response.split()[1]),
odrive.handle.axis0.encoder.vel_estimate,
accuracy=0.001)
test_watchdog(odrive.handle.axis0, lambda: ser.write(b'u 0\n'),
logger)
test_assert_eq(
ser.readline(),
b'') # check if the device remained silent during the test