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 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 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_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 find_all_odrives(self,
path="usb",
serial_number=None,
search_cancellation_token=None,
channel_termination_token=None,
timeout=30,
logger=Logger(verbose=True)):
"""
Blocks until timeout
"""
result = []
done_signal = Event(search_cancellation_token)
self.drive_count = 0
def did_discover_object(obj):
result.append(obj)
self.drive_count += 1
odrive.find_all(path, serial_number, did_discover_object, done_signal,
channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
except TimeoutError:
print("Timeouted")
finally:
done_signal.set() # terminate find_all
return result
def find_any(path="usb", serial_number=None,
search_cancellation_token=None, channel_termination_token=None,
timeout=None, logger=Logger(verbose=False), find_multiple=False):
"""
Blocks until the first matching Fibre object is connected and then returns that object
"""
result = []
done_signal = Event(search_cancellation_token)
def did_discover_object(obj):
result.append(obj)
if find_multiple:
if len(result) >= int(find_multiple):
done_signal.set()
else:
done_signal.set()
find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
except TimeoutError:
if not find_multiple:
return None
finally:
done_signal.set() # terminate find_all
if find_multiple:
return result
else:
return result[0] if len(result) > 0 else None
def get_odrives(self):
self.odrvs = [None, None, None]
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in self.SERIAL_NUMS:
self.odrvs[self.SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(
self.SERIAL_NUMS.index(obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in self.odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None,
Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
rospy.loginfo("Found ODrives")
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 find_any(path="usb", serial_number=None,
search_cancellation_token=None, channel_termination_token=None,
timeout=None, logger=Logger(verbose=False)):
"""
Blocks until the first matching Fibre node is connected and then returns that node
"""
result = [ None ]
done_signal = Event(search_cancellation_token)
def did_discover_object(obj):
result[0] = obj
done_signal.set()
find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
finally:
done_signal.set() # terminate find_all
return result[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)
import json
import logging
import threading
import fibre
from fibre.utils import Logger
logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)
fibre_logger = Logger(verbose=logger.getEffectiveLevel() == logging.DEBUG)
class OdriveManager:
_CONFIG_KEY = 'config'
_CRC_KEY = 'crc16'
def __init__(self, path, serial_number, cache_file=None):
self.channel = None
self.path = path
self.serial_number = serial_number
self.cache_file = cache_file or 'odrive-cache-{serial_number}.json' \
.format(serial_number=serial_number)
def setup_odrive(self, config, crc):
"""
Set up fibre remote object using given configuration.
"""
json_data = {"name": "fibre_node", "members": config}
self.channel._interface_definition_crc = crc
# Just some black magic
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
"Firmware", "fibre", "python", "fibre")
fibre_link = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"fibre")
if not os.path.exists(fibre_link):
if sys.version_info > (3, 3):
os.symlink(fibre_src, fibre_link, target_is_directory=True)
else:
os.symlink(fibre_src, fibre_link)
# TODO: find a better place for this
if not creating_package:
import platform
if platform.system() == 'Linux':
from fibre.utils import Logger
try:
odrive.version.setup_udev_rules(Logger())
except Exception:
print(
"Warning: could not set up udev rules. Run `sudo odrivetool udev-setup` to try again."
)
try:
setup(
name='odrive',
packages=['odrive', 'odrive.dfuse', 'fibre'],
scripts=['odrivetool', 'odrivetool.bat', 'odrive_demo.py'],
version=version,
description=
'Control utilities for the ODrive high performance motor controller',
author='Oskar Weigl',
author_email='*****@*****.**',
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 __init__(self, timeout):
# specify left, middle, and right ODrives
rospy.loginfo("Looking for ODrives...")
self.SERIAL_NUMS = [
35593293288011, # Left, 0
35623406809166, # Middle, 1
35563278839886] # Right, 2
self.odrvs = [
None,
None,
None]
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in self.SERIAL_NUMS:
self.odrvs[self.SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(self.SERIAL_NUMS.index(obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in self.odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None, Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
# self.odrv0 = odrive.find_any()
# # odrv1 = odrive.find_any()
# # odrv2 = odrive.find_any()
rospy.loginfo("Found ODrives")
# Set left and right axis
self.leftAxes = [self.odrvs[0].axis0, self.odrvs[0].axis1, self.odrvs[1].axis1]
self.rightAxes = [self.odrvs[1].axis0, self.odrvs[2].axis0, self.odrvs[2].axis1]
self.axes = self.leftAxes + self.rightAxes
# Set axis state
rospy.logdebug("Enabling Watchdog")
for ax in (self.leftAxes + self.rightAxes):
ax.watchdog_feed()
ax.config.watchdog_timeout = 2
ax.encoder.config.ignore_illegal_hall_state = True
# Clear errors
for odrv in self.odrvs:
dump_errors(odrv, True)
for ax in (self.leftAxes + self.rightAxes):
ax.controller.vel_ramp_enable = True
ax.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
ax.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
# Sub to topic
rospy.Subscriber('joy', Joy, self.vel_callback)
# Set first watchdog
self.timeout = timeout # log error if this many seconds occur between received messages
self.timer = rospy.Timer(rospy.Duration(self.timeout), self.watchdog_callback, oneshot=True)
self.watchdog_fired = False
# Init other variables
self.last_msg_time = 0
self.last_recv_time = 0
self.next_wd_feed_time = 0
rospy.loginfo("Ready for topic")
rospy.spin()
odrvs = [None]
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in SERIAL_NUMS:
odrvs[SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(SERIAL_NUMS.index(obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None, Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
# Which odrives
if args.which_odrive == None:
to_calib = odrvs
else:
to_calib = [odrvs[args.which_odrive]]
for odrv in to_calib:
odrv.config.brake_resistance = 0.5
print("Calibrating ODrive # {}".format(to_calib.index(odrv)))
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, 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):
logger.debug(f'Encoder {axis_ctx.num} was passed through')
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in SERIAL_NUMS:
odrvs[SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(SERIAL_NUMS.index(
obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None,
Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
# Which odrives
if args.which_odrive == None:
to_calib = odrvs
else:
to_calib = [odrvs[args.which_odrive]]
for odrv in to_calib:
odrv.config.brake_resistance = 0.5
print("Calibrating ODrive # {}".format(to_calib.index(odrv)))
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 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, 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)
if test_rig_yaml['type'] == 'parallel':
#all_tests.append(TestHighVelocity())
all_tests.append(TestHighVelocityInViscousFluid(load_current=35, driver_current=45))
# all_tests.append(TestVelCtrlVsPosCtrl())
# TODO: test step/dir
# TODO: test sensorless
# TODO: test ASCII protocol
# TODO: test protocol over UART
elif test_rig_yaml['type'] == 'loopback':
all_tests.append(TestSelfLoadedPosVelDistribution(
rpm_range=3000, load_current_range=60, driver_current_lim=70))
print(str(args.ignore))
logger = Logger()
os.chdir(script_path + '/../Firmware')
# 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)
if not name in args.ignore:
odrives_by_name[name] = ODriveTestContext(name, odrv_yaml)
# Build a dictionary of axis test contexts by name (e.g. odrive0.axis0)
axes_by_name = {}
for odrv_ctx in odrives_by_name.values():
for axis_idx, axis_ctx in enumerate(odrv_ctx.axes):
if not axis_ctx.name in args.ignore:
fibre_src = os.path.join(
os.path.dirname(os.path.dirname(os.path.realpath(__file__))),
"Firmware", "fibre", "python", "fibre")
fibre_link = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"fibre")
if not os.path.exists(fibre_link):
os.symlink(fibre_src, fibre_link, True)
# TODO: find a better place for this
if not creating_package:
import platform
if platform.system() == 'Linux':
import odrive.utils
from fibre.utils import Logger
try:
odrive.utils.setup_udev_rules(Logger())
except PermissionError:
print(
"Warning: could not set up udev rules. Run `sudo odrivetool udev-setup` to try again."
)
setup(
name='odrive',
packages=['odrive', 'odrive.dfuse', 'fibre'],
scripts=['odrivetool', 'odrivetool.bat', 'odrive_demo.py'],
version=version,
description=
'Control utilities for the ODrive high performance motor controller',
author='Oskar Weigl',
author_email='*****@*****.**',
license='MIT',
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
