*
This commit is contained in:
@@ -0,0 +1,113 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import time
|
||||
import math
|
||||
import os
|
||||
import numpy as np
|
||||
|
||||
from odrive.enums import *
|
||||
from test_runner import *
|
||||
|
||||
|
||||
teensy_code_template = """
|
||||
void setup() {
|
||||
analogWriteResolution(10);
|
||||
// base clock of the PWM timer is 150MHz (on Teensy 4.0)
|
||||
int freq = 150000000/1024; // ~146.5kHz PWM frequency
|
||||
analogWriteFrequency({analog_out}, freq);
|
||||
|
||||
// for filtering, assuming we have a 150 Ohm resistor, we need a capacitor of
|
||||
// 1/(150000000/1024)*2*pi/150 = 2.85954744646751e-07 F, that's ~0.33uF
|
||||
|
||||
//pinMode({lpf_enable}, OUTPUT);
|
||||
}
|
||||
|
||||
int i = 0;
|
||||
void loop() {
|
||||
i++;
|
||||
i = i & 0x3ff;
|
||||
if (digitalRead({analog_reset}))
|
||||
i = 0;
|
||||
analogWrite({analog_out}, i);
|
||||
delay(1);
|
||||
}
|
||||
"""
|
||||
|
||||
|
||||
class TestAnalogInput():
|
||||
"""
|
||||
Verifies the Analog input.
|
||||
|
||||
The Teensy generates a PWM signal with a duty cycle that follows a sawtooth signal
|
||||
with a period of 1 second. The signal should be connected to the ODrive's
|
||||
analog input through a low-pass-filter.
|
||||
|
||||
___ ___
|
||||
Teensy PWM ----|___|-------o---------|___|----- ODrive Analog Input
|
||||
150 Ohm | 150 Ohm
|
||||
===
|
||||
| 330nF
|
||||
|
|
||||
GND
|
||||
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for odrive_gpio_num, odrive_gpio in [(2, odrive.gpio3), (3, odrive.gpio4)]:
|
||||
analog_out_options = []
|
||||
lpf_gpio = [gpio for lpf in testrig.get_connected_components(odrive_gpio, LowPassFilterComponent)
|
||||
for gpio in testrig.get_connected_components(lpf.en, LinuxGpioComponent)]
|
||||
for teensy_gpio in testrig.get_connected_components(odrive_gpio, TeensyGpio):
|
||||
teensy = teensy_gpio.parent
|
||||
analog_reset_options = []
|
||||
for gpio in teensy.gpios:
|
||||
for local_gpio in testrig.get_connected_components(gpio, LinuxGpioComponent):
|
||||
analog_reset_options.append((gpio, local_gpio))
|
||||
analog_out_options.append((teensy, teensy_gpio, analog_reset_options))
|
||||
yield (odrive, lpf_gpio, odrive_gpio_num, analog_out_options)
|
||||
|
||||
|
||||
def run_test(self, odrive: ODriveComponent, lpf_enable: LinuxGpioComponent, analog_in_num: int, teensy: TeensyComponent, teensy_analog_out: Component, teensy_analog_reset: Component, analog_reset_gpio: LinuxGpioComponent, logger: Logger):
|
||||
code = teensy_code_template.replace("{analog_out}", str(teensy_analog_out.num)).replace("{analog_reset}", str(teensy_analog_reset.num)) #.replace("lpf_enable", str(lpf_enable.num))
|
||||
teensy.compile_and_program(code)
|
||||
analog_reset_gpio.config(output=True)
|
||||
analog_reset_gpio.write(True)
|
||||
lpf_enable.config(output=True)
|
||||
lpf_enable.write(False)
|
||||
|
||||
logger.debug("Set up analog input...")
|
||||
|
||||
min_val = -20000
|
||||
max_val = 20000
|
||||
period = 1.025 # period in teensy code is 1s, but due to tiny overhead it's a bit longer
|
||||
|
||||
analog_mapping = [
|
||||
None, #odrive.handle.config.gpio1_analog_mapping,
|
||||
None, #odrive.handle.config.gpio2_analog_mapping,
|
||||
odrive.handle.config.gpio3_analog_mapping,
|
||||
odrive.handle.config.gpio4_analog_mapping,
|
||||
None, #odrive.handle.config.gpio5_analog_mapping,
|
||||
][analog_in_num]
|
||||
|
||||
odrive.unuse_gpios()
|
||||
analog_mapping.endpoint = odrive.handle.axis0.controller._remote_attributes['input_pos']
|
||||
analog_mapping.min = min_val
|
||||
analog_mapping.max = max_val
|
||||
odrive.save_config_and_reboot()
|
||||
|
||||
analog_reset_gpio.write(False)
|
||||
data = record_log(lambda: [odrive.handle.axis0.controller.input_pos], duration=5.0)
|
||||
|
||||
# Expect mean error to be at most 2% (of the full scale).
|
||||
# Expect there to be less than 2% outliers, where an outlier is anything that is more than 5% (of full scale) away from the expected value.
|
||||
full_range = abs(max_val - min_val)
|
||||
slope, offset, fitted_curve = fit_sawtooth(data, min_val, max_val, sigma=30)
|
||||
test_assert_eq(slope, (max_val - min_val) / period, accuracy=0.005)
|
||||
test_curve_fit(data, fitted_curve, max_mean_err = full_range * 0.02, inlier_range = full_range * 0.05, max_outliers = len(data[:,0]) * 0.02)
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run(TestAnalogInput())
|
||||
@@ -0,0 +1,241 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import time
|
||||
from math import pi
|
||||
import os
|
||||
|
||||
from fibre.utils import Logger
|
||||
from test_runner import *
|
||||
from odrive.enums import *
|
||||
|
||||
|
||||
class TestMotorCalibration():
|
||||
"""
|
||||
Runs the motor calibration (phase inductance and phase resistance measurement)
|
||||
and checks if the measurements match the expectation.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
"""Returns all axes that are connected to a motor, along with the corresponding motor(s)"""
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for axis in odrive.axes:
|
||||
for motor in testrig.get_connected_components(axis, MotorComponent):
|
||||
yield (axis, motor)
|
||||
|
||||
def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, logger: Logger):
|
||||
# reset old calibration values
|
||||
|
||||
if axis_ctx.handle.encoder.config.mode != ENCODER_MODE_INCREMENTAL:
|
||||
axis_ctx.handle.encoder.config.mode = ENCODER_MODE_INCREMENTAL
|
||||
axis_ctx.parent.save_config_and_reboot()
|
||||
|
||||
axis_ctx.handle.motor.config.phase_resistance = 0.0
|
||||
axis_ctx.handle.motor.config.phase_inductance = 0.0
|
||||
axis_ctx.handle.motor.config.pre_calibrated = False
|
||||
axis_ctx.handle.config.enable_watchdog = False
|
||||
|
||||
axis_ctx.handle.clear_errors()
|
||||
|
||||
# run calibration
|
||||
request_state(axis_ctx, AXIS_STATE_MOTOR_CALIBRATION)
|
||||
time.sleep(6)
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
# check if measurements match expectation
|
||||
test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, float(motor_ctx.yaml['phase-resistance']), accuracy=0.2)
|
||||
test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, float(motor_ctx.yaml['phase-inductance']), accuracy=0.5)
|
||||
test_assert_eq(axis_ctx.handle.motor.is_calibrated, True)
|
||||
|
||||
|
||||
class TestDisconnectedMotorCalibration():
|
||||
"""
|
||||
Tests if the motor calibration fails as expected if the phases are floating.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
"""Returns all axes that are disconnected"""
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for axis in odrive.axes:
|
||||
if axis.yaml == 'floating':
|
||||
yield (axis,)
|
||||
|
||||
def run_test(self, axis_ctx: ODriveAxisComponent, logger: Logger):
|
||||
axis = axis_ctx.handle
|
||||
|
||||
# reset old calibration values
|
||||
axis_ctx.handle.motor.config.phase_resistance = 0.0
|
||||
axis_ctx.handle.motor.config.phase_inductance = 0.0
|
||||
axis_ctx.handle.motor.config.pre_calibrated = False
|
||||
|
||||
axis_ctx.handle.clear_errors()
|
||||
|
||||
# run test
|
||||
request_state(axis_ctx, AXIS_STATE_MOTOR_CALIBRATION)
|
||||
time.sleep(6)
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_MOTOR_FAILED)
|
||||
test_assert_eq(axis_ctx.handle.motor.error, MOTOR_ERROR_PHASE_RESISTANCE_OUT_OF_RANGE)
|
||||
|
||||
|
||||
class TestEncoderDirFind():
|
||||
"""
|
||||
Runs the encoder index search.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for num in range(2):
|
||||
encoders = testrig.get_connected_components({
|
||||
'a': (odrive.encoders[num].a, False),
|
||||
'b': (odrive.encoders[num].b, False)
|
||||
}, EncoderComponent)
|
||||
motors = testrig.get_connected_components(odrive.axes[num], MotorComponent)
|
||||
|
||||
for motor, encoder in itertools.product(motors, encoders):
|
||||
if encoder.impl in testrig.get_connected_components(motor):
|
||||
yield (odrive.axes[num], motor, encoder)
|
||||
|
||||
def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger):
|
||||
axis = axis_ctx.handle
|
||||
time.sleep(1.0) # wait for PLLs to stabilize
|
||||
|
||||
# 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.config.calibration_lockin.vel = 12.566 # 2 electrical revolutions per second
|
||||
|
||||
axis_ctx.handle.clear_errors()
|
||||
|
||||
# run test
|
||||
request_state(axis_ctx, AXIS_STATE_ENCODER_DIR_FIND)
|
||||
|
||||
time.sleep(4) # actual calibration takes 3 seconds
|
||||
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
test_assert_eq(axis_ctx.handle.motor.config.direction in [-1, 1], True)
|
||||
|
||||
|
||||
class TestEncoderOffsetCalibration():
|
||||
"""
|
||||
Runs the encoder index search.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for num in range(2):
|
||||
encoders = testrig.get_connected_components({
|
||||
'a': (odrive.encoders[num].a, False),
|
||||
'b': (odrive.encoders[num].b, False)
|
||||
}, EncoderComponent)
|
||||
motors = testrig.get_connected_components(odrive.axes[num], MotorComponent)
|
||||
|
||||
for motor, encoder in itertools.product(motors, encoders):
|
||||
if encoder.impl in testrig.get_connected_components(motor):
|
||||
yield (odrive.axes[num], motor, encoder)
|
||||
|
||||
def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger):
|
||||
axis = axis_ctx.handle
|
||||
time.sleep(1.0) # wait for PLLs to stabilize
|
||||
|
||||
# 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.clear_errors()
|
||||
|
||||
# run test
|
||||
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)
|
||||
|
||||
test_assert_eq(axis_ctx.handle.encoder.is_ready, True)
|
||||
test_assert_eq(axis_ctx.handle.motor.config.direction in [-1, 1], True)
|
||||
|
||||
|
||||
class TestEncoderIndexSearch():
|
||||
"""
|
||||
Runs the encoder index search.
|
||||
The index pin is triggered manually after three seconds from the testbench
|
||||
host's GPIO.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for num in range(2):
|
||||
encoders = testrig.get_connected_components({
|
||||
'a': (odrive.encoders[num].a, False),
|
||||
'b': (odrive.encoders[num].b, False)
|
||||
}, EncoderComponent)
|
||||
motors = testrig.get_connected_components(odrive.axes[num], MotorComponent)
|
||||
z_gpio = list(testrig.get_connected_components((odrive.encoders[num].z, False), LinuxGpioComponent))
|
||||
|
||||
for motor, encoder in itertools.product(motors, encoders):
|
||||
if encoder.impl in testrig.get_connected_components(motor):
|
||||
yield (odrive.axes[num], motor, encoder, z_gpio)
|
||||
|
||||
def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, z_gpio: LinuxGpioComponent, logger: Logger):
|
||||
axis = axis_ctx.handle
|
||||
cpr = int(enc_ctx.yaml['cpr'])
|
||||
|
||||
z_gpio.config(output=True)
|
||||
z_gpio.write(False)
|
||||
|
||||
time.sleep(1.0) # wait for PLLs to stabilize
|
||||
|
||||
# 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.config.calibration_lockin.vel = 12.566 # 2 electrical revolutions per second
|
||||
|
||||
axis_ctx.handle.clear_errors()
|
||||
|
||||
# run test
|
||||
request_state(axis_ctx, AXIS_STATE_ENCODER_INDEX_SEARCH)
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
test_assert_eq(axis_ctx.handle.encoder.index_found, False)
|
||||
time.sleep(0.1)
|
||||
z_gpio.write(True)
|
||||
test_assert_eq(axis_ctx.handle.encoder.index_found, True)
|
||||
z_gpio.write(False)
|
||||
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
test_assert_eq(axis_ctx.handle.encoder.shadow_count, 0.0, range=50)
|
||||
test_assert_eq(modpm(axis_ctx.handle.encoder.count_in_cpr, cpr), 0.0, range=50)
|
||||
test_assert_eq(axis_ctx.handle.encoder.pos_estimate, 0.0, range=50)
|
||||
test_assert_eq(modpm(axis_ctx.handle.encoder.pos_cpr, cpr), 0.0, range=50)
|
||||
test_assert_eq(axis_ctx.handle.encoder.pos_abs, 0.0, range=50)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run([
|
||||
TestMotorCalibration(),
|
||||
TestDisconnectedMotorCalibration(),
|
||||
TestEncoderDirFind(),
|
||||
TestEncoderOffsetCalibration(),
|
||||
TestEncoderIndexSearch()
|
||||
])
|
||||
@@ -0,0 +1,235 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import struct
|
||||
import can
|
||||
import asyncio
|
||||
import time
|
||||
import math
|
||||
|
||||
from fibre.utils import Logger
|
||||
from odrive.enums import *
|
||||
from test_runner import *
|
||||
|
||||
# Each argument is described as tuple (name, format, scale).
|
||||
# Struct format codes: https://docs.python.org/2/library/struct.html
|
||||
command_set = {
|
||||
'heartbeat': (0x001, [('error', 'I', 1), ('current_state', 'I', 1)]), # tested
|
||||
'estop': (0x002, []), # tested
|
||||
'get_motor_error': (0x003, [('motor_error', 'I', 1)]), # untested
|
||||
'get_encoder_error': (0x004, [('encoder_error', 'I', 1)]), # untested
|
||||
'get_sensorless_error': (0x005, [('sensorless_error', 'I', 1)]), # untested
|
||||
'set_node_id': (0x006, [('node_id', 'I', 1)]), # tested
|
||||
'set_requested_state': (0x007, [('requested_state', 'I', 1)]), # tested
|
||||
# 0x008 not yet implemented
|
||||
'get_encoder_estimates': (0x009, [('encoder_pos_estimate', 'f', 1), ('encoder_vel_estimate', 'f', 1)]), # partially tested
|
||||
'get_encoder_count': (0x00a, [('encoder_shadow_count', 'i', 1), ('encoder_count', 'i', 1)]), # partially tested
|
||||
'set_controller_modes': (0x00b, [('control_mode', 'i', 1), ('input_mode', 'i', 1)]), # tested
|
||||
'set_input_pos': (0x00c, [('input_pos', 'f', 1), ('vel_ff', 'h', 0.001), ('torque_ff', 'h', 0.001)]), # tested
|
||||
'set_input_vel': (0x00d, [('input_vel', 'f', 1), ('torque_ff', 'f', 1)]), # tested
|
||||
'set_input_torque': (0x00e, [('input_torque', 'f', 1)]), # tested
|
||||
'set_velocity_limit': (0x00f, [('velocity_limit', 'f', 1)]), # tested
|
||||
'start_anticogging': (0x010, []), # untested
|
||||
'set_traj_vel_limit': (0x011, [('traj_vel_limit', 'f', 1)]), # tested
|
||||
'set_traj_accel_limits': (0x012, [('traj_accel_limit', 'f', 1), ('traj_decel_limit', 'f', 1)]), # tested
|
||||
'set_traj_inertia': (0x013, [('inertia', 'f', 1)]), # tested
|
||||
'get_iq': (0x014, [('iq_setpoint', 'f', 1), ('iq_measured', 'f', 1)]), # untested
|
||||
'get_sensorless_estimates': (0x015, [('sensorless_pos_estimate', 'f', 1), ('sensorless_vel_estimate', 'f', 1)]), # untested
|
||||
'reboot': (0x016, []), # tested
|
||||
'get_vbus_voltage': (0x017, [('vbus_voltage', 'f', 1)]), # tested
|
||||
'clear_errors': (0x018, []), # partially tested
|
||||
}
|
||||
|
||||
def command(bus, node_id_, extended_id, cmd_name, **kwargs):
|
||||
cmd_spec = command_set[cmd_name]
|
||||
cmd_id = cmd_spec[0]
|
||||
fmt = '<' + ''.join([f for (n, f, s) in cmd_spec[1]]) # all little endian
|
||||
|
||||
if (sorted([n for (n, f, s) in cmd_spec[1]]) != sorted(kwargs.keys())):
|
||||
raise Exception("expected arguments: " + str([n for (n, f, s) in cmd_spec[1]]))
|
||||
|
||||
fields = [((kwargs[n] / s) if f == 'f' else int(kwargs[n] / s)) for (n, f, s) in cmd_spec[1]]
|
||||
data = struct.pack(fmt, *fields)
|
||||
msg = can.Message(arbitration_id=((node_id_ << 5) | cmd_id), extended_id=extended_id, data=data)
|
||||
bus.send(msg)
|
||||
|
||||
async def record_messages(bus, node_id, extended_id, cmd_name, timeout = 5.0):
|
||||
"""
|
||||
Returns an async generator that yields a dictionary for each CAN message that
|
||||
is received, provided that the CAN ID matches the expected value.
|
||||
"""
|
||||
|
||||
cmd_spec = command_set[cmd_name]
|
||||
cmd_id = cmd_spec[0]
|
||||
fmt = '<' + ''.join([f for (n, f, s) in cmd_spec[1]]) # all little endian
|
||||
|
||||
reader = can.AsyncBufferedReader()
|
||||
notifier = can.Notifier(bus, [reader], timeout = timeout, loop = asyncio.get_event_loop())
|
||||
|
||||
try:
|
||||
# The timeout in can.Notifier only triggers if no new messages are received at all,
|
||||
# so we need a second monitoring method.
|
||||
start = time.monotonic()
|
||||
while True:
|
||||
msg = await reader.get_message()
|
||||
if ((msg.arbitration_id == ((node_id << 5) | cmd_id)) and (msg.is_extended_id == extended_id) and not msg.is_remote_frame):
|
||||
fields = struct.unpack(fmt, msg.data[:(struct.calcsize(fmt))])
|
||||
res = {n: (fields[i] * s) for (i, (n, f, s)) in enumerate(cmd_spec[1])}
|
||||
res['t'] = time.monotonic()
|
||||
yield res
|
||||
if (time.monotonic() - start) > timeout:
|
||||
break
|
||||
finally:
|
||||
notifier.stop()
|
||||
|
||||
async def request(bus, node_id, extended_id, cmd_name, timeout = 1.0):
|
||||
cmd_spec = command_set[cmd_name]
|
||||
cmd_id = cmd_spec[0]
|
||||
|
||||
msg_generator = record_messages(bus, node_id, extended_id, cmd_name, timeout)
|
||||
|
||||
msg = can.Message(arbitration_id=((node_id << 5) | cmd_id), extended_id=extended_id, data=[], is_remote_frame=True)
|
||||
bus.send(msg)
|
||||
|
||||
async for msg in msg_generator:
|
||||
return msg
|
||||
|
||||
raise TimeoutError()
|
||||
|
||||
async def get_all(async_iterator):
|
||||
return [x async for x in async_iterator]
|
||||
|
||||
|
||||
class TestSimpleCAN():
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
can_interfaces = list(testrig.get_connected_components(odrive.can, CanInterfaceComponent))
|
||||
yield (odrive, can_interfaces, 0, False) # standard ID
|
||||
yield (odrive, can_interfaces, 0xfedcba, True) # extended ID
|
||||
|
||||
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)
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run(TestSimpleCAN())
|
||||
@@ -0,0 +1,391 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import time
|
||||
from math import pi, inf
|
||||
import os
|
||||
|
||||
from fibre.utils import Logger
|
||||
from test_runner import *
|
||||
from odrive.enums import *
|
||||
|
||||
|
||||
class TestClosedLoopControlBase():
|
||||
"""
|
||||
Base class for close loop control tests.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for num in range(2):
|
||||
encoders = testrig.get_connected_components({
|
||||
'a': (odrive.encoders[num].a, False),
|
||||
'b': (odrive.encoders[num].b, False)
|
||||
}, EncoderComponent)
|
||||
motors = testrig.get_connected_components(odrive.axes[num], MotorComponent)
|
||||
|
||||
for motor, encoder in itertools.product(motors, encoders):
|
||||
if encoder.impl in testrig.get_connected_components(motor):
|
||||
yield (odrive.axes[num], motor, encoder)
|
||||
|
||||
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()
|
||||
|
||||
|
||||
class TestClosedLoopControl(TestClosedLoopControlBase):
|
||||
"""
|
||||
Tests position and velocity control
|
||||
"""
|
||||
|
||||
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 = nominal_rps
|
||||
logger.debug(f'Testing closed loop velocity control at {nominal_rps} rounds/s...')
|
||||
|
||||
axis_ctx.handle.controller.config.control_mode = CONTROL_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 = CONTROL_MODE_POSITION_CONTROL
|
||||
axis_ctx.handle.controller.input_pos = 0
|
||||
axis_ctx.handle.controller.config.vel_limit = 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
|
||||
test_pos = 5000 / float(enc_ctx.yaml['cpr'])
|
||||
axis_ctx.handle.controller.input_pos = test_pos
|
||||
time.sleep(0.3)
|
||||
test_assert_no_error(axis_ctx)
|
||||
test_assert_eq(axis_ctx.handle.encoder.pos_estimate, test_pos, range=0.4*test_pos) # large range needed because of cogging torque
|
||||
axis_ctx.handle.controller.input_pos = -1 * test_pos
|
||||
time.sleep(0.3)
|
||||
test_assert_no_error(axis_ctx)
|
||||
test_assert_eq(axis_ctx.handle.encoder.pos_estimate, -1 * test_pos, range=0.4*test_pos)
|
||||
|
||||
axis_ctx.handle.controller.input_pos = 0
|
||||
time.sleep(0.3)
|
||||
|
||||
nominal_vel = 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)
|
||||
|
||||
|
||||
class TestRegenProtection(TestClosedLoopControlBase):
|
||||
"""
|
||||
Tries to brake with a disabled brake resistor.
|
||||
This should result in a low level error disabling all power outputs.
|
||||
|
||||
Note: If this test fails then try to run it at a DC voltage of 24V.
|
||||
Ibus seems to be more noisy/sensitive at lower DC voltages.
|
||||
"""
|
||||
|
||||
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 = 15.0
|
||||
nominal_vel = nominal_rps
|
||||
max_current = 30.0
|
||||
|
||||
# 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 = 25.0 # max 15 rps
|
||||
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_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, AXIS_ERROR_MOTOR_DISARMED | AXIS_ERROR_BRAKE_RESISTOR_DISARMED)
|
||||
test_assert_eq(axis_ctx.handle.motor.error, MOTOR_ERROR_DC_BUS_OVER_REGEN_CURRENT)
|
||||
|
||||
|
||||
class TestVelLimitInTorqueControl(TestClosedLoopControlBase):
|
||||
"""
|
||||
Ensures that the current setpoint in torque control is always within the
|
||||
parallelogram that arises from -Ilim, +Ilim, vel_limit and vel_gain.
|
||||
"""
|
||||
|
||||
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 = max_rps
|
||||
absolute_max_vel = max_vel * 1.2
|
||||
max_current = 30.0
|
||||
torque_constant = 0.0305 #correct for 5065 motor
|
||||
|
||||
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
|
||||
direction = axis_ctx.handle.motor.config.direction
|
||||
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.motor.config.torque_constant = torque_constant
|
||||
axis_ctx.handle.controller.config.control_mode = CONTROL_MODE_TORQUE_CONTROL
|
||||
|
||||
# Returns the expected limited setpoint for a given velocity and current
|
||||
def get_expected_setpoint(input_setpoint, velocity):
|
||||
return clamp(clamp(input_setpoint / torque_constant, (velocity + max_vel) * -vel_gain / torque_constant, (velocity - max_vel) * -vel_gain / torque_constant), -max_current, max_current) * direction
|
||||
|
||||
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_torque, velocity, current_setpoint, get_expected_setpoint(input_torque, velocity)
|
||||
|
||||
axis_ctx.handle.controller.input_torque = input_torque = 0.0
|
||||
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
|
||||
# Move the system around its operating envelope
|
||||
axis_ctx.handle.controller.input_torque = input_torque = 2.0 * torque_constant
|
||||
dataA = record_log(data_getter, duration=1.0)
|
||||
axis_ctx.handle.controller.input_torque = input_torque = -2.0 * torque_constant
|
||||
dataA = np.concatenate([dataA, record_log(data_getter, duration=1.0)])
|
||||
axis_ctx.handle.controller.input_torque = input_torque = 4.0 * torque_constant
|
||||
dataA = np.concatenate([dataA, record_log(data_getter, duration=1.0)])
|
||||
axis_ctx.handle.controller.input_torque = input_torque = -4.0 * torque_constant
|
||||
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 = max_rps
|
||||
axis_ctx.handle.controller.config.vel_limit = max_vel
|
||||
|
||||
# Move the system around its operating envelope
|
||||
axis_ctx.handle.controller.input_torque = input_torque = 2.0 * torque_constant
|
||||
dataB = record_log(data_getter, duration=1.0)
|
||||
axis_ctx.handle.controller.input_torque = input_torque = -2.0 * torque_constant
|
||||
dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)])
|
||||
axis_ctx.handle.controller.input_torque = input_torque = 4.0 * torque_constant
|
||||
dataB = np.concatenate([dataB, record_log(data_getter, duration=1.0)])
|
||||
axis_ctx.handle.controller.input_torque = input_torque = -4.0 * torque_constant
|
||||
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
|
||||
axis_ctx.handle.controller.input_torque = 4.0 * torque_constant
|
||||
time.sleep(0.5)
|
||||
axis_ctx.handle.controller.config.vel_limit = max_vel
|
||||
|
||||
axis_ctx.handle.controller.input_torque = input_torque = 2.0 * torque_constant
|
||||
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)
|
||||
|
||||
class TestTorqueLimit(TestClosedLoopControlBase):
|
||||
"""
|
||||
Checks that the torque limit is respected in position, velocity, and torque control modes
|
||||
"""
|
||||
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
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run([
|
||||
TestClosedLoopControl(),
|
||||
TestRegenProtection(),
|
||||
TestVelLimitInTorqueControl(),
|
||||
TestTorqueLimit()
|
||||
])
|
||||
@@ -0,0 +1,512 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import time
|
||||
from math import pi
|
||||
import os
|
||||
|
||||
from fibre.utils import Logger
|
||||
from odrive.enums import *
|
||||
from test_runner import *
|
||||
|
||||
|
||||
class TestEncoderBase():
|
||||
"""
|
||||
Base class for encoder tests.
|
||||
TODO: incremental encoder doesn't use this yet.
|
||||
|
||||
All encoder tests expect the encoder to run at a constant velocity.
|
||||
This can be achieved by generating an encoder signal with a Teensy.
|
||||
|
||||
During 5 seconds, several variables are recorded and then compared against
|
||||
the expected waveform. This is either a straight line, a sawtooth function
|
||||
or a constant.
|
||||
"""
|
||||
|
||||
def run_generic_encoder_test(self, encoder, true_cpr, true_rps, noise=1):
|
||||
encoder.config.cpr = true_cpr
|
||||
true_cps = true_cpr * true_rps
|
||||
|
||||
encoder.set_linear_count(0) # prevent numerical errors
|
||||
data = record_log(lambda: [
|
||||
encoder.shadow_count,
|
||||
encoder.count_in_cpr,
|
||||
encoder.phase,
|
||||
encoder.pos_estimate_counts,
|
||||
encoder.pos_cpr_counts,
|
||||
encoder.vel_estimate_counts,
|
||||
], duration=5.0)
|
||||
|
||||
short_period = (abs(1 / true_rps) < 5.0)
|
||||
reverse = (true_rps < 0)
|
||||
|
||||
# encoder.shadow_count
|
||||
slope, offset, fitted_curve = fit_line(data[:,(0,1)])
|
||||
test_assert_eq(slope, true_cps, accuracy=0.005)
|
||||
test_curve_fit(data[:,(0,1)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02)
|
||||
|
||||
# encoder.count_in_cpr
|
||||
slope, offset, fitted_curve = fit_sawtooth(data[:,(0,2)], true_cpr if reverse else 0, 0 if reverse else true_cpr)
|
||||
test_assert_eq(slope, true_cps, accuracy=0.005)
|
||||
test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02)
|
||||
|
||||
# encoder.phase
|
||||
slope, offset, fitted_curve = fit_sawtooth(data[:,(0,3)], pi if reverse else -pi, -pi if reverse else pi, sigma=5)
|
||||
test_assert_eq(slope / 7, 2*pi*true_rps, accuracy=0.05)
|
||||
test_curve_fit(data[:,(0,3)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02)
|
||||
|
||||
# encoder.pos_estimate
|
||||
slope, offset, fitted_curve = fit_line(data[:,(0,4)])
|
||||
test_assert_eq(slope, true_cps, accuracy=0.005)
|
||||
test_curve_fit(data[:,(0,4)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02)
|
||||
|
||||
# encoder.pos_cpr
|
||||
slope, offset, fitted_curve = fit_sawtooth(data[:,(0,5)], true_cpr if reverse else 0, 0 if reverse else true_cpr)
|
||||
test_assert_eq(slope, true_cps, accuracy=0.005)
|
||||
test_curve_fit(data[:,(0,5)], fitted_curve, max_mean_err = true_cpr * 0.05, inlier_range = true_cpr * 0.05, max_outliers = len(data[:,0]) * 0.02)
|
||||
|
||||
# encoder.vel_estimate
|
||||
slope, offset, fitted_curve = fit_line(data[:,(0,6)])
|
||||
test_assert_eq(slope, 0.0, range = true_cpr * abs(true_rps) * 0.01)
|
||||
test_assert_eq(offset, true_cpr * true_rps, accuracy = 0.02)
|
||||
test_curve_fit(data[:,(0,6)], fitted_curve, max_mean_err = true_cpr * 0.05, inlier_range = true_cpr * 0.05 * noise, max_outliers = len(data[:,0]) * 0.05)
|
||||
|
||||
|
||||
|
||||
teensy_incremental_encoder_emulation_code = """
|
||||
void setup() {
|
||||
pinMode({enc_a}, OUTPUT);
|
||||
pinMode({enc_b}, OUTPUT);
|
||||
}
|
||||
|
||||
int cpr = 8192;
|
||||
int rpm = 30;
|
||||
|
||||
// the loop routine runs over and over again forever:
|
||||
void loop() {
|
||||
int microseconds_per_count = (1000000 * 60 / cpr / rpm);
|
||||
|
||||
for (;;) {
|
||||
digitalWrite({enc_a}, HIGH);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({enc_b}, HIGH);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({enc_a}, LOW);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({enc_b}, LOW);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
}
|
||||
}
|
||||
"""
|
||||
|
||||
class TestIncrementalEncoder(TestEncoderBase):
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for encoder in odrive.encoders:
|
||||
# Find the Teensy that is connected to the encoder pins and the corresponding Teensy GPIOs
|
||||
|
||||
gpio_conns = [
|
||||
testrig.get_directly_connected_components(encoder.a),
|
||||
testrig.get_directly_connected_components(encoder.b),
|
||||
]
|
||||
|
||||
valid_combinations = [
|
||||
(combination[0].parent,) + tuple(combination)
|
||||
for combination in itertools.product(*gpio_conns)
|
||||
if ((len(set(c.parent for c in combination)) == 1) and isinstance(combination[0].parent, TeensyComponent))
|
||||
]
|
||||
|
||||
yield (encoder, valid_combinations)
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
|
||||
teensy_sin_cos_encoder_emulation_code = """
|
||||
void setup() {
|
||||
analogWriteResolution(10);
|
||||
int freq = 150000000/1024; // ~146.5kHz PWM frequency
|
||||
analogWriteFrequency({enc_sin}, freq);
|
||||
analogWriteFrequency({enc_cos}, freq);
|
||||
}
|
||||
|
||||
float rps = 1.0f;
|
||||
float pos = 0;
|
||||
|
||||
void loop() {
|
||||
pos += 0.001f * rps;
|
||||
if (pos > 1.0f)
|
||||
pos -= 1.0f;
|
||||
analogWrite({enc_sin}, (int)(512.0f + 512.0f * sin(2.0f * M_PI * pos)));
|
||||
analogWrite({enc_cos}, (int)(512.0f + 512.0f * cos(2.0f * M_PI * pos)));
|
||||
delay(1);
|
||||
}
|
||||
"""
|
||||
|
||||
class TestSinCosEncoder(TestEncoderBase):
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
gpio_conns = [
|
||||
testrig.get_directly_connected_components(odrive.gpio3),
|
||||
testrig.get_directly_connected_components(odrive.gpio4),
|
||||
]
|
||||
|
||||
valid_combinations = [
|
||||
(combination[0].parent,) + tuple(combination)
|
||||
for combination in itertools.product(*gpio_conns)
|
||||
if ((len(set(c.parent for c in combination)) == 1) and isinstance(combination[0].parent, TeensyComponent))
|
||||
]
|
||||
|
||||
yield (odrive.encoders[0], valid_combinations)
|
||||
|
||||
|
||||
def run_test(self, enc: ODriveEncoderComponent, teensy: TeensyComponent, teensy_gpio_sin: TeensyGpio, teensy_gpio_cos: TeensyGpio, logger: Logger):
|
||||
code = teensy_sin_cos_encoder_emulation_code.replace("{enc_sin}", str(teensy_gpio_sin.num)).replace("{enc_cos}", str(teensy_gpio_cos.num))
|
||||
teensy.compile_and_program(code)
|
||||
|
||||
if enc.handle.config.mode != ENCODER_MODE_SINCOS:
|
||||
enc.parent.unuse_gpios()
|
||||
enc.handle.config.mode = ENCODER_MODE_SINCOS
|
||||
enc.parent.save_config_and_reboot()
|
||||
else:
|
||||
time.sleep(1.0) # wait for PLLs to stabilize
|
||||
|
||||
enc.handle.config.bandwidth = 100
|
||||
|
||||
self.run_generic_encoder_test(enc.handle, 6283, 1.0, 2.0)
|
||||
|
||||
|
||||
|
||||
teensy_hall_effect_encoder_emulation_code = """
|
||||
void setup() {
|
||||
pinMode({hall_a}, OUTPUT);
|
||||
pinMode({hall_b}, OUTPUT);
|
||||
pinMode({hall_c}, OUTPUT);
|
||||
digitalWrite({hall_a}, HIGH);
|
||||
}
|
||||
|
||||
int cpr = 90; // 15 pole-pairs. Value suggested in hoverboard.md
|
||||
float rps = 1.0f;
|
||||
int us_per_count = (1000000.0f / cpr / rps);
|
||||
|
||||
void loop() {
|
||||
digitalWrite({hall_b}, HIGH);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_a}, LOW);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_c}, HIGH);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_b}, LOW);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_a}, HIGH);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_c}, LOW);
|
||||
delayMicroseconds(us_per_count);
|
||||
}
|
||||
"""
|
||||
|
||||
class TestHallEffectEncoder(TestEncoderBase):
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for encoder in odrive.encoders:
|
||||
# Find the Teensy that is connected to the encoder pins and the corresponding Teensy GPIOs
|
||||
|
||||
gpio_conns = [
|
||||
testrig.get_directly_connected_components(encoder.a),
|
||||
testrig.get_directly_connected_components(encoder.b),
|
||||
testrig.get_directly_connected_components(encoder.z),
|
||||
]
|
||||
|
||||
valid_combinations = [
|
||||
(combination[0].parent,) + tuple(combination)
|
||||
for combination in itertools.product(*gpio_conns)
|
||||
if ((len(set(c.parent for c in combination)) == 1) and isinstance(combination[0].parent, TeensyComponent))
|
||||
]
|
||||
|
||||
yield (encoder, valid_combinations)
|
||||
|
||||
|
||||
def run_test(self, enc: ODriveEncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, teensy_gpio_c: TeensyGpio, logger: Logger):
|
||||
true_cpr = 90
|
||||
true_rps = 1.0
|
||||
|
||||
code = teensy_hall_effect_encoder_emulation_code.replace("{hall_a}", str(teensy_gpio_a.num)).replace("{hall_b}", str(teensy_gpio_b.num)).replace("{hall_c}", str(teensy_gpio_c.num))
|
||||
teensy.compile_and_program(code)
|
||||
|
||||
if enc.handle.config.mode != ENCODER_MODE_HALL:
|
||||
enc.handle.config.mode = ENCODER_MODE_HALL
|
||||
enc.parent.save_config_and_reboot()
|
||||
else:
|
||||
time.sleep(1.0) # wait for PLLs to stabilize
|
||||
|
||||
enc.handle.config.bandwidth = 100
|
||||
|
||||
self.run_generic_encoder_test(enc.handle, true_cpr, true_rps)
|
||||
enc.handle.config.cpr = 8192
|
||||
|
||||
|
||||
|
||||
# This encoder emulation mimics the specification given in the following datasheets:
|
||||
#
|
||||
# With {mode} == ENCODER_MODE_SPI_ABS_CUI:
|
||||
# AMT23xx: https://www.cuidevices.com/product/resource/amt23.pdf
|
||||
#
|
||||
# With {mode} == ENCODER_MODE_SPI_ABS_AMS:
|
||||
# AS5047P: https://ams.com/documents/20143/36005/AS5047P_DS000324_2-00.pdf/a7d44138-51f1-2f6e-c8b6-2577b369ace8
|
||||
# AS5048A/AS5048B: https://ams.com/documents/20143/36005/AS5048_DS000298_4-00.pdf/910aef1f-6cd3-cbda-9d09-41f152104832
|
||||
# => Only the read command on address 0x3fff is currently implemented.
|
||||
|
||||
teensy_spi_encoder_emulation_code = """
|
||||
#define ENCODER_MODE_SPI_ABS_CUI 0x100
|
||||
#define ENCODER_MODE_SPI_ABS_AMS 0x101
|
||||
#define ENCODER_MODE_SPI_ABS_AEAT 0x102
|
||||
|
||||
static float rps = 1.0f;
|
||||
static uint32_t cpr = 16384;
|
||||
static uint32_t us_per_revolution = (uint32_t)(1000000.0f / rps);
|
||||
static uint16_t spi_txd = 0; // first output word: NOP
|
||||
static uint32_t zerotime = 0;
|
||||
|
||||
void setup() {
|
||||
pinMode({ncs}, INPUT_PULLUP);
|
||||
}
|
||||
|
||||
uint16_t get_pos_now() {
|
||||
uint32_t time = micros();
|
||||
return ((uint64_t)((time - zerotime) % us_per_revolution)) * cpr / us_per_revolution;
|
||||
}
|
||||
|
||||
|
||||
#if {mode} == ENCODER_MODE_SPI_ABS_AMS
|
||||
|
||||
uint8_t ams_parity(uint16_t v) {
|
||||
v ^= v >> 8;
|
||||
v ^= v >> 4;
|
||||
v ^= v >> 2;
|
||||
v ^= v >> 1;
|
||||
return v & 1;
|
||||
}
|
||||
|
||||
uint16_t handle_command(uint16_t cmd) {
|
||||
const uint16_t ERROR_RESPONSE = 0xc000; // error flag and parity bit set
|
||||
|
||||
if (ams_parity(cmd)) {
|
||||
return ERROR_RESPONSE;
|
||||
}
|
||||
|
||||
if (!(cmd & 14)) { // write not supported
|
||||
return ERROR_RESPONSE;
|
||||
}
|
||||
|
||||
uint16_t addr = cmd & 0x3fff;
|
||||
uint16_t data;
|
||||
|
||||
switch (addr) {
|
||||
case 0x3fff: data = get_pos_now(); break;
|
||||
default: return ERROR_RESPONSE;
|
||||
}
|
||||
|
||||
return data | (ams_parity(data) << 15);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if {mode} == ENCODER_MODE_SPI_ABS_CUI
|
||||
|
||||
uint8_t cui_parity(uint16_t v) {
|
||||
v ^= v >> 8;
|
||||
v ^= v >> 4;
|
||||
v ^= v >> 2;
|
||||
return ~v & 3;
|
||||
}
|
||||
|
||||
uint16_t handle_command(uint16_t cmd) {
|
||||
(void) cmd; // input not used on CUI
|
||||
|
||||
// Test the cui_parity function itself with the example given in the datasheet
|
||||
if ((0x21AB | (cui_parity(0x21AB) << 14)) != 0x61AB) {
|
||||
return 0x0000;
|
||||
}
|
||||
|
||||
uint16_t data = get_pos_now();
|
||||
return data | (cui_parity(data) << 14);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
void loop() {
|
||||
while (digitalReadFast({reset})) {
|
||||
zerotime = micros();
|
||||
}
|
||||
|
||||
if (!digitalReadFast({ncs})) {
|
||||
static uint16_t spi_rxd = 0;
|
||||
|
||||
pinMode({miso}, OUTPUT);
|
||||
|
||||
for (;;) {
|
||||
while (!digitalReadFast({sck}))
|
||||
if (digitalReadFast({ncs}))
|
||||
goto cs_deasserted;
|
||||
|
||||
// Rising edge: Push output bit
|
||||
|
||||
bool output_bit = spi_txd & 0x8000;
|
||||
digitalWriteFast({miso}, output_bit);
|
||||
spi_txd <<= 1;
|
||||
|
||||
while (digitalReadFast({sck}))
|
||||
if (digitalReadFast({ncs}))
|
||||
goto cs_deasserted;
|
||||
|
||||
// Falling edge: Sample input bit (only in AMS mode)
|
||||
|
||||
#if {mode} == ENCODER_MODE_SPI_ABS_AMS
|
||||
bool input_bit = digitalReadFast({mosi});
|
||||
spi_rxd <<= 1;
|
||||
if (input_bit) {
|
||||
spi_rxd |= 1;
|
||||
} else {
|
||||
spi_rxd &= ~1;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
cs_deasserted:
|
||||
// chip deselected: Process command
|
||||
pinMode({miso}, INPUT);
|
||||
|
||||
spi_txd = handle_command(spi_rxd);
|
||||
}
|
||||
}
|
||||
"""
|
||||
|
||||
class TestSpiEncoder(TestEncoderBase):
|
||||
def __init__(self, mode: int):
|
||||
self.mode = mode
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for encoder in odrive.encoders:
|
||||
odrive_ncs_gpio = odrive.gpio7 # this GPIO choice is completely arbitrary
|
||||
gpio_conns = [
|
||||
testrig.get_connected_components(odrive.sck, TeensyGpio),
|
||||
testrig.get_connected_components(odrive.miso, TeensyGpio),
|
||||
testrig.get_connected_components(odrive.mosi, TeensyGpio),
|
||||
testrig.get_connected_components(odrive_ncs_gpio, TeensyGpio),
|
||||
]
|
||||
|
||||
valid_combinations = []
|
||||
for combination in itertools.product(*gpio_conns):
|
||||
if (len(set(c.parent for c in combination)) != 1):
|
||||
continue
|
||||
teensy = combination[0].parent
|
||||
reset_pin_options = []
|
||||
for gpio in teensy.gpios:
|
||||
for local_gpio in testrig.get_connected_components(gpio, LinuxGpioComponent):
|
||||
reset_pin_options.append((gpio, local_gpio))
|
||||
valid_combinations.append((teensy, *combination, reset_pin_options))
|
||||
|
||||
yield (encoder, 7, valid_combinations)
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run([
|
||||
TestIncrementalEncoder(),
|
||||
TestSinCosEncoder(),
|
||||
TestHallEffectEncoder(),
|
||||
TestSpiEncoder(ENCODER_MODE_SPI_ABS_AMS),
|
||||
TestSpiEncoder(ENCODER_MODE_SPI_ABS_CUI),
|
||||
])
|
||||
@@ -0,0 +1,34 @@
|
||||
import odrive
|
||||
from odrive.enums import *
|
||||
from odrive.utils import *
|
||||
|
||||
print("finding an odrive...")
|
||||
odrv0 = odrive.find_any()
|
||||
print('Odrive found')
|
||||
|
||||
odrv0.axis1.controller.config.vel_limit = 50000
|
||||
odrv0.axis1.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL
|
||||
odrv0.axis1.controller.config.input_mode = INPUT_MODE_PASSTHROUGH
|
||||
odrv0.axis1.encoder.config.cpr = 2400
|
||||
odrv0.axis1.encoder.config.bandwidth = 1000
|
||||
odrv0.axis1.motor.config.calibration_current = 5
|
||||
odrv0.axis1.motor.config.current_lim = 5
|
||||
odrv0.axis1.controller.config.homing_speed = 5000
|
||||
odrv0.config.brake_resistance = 0
|
||||
|
||||
odrv0.axis0.min_endstop.config.gpio_num = 6
|
||||
odrv0.axis0.min_endstop.config.enabled = True
|
||||
odrv0.axis0.min_endstop.config.offset = -1000
|
||||
odrv0.axis0.max_endstop.config.gpio_num = 5
|
||||
odrv0.axis0.max_endstop.config.enabled = True
|
||||
|
||||
odrv0.axis1.min_endstop.config.gpio_num = 8
|
||||
odrv0.axis1.min_endstop.config.enabled = True
|
||||
odrv0.axis1.min_endstop.config.offset = -1000
|
||||
odrv0.axis1.max_endstop.config.gpio_num = 7
|
||||
odrv0.axis1.max_endstop.config.enabled = True
|
||||
|
||||
odrv0.axis1.config.startup_encoder_offset_calibration = True
|
||||
odrv0.axis1.config.startup_motor_calibration = True
|
||||
odrv0.axis1.config.startup_homing = True
|
||||
odrv0.axis1.config.startup_closed_loop_control = True
|
||||
@@ -0,0 +1,56 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import time
|
||||
|
||||
from fibre.utils import Logger
|
||||
from odrive.enums import *
|
||||
from test_runner import *
|
||||
|
||||
class FibreFunctionalTest():
|
||||
"""
|
||||
Tests basic protocol functionality.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
return testrig.get_components(ODriveComponent)
|
||||
|
||||
def run_test(self, odrive: ODriveComponent, logger: Logger):
|
||||
# Test property read/write
|
||||
odrive.handle.test_property = 42
|
||||
test_assert_eq(odrive.handle.test_property, 42)
|
||||
odrive.handle.test_property = 0xffffffff
|
||||
test_assert_eq(odrive.handle.test_property, 0xffffffff)
|
||||
|
||||
# Test function call
|
||||
val = odrive.handle.get_adc_voltage(0)
|
||||
test_assert_within(val, 0.01, 3.29)
|
||||
|
||||
# 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
|
||||
odrive.handle.axis0.motor.config.current_control_bandwidth /= 2
|
||||
test_assert_eq(odrive.handle.axis0.motor.current_control.p_gain, old_gain / 2, accuracy=0.0001)
|
||||
|
||||
class FibreBurnInTest():
|
||||
"""
|
||||
Tests continuous usage of the protocol.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
return testrig.get_components(ODriveComponent)
|
||||
|
||||
def run_test(self, odrive: ODriveComponent, logger: Logger):
|
||||
data = record_log(lambda: [odrive.handle.vbus_voltage], duration=10.0)
|
||||
expected_data = np.mean(data[:,1]) * np.ones(data[:,1].size)
|
||||
test_curve_fit(data, expected_data, max_mean_err = 0.1, inlier_range = 0.5, max_outliers = 0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run([
|
||||
FibreFunctionalTest(),
|
||||
FibreBurnInTest(),
|
||||
])
|
||||
@@ -0,0 +1,230 @@
|
||||
# this test runs the motor using CAN
|
||||
# TODO - run a motor using all common use cases (uart, step/dir, pwm)
|
||||
|
||||
import test_runner
|
||||
|
||||
import struct
|
||||
import can
|
||||
import asyncio
|
||||
import time
|
||||
import math
|
||||
|
||||
from fibre.utils import Logger
|
||||
from odrive.enums import *
|
||||
from test_runner import *
|
||||
|
||||
# Each argument is described as tuple (name, format, scale).
|
||||
# Struct format codes: https://docs.python.org/2/library/struct.html
|
||||
command_set = {
|
||||
'heartbeat': (0x001, [('error', 'I', 1), ('current_state', 'I', 1)]), # tested
|
||||
'estop': (0x002, []), # tested
|
||||
'get_motor_error': (0x003, [('motor_error', 'I', 1)]), # untested
|
||||
'get_encoder_error': (0x004, [('encoder_error', 'I', 1)]), # untested
|
||||
'get_sensorless_error': (0x005, [('sensorless_error', 'I', 1)]), # untested
|
||||
'set_node_id': (0x006, [('node_id', 'I', 1)]), # tested
|
||||
'set_requested_state': (0x007, [('requested_state', 'I', 1)]), # tested
|
||||
# 0x008 not yet implemented
|
||||
'get_encoder_estimates': (0x009, [('encoder_pos_estimate', 'f', 1), ('encoder_vel_estimate', 'f', 1)]), # partially tested
|
||||
'get_encoder_count': (0x00a, [('encoder_shadow_count', 'i', 1), ('encoder_count', 'i', 1)]), # partially tested
|
||||
'set_controller_modes': (0x00b, [('control_mode', 'i', 1), ('input_mode', 'i', 1)]), # tested
|
||||
'set_input_pos': (0x00c, [('input_pos', 'f', 1), ('vel_ff', 'h', 0.001), ('torque_ff', 'h', 0.001)]), # tested
|
||||
'set_input_vel': (0x00d, [('input_vel', 'f', 1), ('torque_ff', 'f', 1)]), # tested
|
||||
'set_input_torque': (0x00e, [('input_torque', 'f', 1)]), # tested
|
||||
'set_velocity_limit': (0x00f, [('velocity_limit', 'f', 1)]), # tested
|
||||
'start_anticogging': (0x010, []), # untested
|
||||
'set_traj_vel_limit': (0x011, [('traj_vel_limit', 'f', 1)]), # tested
|
||||
'set_traj_accel_limits': (0x012, [('traj_accel_limit', 'f', 1), ('traj_decel_limit', 'f', 1)]), # tested
|
||||
'set_traj_inertia': (0x013, [('inertia', 'f', 1)]), # tested
|
||||
'get_iq': (0x014, [('iq_setpoint', 'f', 1), ('iq_measured', 'f', 1)]), # untested
|
||||
'get_sensorless_estimates': (0x015, [('sensorless_pos_estimate', 'f', 1), ('sensorless_vel_estimate', 'f', 1)]), # untested
|
||||
'reboot': (0x016, []), # tested
|
||||
'get_vbus_voltage': (0x017, [('vbus_voltage', 'f', 1)]), # tested
|
||||
'clear_errors': (0x018, []), # partially tested
|
||||
}
|
||||
|
||||
def command(bus, node_id_, extended_id, cmd_name, **kwargs):
|
||||
cmd_spec = command_set[cmd_name]
|
||||
cmd_id = cmd_spec[0]
|
||||
fmt = '<' + ''.join([f for (n, f, s) in cmd_spec[1]]) # all little endian
|
||||
|
||||
if (sorted([n for (n, f, s) in cmd_spec[1]]) != sorted(kwargs.keys())):
|
||||
raise Exception("expected arguments: " + str([n for (n, f, s) in cmd_spec[1]]))
|
||||
|
||||
fields = [((kwargs[n] / s) if f == 'f' else int(kwargs[n] / s)) for (n, f, s) in cmd_spec[1]]
|
||||
data = struct.pack(fmt, *fields)
|
||||
msg = can.Message(arbitration_id=((node_id_ << 5) | cmd_id), extended_id=extended_id, data=data)
|
||||
bus.send(msg)
|
||||
|
||||
async def record_messages(bus, node_id, extended_id, cmd_name, timeout = 5.0):
|
||||
"""
|
||||
Returns an async generator that yields a dictionary for each CAN message that
|
||||
is received, provided that the CAN ID matches the expected value.
|
||||
"""
|
||||
|
||||
cmd_spec = command_set[cmd_name]
|
||||
cmd_id = cmd_spec[0]
|
||||
fmt = '<' + ''.join([f for (n, f, s) in cmd_spec[1]]) # all little endian
|
||||
|
||||
reader = can.AsyncBufferedReader()
|
||||
notifier = can.Notifier(bus, [reader], timeout = timeout, loop = asyncio.get_event_loop())
|
||||
|
||||
try:
|
||||
# The timeout in can.Notifier only triggers if no new messages are received at all,
|
||||
# so we need a second monitoring method.
|
||||
start = time.monotonic()
|
||||
while True:
|
||||
msg = await reader.get_message()
|
||||
if ((msg.arbitration_id == ((node_id << 5) | cmd_id)) and (msg.is_extended_id == extended_id) and not msg.is_remote_frame):
|
||||
fields = struct.unpack(fmt, msg.data[:(struct.calcsize(fmt))])
|
||||
res = {n: (fields[i] * s) for (i, (n, f, s)) in enumerate(cmd_spec[1])}
|
||||
res['t'] = time.monotonic()
|
||||
yield res
|
||||
if (time.monotonic() - start) > timeout:
|
||||
break
|
||||
finally:
|
||||
notifier.stop()
|
||||
|
||||
async def request(bus, node_id, extended_id, cmd_name, timeout = 1.0):
|
||||
cmd_spec = command_set[cmd_name]
|
||||
cmd_id = cmd_spec[0]
|
||||
|
||||
msg_generator = record_messages(bus, node_id, extended_id, cmd_name, timeout)
|
||||
|
||||
msg = can.Message(arbitration_id=((node_id << 5) | cmd_id), extended_id=extended_id, data=[], is_remote_frame=True)
|
||||
bus.send(msg)
|
||||
|
||||
async for msg in msg_generator:
|
||||
return msg
|
||||
|
||||
raise TimeoutError()
|
||||
|
||||
async def get_all(async_iterator):
|
||||
return [x async for x in async_iterator]
|
||||
|
||||
class TestSimpleCANClosedLoop():
|
||||
def prepare(self, odrive: ODriveComponent, canbus: CanInterfaceComponent, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, node_id: int, extended_id: bool, logger: Logger):
|
||||
# Make sure there are no funny configurations active
|
||||
logger.debug('Setting up clean configuration...')
|
||||
axis_ctx.parent.erase_config_and_reboot()
|
||||
|
||||
# run calibration
|
||||
axis_ctx.handle.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
|
||||
while axis_ctx.handle.current_state != AXIS_STATE_IDLE:
|
||||
time.sleep(1)
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
# Return a context that can be used in a with-statement.
|
||||
class safe_terminator():
|
||||
def __enter__(self):
|
||||
pass
|
||||
def __exit__(self, exc_type, exc_val, exc_tb):
|
||||
logger.debug('clearing config...')
|
||||
axis_ctx.handle.requested_state = AXIS_STATE_IDLE
|
||||
time.sleep(0.005)
|
||||
axis_ctx.parent.erase_config_and_reboot()
|
||||
return safe_terminator()
|
||||
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
can_interfaces = list(testrig.get_connected_components(odrive.can, CanInterfaceComponent))
|
||||
for num in range(2):
|
||||
encoders = testrig.get_connected_components({
|
||||
'a': (odrive.encoders[num].a, False),
|
||||
'b': (odrive.encoders[num].b, False)
|
||||
}, EncoderComponent)
|
||||
motors = testrig.get_connected_components(odrive.axes[num], MotorComponent)
|
||||
for motor, encoder in itertools.product(motors, encoders):
|
||||
if encoder.impl in testrig.get_connected_components(motor):
|
||||
yield (odrive, can_interfaces, odrive.axes[num], motor, encoder, 0, False)
|
||||
|
||||
def run_test(self, odrive: ODriveComponent, canbus: CanInterfaceComponent, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, node_id: int, extended_id: bool, logger: Logger):
|
||||
# this test is a sanity check to make sure that closed loop operation works
|
||||
# actual testing of closed loop functionality should be tested using closed_loop_test.py
|
||||
|
||||
with self.prepare(odrive, canbus, axis_ctx, motor_ctx, enc_ctx, node_id, extended_id, logger):
|
||||
def my_cmd(cmd_name, **kwargs): command(canbus.handle, node_id, extended_id, cmd_name, **kwargs)
|
||||
def my_req(cmd_name, **kwargs): return asyncio.run(request(canbus.handle, node_id, extended_id, cmd_name, **kwargs))
|
||||
def fence(): my_req('get_vbus_voltage') # fence to ensure the CAN command was sent
|
||||
|
||||
# make sure no gpio input is overwriting our values
|
||||
odrive.unuse_gpios()
|
||||
|
||||
axis_ctx.handle.config.enable_watchdog = False
|
||||
axis_ctx.handle.clear_errors()
|
||||
axis_ctx.handle.config.can_node_id = node_id
|
||||
axis_ctx.handle.config.can_node_id_extended = extended_id
|
||||
time.sleep(0.1)
|
||||
|
||||
my_cmd('set_node_id', node_id=node_id+20)
|
||||
asyncio.run(request(canbus.handle, node_id+20, extended_id, 'get_vbus_voltage'))
|
||||
test_assert_eq(axis_ctx.handle.config.can_node_id, node_id+20)
|
||||
|
||||
# Reset node ID to default value
|
||||
command(canbus.handle, node_id+20, extended_id, 'set_node_id', node_id=node_id)
|
||||
fence()
|
||||
test_assert_eq(axis_ctx.handle.config.can_node_id, node_id)
|
||||
|
||||
vel_limit = 15.0
|
||||
nominal_vel = 10.0
|
||||
axis_ctx.handle.controller.config.vel_limit = vel_limit
|
||||
axis_ctx.handle.motor.config.current_lim = 30.0
|
||||
|
||||
my_cmd('set_requested_state', requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
fence()
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
start_pos = axis_ctx.handle.encoder.pos_estimate
|
||||
|
||||
# position test
|
||||
logger.debug('Position control test')
|
||||
my_cmd('set_controller_modes', control_mode=CONTROL_MODE_POSITION_CONTROL, input_mode=INPUT_MODE_PASSTHROUGH) # position control, passthrough
|
||||
fence()
|
||||
my_cmd('set_input_pos', input_pos=1.0, vel_ff=0, torque_ff=0)
|
||||
fence()
|
||||
test_assert_eq(axis_ctx.handle.controller.input_pos, 1.0, range=0.1)
|
||||
time.sleep(2)
|
||||
test_assert_eq(axis_ctx.handle.encoder.pos_estimate, start_pos + 1.0, range=0.1)
|
||||
my_cmd('set_input_pos', input_pos=0, vel_ff=0, torque_ff=0)
|
||||
fence()
|
||||
time.sleep(2)
|
||||
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
# velocity test
|
||||
logger.debug('Velocity control test')
|
||||
my_cmd('set_controller_modes', control_mode=CONTROL_MODE_VELOCITY_CONTROL, input_mode=INPUT_MODE_PASSTHROUGH) # velocity control, passthrough
|
||||
fence()
|
||||
my_cmd('set_input_vel', input_vel = nominal_vel, torque_ff=0)
|
||||
fence()
|
||||
time.sleep(5)
|
||||
test_assert_eq(axis_ctx.handle.encoder.vel_estimate, nominal_vel, range=nominal_vel * 0.05) # big range here due to cogging and other issues
|
||||
my_cmd('set_input_vel', input_vel = 0, torque_ff=0)
|
||||
fence()
|
||||
time.sleep(2)
|
||||
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
# torque test
|
||||
logger.debug('Torque control test')
|
||||
my_cmd('set_controller_modes', control_mode=CONTROL_MODE_TORQUE_CONTROL, input_mode=INPUT_MODE_PASSTHROUGH) # torque control, passthrough
|
||||
fence()
|
||||
my_cmd('set_input_torque', input_torque=0.5)
|
||||
fence()
|
||||
time.sleep(5)
|
||||
test_assert_eq(axis_ctx.handle.controller.input_torque, 0.5, range=0.1)
|
||||
my_cmd('set_input_torque', input_torque = 0)
|
||||
fence()
|
||||
time.sleep(2)
|
||||
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
# go back to idle
|
||||
my_cmd('set_requested_state', requested_state = AXIS_STATE_IDLE)
|
||||
fence()
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run(TestSimpleCANClosedLoop())
|
||||
@@ -0,0 +1,30 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
from fibre.utils import Logger
|
||||
from test_runner import *
|
||||
|
||||
class EncoderPassthrough():
|
||||
"""
|
||||
Does nothing except passing encoder0 through.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for num in range(1):
|
||||
encoders = testrig.get_connected_components({
|
||||
'a': (odrive.encoders[num].a, False),
|
||||
'b': (odrive.encoders[num].b, False),
|
||||
'z': (odrive.encoders[num].z, False)
|
||||
}, EncoderComponent)
|
||||
motors = testrig.get_connected_components(odrive.axes[num], MotorComponent)
|
||||
|
||||
for motor, encoder in itertools.product(motors, encoders):
|
||||
if encoder.impl in testrig.get_connected_components(motor):
|
||||
yield (odrive.axes[num], motor, encoder)
|
||||
|
||||
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')
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run(EncoderPassthrough())
|
||||
@@ -0,0 +1,46 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import time
|
||||
from math import pi
|
||||
import os
|
||||
|
||||
import fibre
|
||||
from fibre.utils import Logger
|
||||
from test_runner import *
|
||||
|
||||
class TestStoreAndReboot():
|
||||
"""
|
||||
Stores the current configuration to NVM and reboots.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
yield (odrive,)
|
||||
|
||||
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, logger: Logger):
|
||||
self.run_with_values(odrive, [0.5, 1.0, 1.5], logger)
|
||||
self.run_with_values(odrive, [2.5, 3.7], logger)
|
||||
self.run_with_values(odrive, [0.47], logger)
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run(TestStoreAndReboot())
|
||||
@@ -0,0 +1,718 @@
|
||||
from __future__ import print_function
|
||||
import subprocess
|
||||
import shlex
|
||||
import math
|
||||
import time
|
||||
import sys
|
||||
import threading
|
||||
import fibre
|
||||
import odrive
|
||||
from odrive.enums import *
|
||||
import odrive.utils
|
||||
import numpy as np
|
||||
|
||||
import functools
|
||||
print = functools.partial(print, flush=True)
|
||||
|
||||
import abc
|
||||
ABC = abc.ABC
|
||||
|
||||
|
||||
class PreconditionsNotMet(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class AxisTestContext():
|
||||
def __init__(self, name: str, yaml: dict, odrv_ctx: ODriveTestContext):
|
||||
self.handle = None
|
||||
self.yaml = yaml
|
||||
self.name = name
|
||||
self.lock = threading.Lock()
|
||||
self.odrv_ctx = odrv_ctx
|
||||
|
||||
def get_errors(axis_ctx: AxisTestContext):
|
||||
errors = []
|
||||
if axis_ctx.handle.motor.error != 0:
|
||||
errors.append("motor failed with error 0x{:04X}".format(axis_ctx.handle.motor.error))
|
||||
if axis_ctx.handle.encoder.error != 0:
|
||||
errors.append("encoder failed with error 0x{:04X}".format(axis_ctx.handle.encoder.error))
|
||||
if axis_ctx.handle.sensorless_estimator.error != 0:
|
||||
errors.append("sensorless_estimator failed with error 0x{:04X}".format(axis_ctx.handle.sensorless_estimator.error))
|
||||
if axis_ctx.handle.error != 0:
|
||||
errors.append("axis failed with error 0x{:04X}".format(axis_ctx.handle.error))
|
||||
elif len(errors) > 0:
|
||||
errors.append("and by the way: axis reports no error even though there is one")
|
||||
return errors
|
||||
|
||||
def dump_errors(axis_ctx: AxisTestContext, logger):
|
||||
errors = get_errors(axis_ctx)
|
||||
if len(errors):
|
||||
logger.error("errors on " + axis_ctx.name)
|
||||
for error in errors:
|
||||
logger.error(error)
|
||||
|
||||
def clear_errors(axis_ctx: AxisTestContext):
|
||||
axis_ctx.handle.error = 0
|
||||
axis_ctx.handle.encoder.error = 0
|
||||
axis_ctx.handle.motor.error = 0
|
||||
axis_ctx.handle.sensorless_estimator.error = 0
|
||||
|
||||
def test_assert_no_error(axis_ctx: AxisTestContext):
|
||||
errors = get_errors(axis_ctx)
|
||||
if len(errors) > 0:
|
||||
raise TestFailed("\n".join(errors))
|
||||
|
||||
def run(command_line, logger, timeout=None):
|
||||
"""
|
||||
Runs a shell command in the current directory
|
||||
"""
|
||||
logger.debug("invoke: " + command_line)
|
||||
cmd = shlex.split(command_line)
|
||||
result = subprocess.run(cmd, timeout=timeout,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.STDOUT)
|
||||
if result.returncode != 0:
|
||||
logger.error(result.stdout.decode(sys.stdout.encoding))
|
||||
raise TestFailed("command {} failed".format(command_line))
|
||||
|
||||
def request_state(axis_ctx: AxisTestContext, state, expect_success=True):
|
||||
axis_ctx.handle.requested_state = state
|
||||
time.sleep(0.001)
|
||||
if expect_success:
|
||||
test_assert_eq(axis_ctx.handle.current_state, state)
|
||||
else:
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_INVALID_STATE)
|
||||
axis_ctx.handle.error = AXIS_ERROR_NONE # reset error
|
||||
|
||||
def set_limits(axis_ctx: AxisTestContext, logger, vel_limit=20000, current_limit=10):
|
||||
"""
|
||||
Sets the velocity and current limits for the axis, subject to the following constraints:
|
||||
- the arguments given to this function are not exceeded
|
||||
- max motor current is not exceeded
|
||||
- max brake resistor power divided by two is not exceeded (here velocity takes precedence over current)
|
||||
"""
|
||||
max_rpm = vel_limit / axis_ctx.yaml['encoder-cpr'] * 60
|
||||
max_emf_voltage = max_rpm / axis_ctx.yaml['motor-kv']
|
||||
max_brake_power = axis_ctx.odrv_ctx.yaml['max-brake-power'] / 2 * 0.8 # 20% safety margin
|
||||
max_motor_current = max_brake_power / max_emf_voltage
|
||||
logger.debug("velocity limit = {} => V_emf = {:.3}V, I_lim = {:.3}A".format(vel_limit, max_emf_voltage, max_motor_current))
|
||||
|
||||
# Bound current limit based on the motor's current limit and the brake resistor current limit
|
||||
current_limit = min(current_limit, axis_ctx.yaml['motor-max-current'], max_motor_current)
|
||||
# TODO: set as an atomic operation
|
||||
axis_ctx.handle.motor.config.current_lim = current_limit
|
||||
axis_ctx.handle.controller.config.vel_limit = vel_limit
|
||||
|
||||
def get_max_rpm(axis_ctx: AxisTestContext):
|
||||
|
||||
# Calculate theoretical max velocity in rpm based on the nominal
|
||||
# V_bus and motor KV rating.
|
||||
# The KV-rating assumes square-waves on the motor phases (hexagonal space vector trajectory)
|
||||
# whereas the ODrive modulates the space vector around a circular trajectory.
|
||||
# See Fig 4.28 here: http://krex.k-state.edu/dspace/bitstream/handle/2097/1507/JamesMevey2009.pdf
|
||||
effective_bus_voltage = axis_ctx.odrv_ctx.yaml['vbus-voltage']
|
||||
effective_bus_voltage *= (2/math.sqrt(3)) / (4/math.pi) # roughtly 90%
|
||||
# The ODrive only goes to 80% modulation depth in order to save some time for the ADC measurements.
|
||||
# See FOC_current in motor.cpp.
|
||||
effective_bus_voltage *= 0.8
|
||||
|
||||
# If we are using a higher bus voltage than rated: use rated voltage,
|
||||
# since that is an effective speed rating of the motor
|
||||
voltage_for_speed = min(effective_bus_voltage, axis_ctx.yaml['motor-max-voltage'])
|
||||
base_speed_rpm = voltage_for_speed * axis_ctx.yaml['motor-kv']
|
||||
|
||||
#but don't go over encoder max rpm
|
||||
rated_rpm = min(base_speed_rpm, axis_ctx.yaml['encoder-max-rpm'])
|
||||
return rated_rpm
|
||||
|
||||
def get_sensorless_vel(axis_ctx: AxisTestContext, vel):
|
||||
return vel * 2 * math.pi / axis_ctx.yaml['encoder-cpr'] * axis_ctx.yaml['motor-pole-pairs']
|
||||
|
||||
class ODriveTest(ABC):
|
||||
"""
|
||||
Tests inheriting from this class get full ownership of the ODrive
|
||||
being tested. However no guarantees are made for the mechanical
|
||||
state of the axes.
|
||||
The test can demand exclusive run time which means that the host will
|
||||
not run any other test at the same time. This can be used if the test
|
||||
invokes a command that's so lame that it can't run twice concurrently.
|
||||
"""
|
||||
def __init__(self, exclusive=False):
|
||||
self._exclusive = exclusive
|
||||
def check_preconditions(self, odrv_ctx: ODriveTestContext, logger):
|
||||
pass
|
||||
@abc.abstractmethod
|
||||
def run_test(self, odrv_ctx: ODriveTestContext, logger):
|
||||
pass
|
||||
|
||||
class AxisTest(ABC):
|
||||
"""
|
||||
Tests inheriting from this class get ownership of one axis of
|
||||
an ODrive. If the axis is mechanically coupled to another
|
||||
axis, the other axis is guaranteed to be disabled (high impedance)
|
||||
during this test.
|
||||
"""
|
||||
def check_preconditions(self, axis_ctx: AxisTestContext, logger):
|
||||
test_assert_no_error(axis_ctx)
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
if (abs(axis_ctx.handle.encoder.vel_estimate) > 100):
|
||||
logger.warn("axis still in motion, delaying 2 sec...")
|
||||
time.sleep(2)
|
||||
test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 0, range=500)
|
||||
test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.85, accuracy=0.001)
|
||||
test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001)
|
||||
#test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.96, accuracy=0.001)
|
||||
#test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.04, accuracy=0.001)
|
||||
|
||||
@abc.abstractmethod
|
||||
def run_test(self, axis_ctx: AxisTestContext, logger):
|
||||
pass
|
||||
|
||||
class DualAxisTest(ABC):
|
||||
"""
|
||||
Tests using this scope get ownership of two axes that are mechanically
|
||||
coupled.
|
||||
"""
|
||||
def check_preconditions(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger):
|
||||
test_assert_no_error(axis0_ctx)
|
||||
test_assert_no_error(axis1_ctx)
|
||||
test_assert_eq(axis0_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_eq(axis1_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
if (abs(axis0_ctx.handle.encoder.vel_estimate) > 100) or (abs(axis1_ctx.handle.encoder.vel_estimate) > 100):
|
||||
logger.warn("some axis still in motion, delaying 2 sec...")
|
||||
time.sleep(2)
|
||||
test_assert_eq(axis0_ctx.handle.encoder.vel_estimate, 0, range=500)
|
||||
test_assert_eq(axis1_ctx.handle.encoder.vel_estimate, 0, range=500)
|
||||
|
||||
@abc.abstractmethod
|
||||
def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger):
|
||||
pass
|
||||
|
||||
class TestDiscoverAndGotoIdle(ODriveTest):
|
||||
def run_test(self, odrv_ctx: ODriveTestContext, logger):
|
||||
odrv_ctx.rediscover()
|
||||
clear_errors(odrv_ctx.axes[0])
|
||||
clear_errors(odrv_ctx.axes[1])
|
||||
request_state(odrv_ctx.axes[0], AXIS_STATE_IDLE)
|
||||
request_state(odrv_ctx.axes[1], AXIS_STATE_IDLE)
|
||||
|
||||
class TestFlashAndErase(ODriveTest):
|
||||
def __init__(self):
|
||||
ODriveTest.__init__(self, exclusive=True)
|
||||
def run_test(self, odrv_ctx: ODriveTestContext, logger):
|
||||
# Set board-version and compile
|
||||
with open("tup.config", mode="w") as tup_config:
|
||||
tup_config.write("CONFIG_STRICT=true\n")
|
||||
tup_config.write("CONFIG_BOARD_VERSION={}\n".format(odrv_ctx.yaml['board-version']))
|
||||
#exit(1)
|
||||
run("make", logger, timeout=10)
|
||||
run("make flash PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=20)
|
||||
# FIXME: device does not reboot correctly after erasing config this way
|
||||
#run("make erase_config PROGRAMMER='" + test_rig.programmer + "'", timeout=10)
|
||||
|
||||
logger.debug("waiting for ODrive...")
|
||||
odrv_ctx.rediscover()
|
||||
# ensure the correct odrive is returned
|
||||
test_assert_eq(format(odrv_ctx.handle.serial_number, 'x').upper(), odrv_ctx.yaml['serial-number'])
|
||||
|
||||
# erase configuration and reboot
|
||||
logger.debug("erasing old configuration...")
|
||||
odrv_ctx.handle.erase_configuration()
|
||||
#time.sleep(0.1)
|
||||
try:
|
||||
# FIXME: sometimes the device does not reappear after this ("no response - probably incompatible")
|
||||
# this is a firmware issue since it persists when unplugging/replugging
|
||||
# but goes away when power cycling the device
|
||||
odrv_ctx.handle.reboot()
|
||||
except fibre.ChannelBrokenException:
|
||||
pass # this is expected
|
||||
time.sleep(0.5)
|
||||
|
||||
class TestSetup(ODriveTest):
|
||||
"""
|
||||
Preconditions: ODrive is unconfigured and just rebooted
|
||||
"""
|
||||
def run_test(self, odrv_ctx: ODriveTestContext, logger):
|
||||
odrv_ctx.rediscover()
|
||||
|
||||
# initial protocol tests and setup
|
||||
logger.debug("setting up ODrive...")
|
||||
odrv_ctx.handle.config.enable_uart = True
|
||||
test_assert_eq(odrv_ctx.handle.config.enable_uart, True)
|
||||
odrv_ctx.handle.config.enable_uart = False
|
||||
test_assert_eq(odrv_ctx.handle.config.enable_uart, False)
|
||||
odrv_ctx.handle.config.brake_resistance = 1.0
|
||||
test_assert_eq(odrv_ctx.handle.config.brake_resistance, 1.0)
|
||||
odrv_ctx.handle.config.brake_resistance = odrv_ctx.yaml['brake-resistance']
|
||||
test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01)
|
||||
odrv_ctx.handle.config.dc_bus_undervoltage_trip_level = odrv_ctx.yaml['vbus-voltage'] * 0.85
|
||||
odrv_ctx.handle.config.dc_bus_overvoltage_trip_level = odrv_ctx.yaml['vbus-voltage'] * 1.08
|
||||
test_assert_eq(odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, odrv_ctx.yaml['vbus-voltage'] * 0.85, accuracy=0.001)
|
||||
test_assert_eq(odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001)
|
||||
|
||||
# firmware has 1500ms startup delay
|
||||
time.sleep(2)
|
||||
|
||||
logger.debug("ensure we're in idle state")
|
||||
test_assert_eq(odrv_ctx.handle.axis0.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_eq(odrv_ctx.handle.axis1.current_state, AXIS_STATE_IDLE)
|
||||
|
||||
class TestMotorCalibration(AxisTest):
|
||||
"""
|
||||
Tests motor calibration.
|
||||
The calibration results are compared against well known test rig values.
|
||||
Preconditions: The motor must be uncalibrated.
|
||||
Postconditions: The motor will be calibrated after this test.
|
||||
"""
|
||||
def check_preconditions(self, axis_ctx: AxisTestContext, logger):
|
||||
super(TestMotorCalibration, self).check_preconditions(axis_ctx, logger)
|
||||
test_assert_eq(axis_ctx.handle.motor.is_calibrated, False)
|
||||
|
||||
def run_test(self, axis_ctx: AxisTestContext, logger):
|
||||
logger.debug("try to enter closed loop control (should be rejected)")
|
||||
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False)
|
||||
|
||||
logger.debug("try to start encoder index search (should be rejected)")
|
||||
request_state(axis_ctx, AXIS_STATE_ENCODER_INDEX_SEARCH, expect_success=False)
|
||||
|
||||
logger.debug("try to start encoder offset calibration (should be rejected)")
|
||||
request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION, expect_success=False)
|
||||
|
||||
logger.debug("motor calibration (takes about 4.5 seconds)")
|
||||
axis_ctx.handle.motor.config.pole_pairs = axis_ctx.yaml['motor-pole-pairs']
|
||||
request_state(axis_ctx, AXIS_STATE_MOTOR_CALIBRATION)
|
||||
time.sleep(6)
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_no_error(axis_ctx)
|
||||
test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.2)
|
||||
test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5)
|
||||
axis_ctx.handle.motor.config.pre_calibrated = True
|
||||
|
||||
class TestEncoderOffsetCalibration(AxisTest):
|
||||
"""
|
||||
Tests encoder offset calibration.
|
||||
Preconditions: The encoder must be non-ready.
|
||||
Postconditions: The encoder will be ready after this test.
|
||||
"""
|
||||
def __init__(self, pass_if_ready=False):
|
||||
AxisTest.__init__(self)
|
||||
self._pass_if_ready = pass_if_ready
|
||||
|
||||
def check_preconditions(self, axis_ctx: AxisTestContext, logger):
|
||||
super(TestEncoderOffsetCalibration, self).check_preconditions(axis_ctx, logger)
|
||||
if not self._pass_if_ready:
|
||||
test_assert_eq(axis_ctx.handle.encoder.is_ready, False)
|
||||
|
||||
def run_test(self, axis_ctx: AxisTestContext, logger):
|
||||
if (self._pass_if_ready and axis_ctx.handle.encoder.is_ready):
|
||||
logger.debug("encoder already ready, skipping this test")
|
||||
return
|
||||
|
||||
logger.debug("try to enter closed loop control (should be rejected)")
|
||||
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False)
|
||||
|
||||
logger.debug("encoder offset calibration (takes about 9.5 seconds)")
|
||||
axis_ctx.handle.encoder.config.cpr = axis_ctx.yaml['encoder-cpr'] # TODO: test setting a wrong CPR
|
||||
request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION)
|
||||
# TODO: ensure the encoder calibration doesn't do crap
|
||||
time.sleep(11)
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_no_error(axis_ctx)
|
||||
test_assert_eq(axis_ctx.handle.motor.config.direction, axis_ctx.yaml['motor-direction'])
|
||||
axis_ctx.handle.encoder.config.pre_calibrated = True
|
||||
|
||||
class TestClosedLoopControl(AxisTest):
|
||||
"""
|
||||
Tests closed loop position control and velocity control
|
||||
and verifies that the sensorless estimator works
|
||||
Precondition: The axis is calibrated and ready for closed loop control
|
||||
"""
|
||||
def check_preconditions(self, axis_ctx: AxisTestContext, logger):
|
||||
super(TestClosedLoopControl, self).check_preconditions(axis_ctx, logger)
|
||||
test_assert_eq(axis_ctx.handle.motor.is_calibrated, True)
|
||||
test_assert_eq(axis_ctx.handle.encoder.is_ready, True)
|
||||
|
||||
def run_test(self, axis_ctx: AxisTestContext, logger):
|
||||
logger.debug("closed loop control: test tiny position changes")
|
||||
axis_ctx.handle.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
|
||||
time.sleep(0.001)
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
time.sleep(0.1) # give the PLL some time to settle
|
||||
init_pos = axis_ctx.handle.encoder.pos_estimate
|
||||
axis_ctx.handle.controller.set_pos_setpoint(init_pos+1000, 0, 0)
|
||||
time.sleep(0.5)
|
||||
test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos+1000, range=200)
|
||||
axis_ctx.handle.controller.set_pos_setpoint(init_pos-1000, 0, 0)
|
||||
time.sleep(0.5)
|
||||
test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos-1000, range=400)
|
||||
|
||||
logger.debug("closed loop control: test vel_limit")
|
||||
axis_ctx.handle.controller.set_pos_setpoint(50000, 0, 0)
|
||||
axis_ctx.handle.controller.config.vel_limit = 40000
|
||||
time.sleep(0.3)
|
||||
test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 40000, range=4000)
|
||||
expected_sensorless_estimation = 40000 * 2 * math.pi / axis_ctx.yaml['encoder-cpr'] * axis_ctx.yaml['motor-pole-pairs']
|
||||
test_assert_eq(axis_ctx.handle.sensorless_estimator.vel_estimate, expected_sensorless_estimation, range=50)
|
||||
time.sleep(3)
|
||||
test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 0, range=1000)
|
||||
time.sleep(0.5)
|
||||
request_state(axis_ctx, AXIS_STATE_IDLE)
|
||||
|
||||
class TestHighVelocity(AxisTest):
|
||||
"""
|
||||
Spins the motor up to it's max speed during a period of 10s.
|
||||
The commanded max speed is based on the motor's KV rating and nominal V_bus,
|
||||
however due to several factors the theoretical limit is about 72% of that.
|
||||
The test passes if the motor follows the commanded ramp closely up to 90% of
|
||||
the theoretical limit (and if no errors occur along the way).
|
||||
"""
|
||||
def __init__(self, override_current_limit=None, load_current=0, brake=True):
|
||||
"""
|
||||
param override_current_limit: If None, the test selects a current limit that is guaranteed
|
||||
not to fry the brake resistor. If you override the limit, you're
|
||||
on your own.
|
||||
"""
|
||||
self._override_current_limit = override_current_limit
|
||||
self._load_current = load_current
|
||||
self._brake = brake
|
||||
|
||||
def check_preconditions(self, axis_ctx: AxisTestContext, logger):
|
||||
# time.sleep(2.5) #delay in case load needs time to stop moving
|
||||
super(TestHighVelocity, self).check_preconditions(axis_ctx, logger)
|
||||
test_assert_eq(axis_ctx.handle.motor.is_calibrated, True)
|
||||
test_assert_eq(axis_ctx.handle.encoder.is_ready, True)
|
||||
|
||||
def run_test(self, axis_ctx: AxisTestContext, logger):
|
||||
rated_limit = get_max_rpm(axis_ctx) / 60 * axis_ctx.yaml['encoder-cpr']
|
||||
expected_limit = rated_limit
|
||||
|
||||
# TODO: remove the following two lines, but for now we want to stay away from the modulation depth limit
|
||||
expected_limit *= 0.6
|
||||
rated_limit = expected_limit
|
||||
|
||||
# Add a 10% margin to account for
|
||||
expected_limit *= 0.9
|
||||
|
||||
logger.debug("rated max speed: {}, expected max speed: >= {}".format(rated_limit, expected_limit))
|
||||
#theoretical_limit = 100000
|
||||
|
||||
# Set the current limit accordingly so we don't burn the brake resistor while slowing down
|
||||
if self._override_current_limit is None:
|
||||
set_limits(axis_ctx, logger, vel_limit=rated_limit, current_limit=50)
|
||||
else:
|
||||
axis_ctx.handle.motor.config.current_lim = self._override_current_limit
|
||||
axis_ctx.handle.controller.config.vel_limit = rated_limit
|
||||
axis_ctx.handle.controller.set_vel_setpoint(0, 0)
|
||||
request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
|
||||
logger.debug("Drive current {}A, Load current {}A".format(axis_ctx.handle.motor.config.current_lim, self._load_current))
|
||||
|
||||
ramp_up_time = 15.0
|
||||
max_measured_vel = 0.0
|
||||
logger.debug("ramping to {} over {} s".format(rated_limit, ramp_up_time))
|
||||
t_0 = time.monotonic()
|
||||
last_print = t_0
|
||||
while True:
|
||||
ratio = (time.monotonic() - t_0) / ramp_up_time
|
||||
if ratio >= 1:
|
||||
break
|
||||
|
||||
#TODO based on integrator gain and torque ramp rate
|
||||
expected_ramp_lag = 1.0 * (rated_limit / ramp_up_time)
|
||||
expected_lag = 0
|
||||
|
||||
# While ramping up we want to remain within +-5% of the setpoint.
|
||||
# However we accept if we can only approach 80% of the theoretical limit.
|
||||
vel_setpoint = ratio * rated_limit
|
||||
expected_velocity = max(vel_setpoint - expected_lag, 0)
|
||||
vel_range = max(0.05*expected_velocity, max(expected_lag+expected_ramp_lag, 2000))
|
||||
if expected_velocity - vel_range > expected_limit:
|
||||
vel_range = expected_velocity - expected_limit
|
||||
|
||||
# set and measure velocity
|
||||
axis_ctx.handle.controller.set_vel_setpoint(vel_setpoint, 0)
|
||||
measured_vel = axis_ctx.handle.encoder.vel_estimate
|
||||
max_measured_vel = max(measured_vel, max_measured_vel)
|
||||
test_assert_eq(measured_vel, expected_velocity, range=vel_range)
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
# log progress
|
||||
if time.monotonic() - last_print > 1:
|
||||
last_print = time.monotonic()
|
||||
logger.debug("ramping up: commanded {}, expected {}, measured {} ".format(vel_setpoint, expected_velocity, measured_vel))
|
||||
|
||||
time.sleep(0.001)
|
||||
|
||||
logger.debug("reached top speed of {} counts/sec".format(max_measured_vel))
|
||||
|
||||
if self._brake:
|
||||
axis_ctx.handle.controller.set_vel_setpoint(0, 0)
|
||||
time.sleep(0.5)
|
||||
# If the velocity integrator at work, it may now work against slowing down.
|
||||
test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 0, range=rated_limit*0.3)
|
||||
# TODO: this is not a good bound, but the encoder float resolution results in a bad velocity estimate after this many turns
|
||||
time.sleep(0.5)
|
||||
test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 0, range=2000)
|
||||
request_state(axis_ctx, AXIS_STATE_IDLE)
|
||||
test_assert_no_error(axis_ctx)
|
||||
|
||||
|
||||
class TestHighVelocityInViscousFluid(DualAxisTest):
|
||||
"""
|
||||
Runs TestHighVelocity on one motor while using the other motor as a load.
|
||||
The load is created by running velocity control with setpoint 0.
|
||||
"""
|
||||
def __init__(self, load_current=10, driver_current=20):
|
||||
self._load_current = load_current
|
||||
self._driver_current = driver_current
|
||||
|
||||
def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger):
|
||||
load_ctx = axis0_ctx
|
||||
driver_ctx = axis1_ctx
|
||||
if driver_ctx.name == 'top-odrive.black':
|
||||
# odrive.utils.start_liveplotter(lambda: [driver_ctx.odrv_ctx.handle.vbus_voltage])
|
||||
odrive.utils.start_liveplotter(lambda: [driver_ctx.handle.motor.current_control.Iq_measured,
|
||||
driver_ctx.handle.motor.current_control.Iq_setpoint])
|
||||
|
||||
# Set up viscous fluid load
|
||||
logger.debug("activating load on {}...".format(load_ctx.name))
|
||||
load_ctx.handle.controller.config.vel_integrator_gain = 0
|
||||
load_ctx.handle.motor.config.current_lim = self._load_current
|
||||
load_ctx.odrv_ctx.handle.config.brake_resistance = 0 # disable brake resistance, the power will go into the bus
|
||||
load_ctx.handle.controller.set_vel_setpoint(0, 0)
|
||||
|
||||
request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
|
||||
driver_test = TestHighVelocity(
|
||||
override_current_limit=self._driver_current,
|
||||
load_current=self._load_current, brake=False)
|
||||
driver_test.check_preconditions(driver_ctx, logger)
|
||||
driver_test.run_test(driver_ctx, logger)
|
||||
|
||||
# put load to idle as quickly as possible, otherwise, because the brake resistor is disabled,
|
||||
# it will try to put the braking power into the power rail where it has nowhere to go.
|
||||
request_state(load_ctx, AXIS_STATE_IDLE)
|
||||
request_state(driver_ctx, AXIS_STATE_IDLE)
|
||||
|
||||
class TestSelfLoadedPosVelDistribution(DualAxisTest):
|
||||
"""
|
||||
Uses an ODrive mechanically connected to itself to test a distribution of
|
||||
speeds and currents. Since it's connected to itself, we can be a lot less
|
||||
strict about the brake resistor power use.
|
||||
"""
|
||||
def __init__(self, rpm_range=1000, load_current_range=10, driver_current_lim=20):
|
||||
self._rpm_range = rpm_range
|
||||
self._load_current_range = load_current_range
|
||||
self._driver_current_lim = driver_current_lim
|
||||
|
||||
def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger):
|
||||
load_ctx = axis0_ctx
|
||||
driver_ctx = axis1_ctx
|
||||
|
||||
logger.debug("Iload range: {} A, Idriver: {} A".format(self._load_current_range, self._driver_current_lim))
|
||||
|
||||
# max speed for rig in counts/s for each encoder (may be different CPR)
|
||||
max_rpm = min(self._rpm_range, get_max_rpm(driver_ctx), get_max_rpm(load_ctx))
|
||||
driver_max_speed = max_rpm / 60 * driver_ctx.yaml['encoder-cpr']
|
||||
load_max_speed = max_rpm / 60 * load_ctx.yaml['encoder-cpr']
|
||||
logger.debug("RPM range: {} = driver {} = load {}".format(max_rpm, driver_max_speed, load_max_speed))
|
||||
|
||||
# Set up velocity controlled load
|
||||
logger.debug("activating load on {}".format(load_ctx.name))
|
||||
load_ctx.handle.controller.config.vel_integrator_gain = 0
|
||||
load_ctx.handle.controller.config.vel_limit = load_max_speed
|
||||
load_ctx.handle.motor.config.current_lim = 0 #load current to be set during runtime
|
||||
load_ctx.handle.controller.set_vel_setpoint(0, 0) # vel sign also set during runtime
|
||||
request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
|
||||
# Set up velocity controlled driver
|
||||
logger.debug("activating driver on {}".format(driver_ctx.name))
|
||||
driver_ctx.handle.motor.config.current_lim = self._driver_current_lim
|
||||
driver_ctx.handle.controller.config.vel_limit = driver_max_speed
|
||||
driver_ctx.handle.controller.set_vel_setpoint(0, 0)
|
||||
request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
|
||||
# Spiral parameters
|
||||
command_rate = 500.0 #Hz (nominal, achived rate is less due to time.sleep approx)
|
||||
test_duration = 20.0 #s
|
||||
num_cycles = 3.0 # number of spiral "rotations"
|
||||
|
||||
t_0 = time.monotonic()
|
||||
t_ratio = 0
|
||||
last_print = t_0
|
||||
while t_ratio < 1:
|
||||
t_ratio = (time.monotonic() - t_0) / test_duration
|
||||
phase = 2 * math.pi * num_cycles * t_ratio
|
||||
driver_speed = t_ratio * driver_max_speed * math.sin(phase)
|
||||
# print(driver_speed)
|
||||
driver_ctx.handle.controller.set_vel_setpoint(driver_speed, 0)
|
||||
load_current = t_ratio * self._load_current_range * math.cos(phase)
|
||||
Iload_mag = abs(load_current)
|
||||
Iload_sign = np.sign(load_current)
|
||||
# print("I: {}, vel {}".format(Iload_mag, Iload_sign * load_max_speed))
|
||||
load_ctx.handle.motor.config.current_lim = Iload_mag
|
||||
load_ctx.handle.controller.set_vel_setpoint(Iload_sign * load_max_speed, 0)
|
||||
|
||||
test_assert_no_error(driver_ctx)
|
||||
test_assert_no_error(load_ctx)
|
||||
|
||||
# log progress
|
||||
if time.monotonic() - last_print > 1:
|
||||
last_print = time.monotonic()
|
||||
logger.debug("Envelope -- vel: {:.2f}, I: {:.2f}".format(t_ratio * driver_max_speed, t_ratio * self._load_current_range))
|
||||
|
||||
time.sleep(1/command_rate)
|
||||
|
||||
request_state(load_ctx, AXIS_STATE_IDLE)
|
||||
request_state(driver_ctx, AXIS_STATE_IDLE)
|
||||
test_assert_no_error(driver_ctx)
|
||||
test_assert_no_error(load_ctx)
|
||||
|
||||
class TestVelCtrlVsPosCtrl(DualAxisTest):
|
||||
"""
|
||||
Uses one ODrive as a load operating in velocity control mode.
|
||||
The other ODrive tries to "fight" against the load in position mode.
|
||||
"""
|
||||
def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger):
|
||||
load_ctx = axis0_ctx
|
||||
driver_ctx = axis1_ctx
|
||||
|
||||
# Set up viscous fluid load
|
||||
logger.debug("activating load on {}...".format(load_ctx.name))
|
||||
load_ctx.handle.controller.config.vel_integrator_gain = 0
|
||||
load_ctx.handle.controller.vel_integrator_torque = 0
|
||||
set_limits(load_ctx, logger, vel_limit=100000, current_limit=50)
|
||||
load_ctx.handle.controller.set_vel_setpoint(0, 0)
|
||||
request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
|
||||
# Turn to some position
|
||||
logger.debug("using {} as driver against load, vel=100000...".format(driver_ctx.name))
|
||||
set_limits(driver_ctx, logger, vel_limit=100000, current_limit=50)
|
||||
init_pos = driver_ctx.handle.encoder.pos_estimate
|
||||
driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0)
|
||||
request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
for _ in range(int(4000/5)):
|
||||
logger.debug(str(driver_ctx.handle.motor.current_control.Iq_setpoint))
|
||||
time.sleep(0.005)
|
||||
|
||||
test_assert_no_error(load_ctx)
|
||||
test_assert_no_error(driver_ctx)
|
||||
|
||||
logger.debug("using {} as driver against load, vel=20000...".format(driver_ctx.name))
|
||||
set_limits(driver_ctx, logger, vel_limit=20000, current_limit=50)
|
||||
init_pos = driver_ctx.handle.encoder.pos_estimate
|
||||
driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0)
|
||||
request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
#for _ in range(int(5*4000/5)):
|
||||
# logger.debug(str(driver_ctx.handle.motor.current_control.Iq_setpoint))
|
||||
# time.sleep(0.005)
|
||||
time.sleep(7)
|
||||
|
||||
odrive.utils.print_drv_regs("load motor ({})".format(load_ctx.name), load_ctx.handle.motor)
|
||||
odrive.utils.print_drv_regs("driver motor ({})".format(driver_ctx.name), driver_ctx.handle.motor)
|
||||
|
||||
test_assert_no_error(load_ctx)
|
||||
test_assert_no_error(driver_ctx)
|
||||
|
||||
## Turn to another position
|
||||
#logger.debug("controlling against load, vel=40000...")
|
||||
#set_limits(axis1_ctx, logger, vel_limit=40000, current_limit=20)
|
||||
#init_pos = axis1_ctx.handle.encoder.pos_estimate
|
||||
#axis1_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0)
|
||||
#request_state(axis1_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
|
||||
|
||||
# ASCII protocol helper functions
|
||||
def gcode_calc_checksum(data):
|
||||
from functools import reduce
|
||||
return reduce(lambda a, b: a ^ b, data)
|
||||
def gcode_append_checksum(data):
|
||||
return data + b'*' + str(gcode_calc_checksum(data)).encode('ascii')
|
||||
def get_lines(port):
|
||||
buf = port.get_bytes(512, time.monotonic() + 0.2)
|
||||
return [line.rstrip(b'\r') for line in buf.split(b'\n') if line.rstrip(b'\r')]
|
||||
|
||||
class TestAsciiProtocol(ODriveTest):
|
||||
def run_test(self, odrv_ctx: ODriveTestContext, logger):
|
||||
import odrive.serial_transport
|
||||
port = odrive.serial_transport.SerialStreamTransport(odrv_ctx.yaml['uart'], 115200)
|
||||
|
||||
# send garbage to throw the device off track
|
||||
port.process_bytes(b"garbage\r\n\r\0trash\n")
|
||||
port.process_bytes(b"\n") # start a new clean line
|
||||
get_lines(port) # flush RX buffer
|
||||
|
||||
# info command without checksum
|
||||
port.process_bytes(b"i\n")
|
||||
# check if it reports the serial number (among other things)
|
||||
lines = get_lines(port)
|
||||
expected_line = ('Serial number: ' + odrv_ctx.yaml['serial-number']).encode('ascii')
|
||||
if not expected_line in lines:
|
||||
raise Exception("expected {} in ASCII protocol response but got {}".format(expected_line, str(lines)))
|
||||
|
||||
# info command with checksum
|
||||
port.process_bytes(gcode_append_checksum(b"i") + b" ; a useless comment\n")
|
||||
# check if it reports the serial number with checksum (among other things)
|
||||
lines = get_lines(port)
|
||||
expected_line = gcode_append_checksum(('Serial number: ' + odrv_ctx.yaml['serial-number']).encode('ascii'))
|
||||
if not expected_line in lines:
|
||||
raise Exception("expected {} in ASCII protocol response but got {}".format(expected_line, str(lines)))
|
||||
|
||||
port.process_bytes(b"p 0 2000 -10 0.002\n")
|
||||
time.sleep(0.01) # 1ms is too short, 2ms usually works, 10ms for good measure
|
||||
test_assert_eq(odrv_ctx.handle.axis0.controller.pos_setpoint, 2000, accuracy=0.001)
|
||||
test_assert_eq(odrv_ctx.handle.axis0.controller.vel_setpoint, -10, accuracy=0.001)
|
||||
test_assert_eq(odrv_ctx.handle.axis0.controller.current_setpoint, 0.002, accuracy=0.001)
|
||||
|
||||
port.process_bytes(b"v 1 -21.1 0.32\n")
|
||||
time.sleep(0.01)
|
||||
test_assert_eq(odrv_ctx.handle.axis1.controller.vel_setpoint, -21.1, accuracy=0.001)
|
||||
test_assert_eq(odrv_ctx.handle.axis1.controller.current_setpoint, 0.32, accuracy=0.001)
|
||||
|
||||
port.process_bytes(b"c 0 0.1\n")
|
||||
time.sleep(0.01)
|
||||
test_assert_eq(odrv_ctx.handle.axis0.controller.current_setpoint, 0.1, accuracy=0.001)
|
||||
|
||||
# write arbitrary parameter
|
||||
port.process_bytes(b"w axis0.controller.pos_setpoint -123.456 ; comment\n")
|
||||
time.sleep(0.01)
|
||||
test_assert_eq(odrv_ctx.handle.axis0.controller.pos_setpoint, -123.456, accuracy=0.001)
|
||||
|
||||
port.process_bytes(b"r axis0.controller.pos_setpoint\n")
|
||||
lines = get_lines(port)
|
||||
expected_line = b'-123.4560'
|
||||
if lines != [expected_line]:
|
||||
raise Exception("expected {} in ASCII protocol response but got {}".format(expected_line, str(lines)))
|
||||
|
||||
# read/write enums
|
||||
port.process_bytes(b"r axis0.error\n")
|
||||
lines = get_lines(port)
|
||||
expected_line = b'0'
|
||||
if lines != [expected_line]:
|
||||
raise Exception("expected {} in ASCII protocol response but got {}".format(expected_line, str(lines)))
|
||||
|
||||
test_assert_eq(odrv_ctx.axes[0].handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL)
|
||||
port.process_bytes(b"w axis0.requested_state {}\n".format(AXIS_STATE_IDLE))
|
||||
time.sleep(0.01)
|
||||
test_assert_eq(odrv_ctx.axes[0].handle.current_state, AXIS_STATE_IDLE)
|
||||
|
||||
# disable axes
|
||||
odrv_ctx.handle.axis0.controller.set_pos_setpoint(0, 0, 0)
|
||||
odrv_ctx.handle.axis1.controller.set_pos_setpoint(0, 0, 0)
|
||||
request_state(odrv_ctx.axes[0], AXIS_STATE_IDLE)
|
||||
request_state(odrv_ctx.axes[1], AXIS_STATE_IDLE)
|
||||
|
||||
|
||||
class TestSensorlessControl(AxisTest):
|
||||
def run_test(self, axis_ctx: AxisTestContext, logger):
|
||||
odrv0.axis0.controller.config.vel_gain = 5 / get_sensorless_vel(axis_ctx, 10000)
|
||||
odrv0.axis0.controller.config.vel_integrator_gain = 10 / get_sensorless_vel(axis_ctx, 10000)
|
||||
target_vel = get_sensorless_vel(axis_ctx, 20000)
|
||||
axis_ctx.handle.controller.set_vel_setpoint(target_vel, 0)
|
||||
request_state(axis_ctx, AXIS_STATE_SENSORLESS_CONTROL)
|
||||
# wait for spinup
|
||||
time.sleep(2)
|
||||
test_assert_eq(odrv0.axis0.encoder.vel_estimate, target_vel, range=2000)
|
||||
|
||||
request_state(axis_ctx, AXIS_STATE_IDLE)
|
||||
@@ -0,0 +1,89 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import time
|
||||
import math
|
||||
import os
|
||||
|
||||
from odrive.enums import *
|
||||
from test_runner import *
|
||||
|
||||
|
||||
teensy_code_template = """
|
||||
float position = 0; // between 0 and 1
|
||||
float velocity = 1; // [position per second]
|
||||
|
||||
void setup() {
|
||||
pinMode({pwm_gpio}, OUTPUT);
|
||||
}
|
||||
|
||||
// the loop routine runs over and over again forever:
|
||||
void loop() {
|
||||
int high_microseconds = 1000 + (int)(position * 1000.0f);
|
||||
|
||||
digitalWrite({pwm_gpio}, HIGH);
|
||||
delayMicroseconds(high_microseconds);
|
||||
digitalWrite({pwm_gpio}, LOW);
|
||||
|
||||
// Wait for a total of 20ms.
|
||||
// delayMicroseconds() only works well for values <= 16383
|
||||
delayMicroseconds(10000 - high_microseconds);
|
||||
delayMicroseconds(10000);
|
||||
|
||||
position += velocity * 0.02;
|
||||
while (position > 1.0)
|
||||
position -= 1.0;
|
||||
}
|
||||
"""
|
||||
|
||||
|
||||
class TestPwmInput():
|
||||
"""
|
||||
Verifies the PWM input.
|
||||
|
||||
The Teensy generates a PWM signal that goes from 0% (1ms high) to 100% (2ms high)
|
||||
in 1 second and then resumes at 0%.
|
||||
|
||||
Note: this test is currently only written for ODrive 3.6 (or similar GPIO layout).
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
# Run a separate test for each PWM-capable GPIO. Use different min/max settings for each test.
|
||||
yield (odrive, 1, -50, 200, list(testrig.get_connected_components(odrive.gpio1, TeensyGpio)))
|
||||
yield (odrive, 2, 20, 400, list(testrig.get_connected_components(odrive.gpio2, TeensyGpio)))
|
||||
yield (odrive, 3, -1000, 0, list(testrig.get_connected_components(odrive.gpio3, TeensyGpio)))
|
||||
yield (odrive, 4, -20000, 20000, list(testrig.get_connected_components(odrive.gpio4, TeensyGpio)))
|
||||
|
||||
def run_test(self, odrive: ODriveComponent, odrive_gpio_num: int, min_val: float, max_val: float, teensy_gpio: Component, logger: Logger):
|
||||
teensy = teensy_gpio.parent
|
||||
code = teensy_code_template.replace("{pwm_gpio}", str(teensy_gpio.num))
|
||||
teensy.compile_and_program(code)
|
||||
|
||||
logger.debug("Set up PWM input...")
|
||||
odrive.unuse_gpios()
|
||||
|
||||
pwm_mapping = [
|
||||
odrive.handle.config.gpio1_pwm_mapping,
|
||||
odrive.handle.config.gpio2_pwm_mapping,
|
||||
odrive.handle.config.gpio3_pwm_mapping,
|
||||
odrive.handle.config.gpio4_pwm_mapping
|
||||
][odrive_gpio_num - 1]
|
||||
|
||||
pwm_mapping.endpoint = odrive.handle.axis0.controller._remote_attributes['input_pos']
|
||||
pwm_mapping.min = min_val
|
||||
pwm_mapping.max = max_val
|
||||
|
||||
odrive.save_config_and_reboot()
|
||||
|
||||
data = record_log(lambda: [odrive.handle.axis0.controller.input_pos], duration=5.0)
|
||||
|
||||
full_scale = max_val - min_val
|
||||
slope, offset, fitted_curve = fit_sawtooth(data, min_val, max_val)
|
||||
test_assert_eq(slope, full_scale / 1.0, accuracy=0.001)
|
||||
test_curve_fit(data, fitted_curve, max_mean_err = full_scale * 0.05, inlier_range = full_scale * 0.05, max_outliers = len(data[:,0]) * 0.01)
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run(TestPwmInput())
|
||||
@@ -0,0 +1,92 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import struct
|
||||
import asyncio
|
||||
import time
|
||||
|
||||
from fibre.utils import Logger
|
||||
from odrive.enums import *
|
||||
from test_runner import *
|
||||
|
||||
class TestStepDir():
|
||||
"""
|
||||
Tests Step/Dir input.
|
||||
Not all possible combinations are tested, but each axis and each GPIO
|
||||
participates in at least one test case.
|
||||
|
||||
The tests are conducted while the axis is in idle.
|
||||
"""
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
gpio_conns = [
|
||||
list(testrig.get_connected_components((odrive.gpio1, False), LinuxGpioComponent)),
|
||||
list(testrig.get_connected_components((odrive.gpio2, False), LinuxGpioComponent)),
|
||||
#list(testrig.get_connected_components((odrive.gpio3, False), LinuxGpioComponent)), # connected to LPF on test rig
|
||||
#list(testrig.get_connected_components((odrive.gpio4, False), LinuxGpioComponent)), # connected to LPF on test rig
|
||||
list(testrig.get_connected_components((odrive.gpio5, False), LinuxGpioComponent)),
|
||||
list(testrig.get_connected_components((odrive.gpio6, False), LinuxGpioComponent)),
|
||||
list(testrig.get_connected_components((odrive.gpio7, False), LinuxGpioComponent)),
|
||||
list(testrig.get_connected_components((odrive.gpio8, False), LinuxGpioComponent)),
|
||||
]
|
||||
|
||||
yield (odrive.axes[0], 1, gpio_conns[0], 2, gpio_conns[1])
|
||||
yield (odrive.axes[0], 5, gpio_conns[2], 6, gpio_conns[3])
|
||||
yield (odrive.axes[0], 7, gpio_conns[4], 8, gpio_conns[5]) # broken
|
||||
# yield (odrive.axes[0], 7, gpio_conns[6], 8, gpio_conns[7]) # broken
|
||||
|
||||
yield (odrive.axes[1], 7, gpio_conns[4], 8, gpio_conns[5])
|
||||
|
||||
def run_test(self, axis: ODriveAxisComponent, step_gpio_num: int, step_gpio: LinuxGpioComponent, dir_gpio_num: int, dir_gpio: LinuxGpioComponent, logger: Logger):
|
||||
step_gpio.config(output=True)
|
||||
step_gpio.write(False)
|
||||
dir_gpio.config(output=True)
|
||||
dir_gpio.write(True)
|
||||
|
||||
if axis.num == 0:
|
||||
axis.parent.handle.config.enable_uart = False
|
||||
axis.handle.config.enable_step_dir = True
|
||||
axis.handle.config.step_dir_always_on = True # needed for testing
|
||||
axis.handle.config.step_gpio_pin = step_gpio_num
|
||||
axis.handle.config.dir_gpio_pin = dir_gpio_num
|
||||
request_state(axis, AXIS_STATE_IDLE) # apply step_dir_always_on config
|
||||
|
||||
|
||||
ref = axis.handle.controller.input_pos
|
||||
axis.handle.config.turns_per_step = turns_per_step = 10
|
||||
|
||||
# On the RPi 4 a ~5kHz GPIO signal can be generated from Python
|
||||
|
||||
for i in range(100):
|
||||
step_gpio.write(True)
|
||||
step_gpio.write(False)
|
||||
test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * turns_per_step, range = 0.4 * turns_per_step)
|
||||
|
||||
ref = axis.handle.controller.input_pos
|
||||
dir_gpio.write(False)
|
||||
|
||||
for i in range(100):
|
||||
step_gpio.write(True)
|
||||
step_gpio.write(False)
|
||||
test_assert_eq(axis.handle.controller.input_pos, ref - (i + 1) * turns_per_step, range = 0.4 * turns_per_step)
|
||||
|
||||
ref = axis.handle.controller.input_pos
|
||||
dir_gpio.write(True)
|
||||
axis.handle.config.turns_per_step = turns_per_step = 1
|
||||
|
||||
for i in range(100):
|
||||
step_gpio.write(True)
|
||||
step_gpio.write(False)
|
||||
test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * turns_per_step, range = 0.4 * turns_per_step)
|
||||
|
||||
ref = axis.handle.controller.input_pos
|
||||
axis.handle.config.turns_per_step = turns_per_step = -1
|
||||
|
||||
for i in range(100):
|
||||
step_gpio.write(True)
|
||||
step_gpio.write(False)
|
||||
test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * turns_per_step, range = 0.4 * abs(turns_per_step))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run(TestStepDir())
|
||||
@@ -0,0 +1,811 @@
|
||||
# Provides utilities for standalone test scripts.
|
||||
# This script is not intended to be run directly.
|
||||
|
||||
import sys, os
|
||||
sys.path.append(os.path.join(os.path.dirname(__file__), '..', '..'))
|
||||
|
||||
import stat
|
||||
import odrive
|
||||
from odrive.enums import *
|
||||
import fibre
|
||||
from fibre import Logger, Event
|
||||
import argparse
|
||||
import yaml
|
||||
from inspect import signature
|
||||
import itertools
|
||||
import time
|
||||
import tempfile
|
||||
import io
|
||||
from typing import Union, Tuple
|
||||
|
||||
# needed for curve fitting
|
||||
import numpy as np
|
||||
import scipy.optimize
|
||||
import scipy.ndimage.filters
|
||||
|
||||
|
||||
# Assert utils ----------------------------------------------------------------#
|
||||
|
||||
class TestFailed(Exception):
|
||||
def __init__(self, message):
|
||||
Exception.__init__(self, message)
|
||||
|
||||
def test_assert_eq(observed, expected, range=None, accuracy=None):
|
||||
sign = lambda x: 1 if x >= 0 else -1
|
||||
|
||||
# Comparision with absolute range
|
||||
if not range is None:
|
||||
if (observed < expected - range) or (observed > expected + range):
|
||||
raise TestFailed("value out of range: expected {}+-{} but observed {}".format(expected, range, observed))
|
||||
|
||||
# Comparision with relative range
|
||||
elif not accuracy is None:
|
||||
if sign(observed) != sign(expected) or (abs(observed) < abs(expected) * (1 - accuracy)) or (abs(observed) > abs(expected) * (1 + accuracy)):
|
||||
raise TestFailed("value out of range: expected {}+-{}% but observed {}".format(expected, accuracy*100.0, observed))
|
||||
|
||||
# Exact comparision
|
||||
else:
|
||||
if observed != expected:
|
||||
raise TestFailed("value mismatch: expected {} but observed {}".format(expected, observed))
|
||||
|
||||
def test_assert_within(observed, lower_bound, upper_bound, accuracy=0.0):
|
||||
"""
|
||||
Checks if the value is within the closed interval [lower_bound, upper_bound]
|
||||
The permissible range can be expanded in both direction by the coefficiont "accuracy".
|
||||
I.e. accuracy of 1.0 would expand the range by a total factor of 3.0
|
||||
"""
|
||||
|
||||
lower_bound, upper_bound = (
|
||||
(lower_bound - (upper_bound - lower_bound) * accuracy),
|
||||
(upper_bound + (upper_bound - lower_bound) * accuracy)
|
||||
)
|
||||
|
||||
if (observed < lower_bound) or (observed > upper_bound):
|
||||
raise TestFailed(f"the oberved value {observed} is outside the interval [{lower_bound}, {upper_bound}]")
|
||||
|
||||
|
||||
# Other utils -----------------------------------------------------------------#
|
||||
|
||||
def disjoint_sets(list_of_sets: list):
|
||||
while len(list_of_sets):
|
||||
current_set, list_of_sets = list_of_sets[0], list_of_sets[1:]
|
||||
did_update = True
|
||||
while did_update:
|
||||
did_update = False
|
||||
for i, s in enumerate(list_of_sets):
|
||||
if len(current_set.intersection(s)):
|
||||
current_set = current_set.union(s)
|
||||
list_of_sets = list_of_sets[:i] + list_of_sets[(i+1):]
|
||||
did_update = True
|
||||
yield current_set
|
||||
|
||||
def is_list_like(arg):
|
||||
return hasattr(arg, '__iter__') and not isinstance(arg, str)
|
||||
|
||||
def all_unique(lst):
|
||||
seen = list()
|
||||
return not any(i in seen or seen.append(i) for i in lst)
|
||||
|
||||
def modpm(val, range):
|
||||
return ((val + (range / 2)) % range) - (range / 2)
|
||||
|
||||
def clamp(val, lower_bound, upper_bound):
|
||||
return min(max(val, lower_bound), upper_bound)
|
||||
|
||||
def record_log(data_getter, duration=5.0):
|
||||
logger.debug(f"Recording log for {duration}s...")
|
||||
data = []
|
||||
start = time.monotonic()
|
||||
while time.monotonic() - start < duration:
|
||||
data.append((time.monotonic() - start,) + tuple(data_getter()))
|
||||
return np.array(data)
|
||||
|
||||
def save_log(data, id=None):
|
||||
import json
|
||||
filename = '/tmp/log{}.json'.format('' if id is None else str(id))
|
||||
with open(filename, 'w+') as fp:
|
||||
json.dump(data.tolist(), fp, indent=2)
|
||||
print(f'data saved to {filename}')
|
||||
|
||||
def fit_line(data):
|
||||
func = lambda x, a, b: x*a + b
|
||||
slope, offset = scipy.optimize.curve_fit(func, data[:,0], data[:,1], [1.0, 0])[0]
|
||||
return slope, offset, func(data[:,0], slope, offset)
|
||||
|
||||
def fit_sawtooth(data, min_val, max_val, sigma=10):
|
||||
"""
|
||||
Fits the data to a sawtooth function.
|
||||
Returns the average absolute error and the number of outliers.
|
||||
The sample data must span at least one full period.
|
||||
data is expected to contain one row (t, y) for each sample.
|
||||
"""
|
||||
|
||||
# Sawtooth function with free parameters for period and x-shift
|
||||
func = lambda x, a, b: np.mod(a * x + b, max_val - min_val) + min_val
|
||||
|
||||
# Fit period and x-shift
|
||||
mid_point = (min_val + max_val) / 2
|
||||
filtered_data = scipy.ndimage.filters.gaussian_filter(data[:,1], sigma=sigma)
|
||||
if max_val > min_val:
|
||||
zero_crossings = data[np.where((filtered_data[:-1] > mid_point) & (filtered_data[1:] < mid_point))[0], 0]
|
||||
else:
|
||||
zero_crossings = data[np.where((filtered_data[:-1] < mid_point) & (filtered_data[1:] > mid_point))[0], 0]
|
||||
|
||||
|
||||
if len(zero_crossings) == 0:
|
||||
# No zero-crossing - fit simple line
|
||||
slope, offset, _ = fit_line(data)
|
||||
|
||||
elif len(zero_crossings) == 1:
|
||||
# One zero-crossing - fit line based on the longer half
|
||||
z_index = np.where(data[:,0] > zero_crossings[0])[0][0]
|
||||
if z_index > len(data[:,0]):
|
||||
slope, offset, _ = fit_line(data[:z_index])
|
||||
else:
|
||||
slope, offset, _ = fit_line(data[z_index:])
|
||||
|
||||
else:
|
||||
# Two or more zero-crossings - determine period based on average distance between zero-crossings
|
||||
|
||||
period = (zero_crossings[1:] - zero_crossings[:-1]).mean()
|
||||
slope = (max_val - min_val) / period
|
||||
|
||||
#shift = scipy.optimize.curve_fit(lambda x, b: func(x, period, b), data[:,0], data[:,1], [0.0])[0][0]
|
||||
if np.std(np.mod(zero_crossings, period)) < np.std(np.mod(zero_crossings + period/2, period)):
|
||||
shift = np.mean(np.mod(zero_crossings, period))
|
||||
else:
|
||||
shift = np.mean(np.mod(zero_crossings + period/2, period)) - period/2
|
||||
offset = -slope * shift
|
||||
|
||||
return slope, offset, func(data[:,0], slope, offset)
|
||||
|
||||
def test_curve_fit(data, fitted_curve, max_mean_err, inlier_range, max_outliers):
|
||||
diffs = data[:,1] - fitted_curve
|
||||
|
||||
mean_err = np.abs(diffs).mean()
|
||||
if mean_err > max_mean_err:
|
||||
save_log(np.concatenate([data, np.array([fitted_curve]).transpose()], 1))
|
||||
raise TestFailed("curve fit has too large mean error: {} > {}".format(mean_err, max_mean_err))
|
||||
|
||||
outliers = np.count_nonzero((diffs > inlier_range) | (diffs < -inlier_range))
|
||||
if outliers > max_outliers:
|
||||
save_log(np.concatenate([data, np.array([fitted_curve]).transpose()], 1))
|
||||
raise TestFailed("curve fit has too many outliers (err > {}): {} > {}".format(inlier_range, outliers, max_outliers))
|
||||
|
||||
def test_watchdog(axis, feed_func, logger: Logger):
|
||||
"""
|
||||
Tests the watchdog of one axis, using the provided function to feed the watchdog.
|
||||
This test assumes that the testing host has no more than 300ms random delays.
|
||||
"""
|
||||
start = time.monotonic()
|
||||
axis.config.enable_watchdog = False
|
||||
axis.error = 0
|
||||
axis.config.watchdog_timeout = 1.0
|
||||
axis.watchdog_feed()
|
||||
axis.config.enable_watchdog = True
|
||||
test_assert_eq(axis.error, 0)
|
||||
for _ in range(5): # keep the watchdog alive for 3.5 seconds
|
||||
time.sleep(0.7)
|
||||
logger.debug('feeding watchdog at {}s'.format(time.monotonic() - start))
|
||||
feed_func()
|
||||
err = axis.error
|
||||
logger.debug('checking error at {}s'.format(time.monotonic() - start))
|
||||
test_assert_eq(err, 0)
|
||||
|
||||
logger.debug('letting watchdog expire...')
|
||||
time.sleep(1.3) # let the watchdog expire
|
||||
test_assert_eq(axis.error, AXIS_ERROR_WATCHDOG_TIMER_EXPIRED)
|
||||
|
||||
# Test Components -------------------------------------------------------------#
|
||||
|
||||
class Component(object):
|
||||
def __init__(self, parent):
|
||||
self.parent = parent
|
||||
|
||||
class ODriveComponent(Component):
|
||||
def __init__(self, yaml: dict):
|
||||
self.handle = None
|
||||
self.yaml = yaml
|
||||
#self.axes = [ODriveAxisComponent(None), ODriveAxisComponent(None)]
|
||||
self.encoders = [ODriveEncoderComponent(self, 0, yaml['encoder0']), ODriveEncoderComponent(self, 1, yaml['encoder1'])]
|
||||
self.axes = [ODriveAxisComponent(self, 0, yaml['motor0']), ODriveAxisComponent(self, 1, yaml['motor1'])]
|
||||
for i in range(1,9):
|
||||
self.__setattr__('gpio' + str(i), Component(self))
|
||||
self.can = Component(self)
|
||||
self.sck = Component(self)
|
||||
self.miso = Component(self)
|
||||
self.mosi = Component(self)
|
||||
|
||||
def get_subcomponents(self):
|
||||
for enc_ctx in self.encoders:
|
||||
yield 'encoder' + str(enc_ctx.num), enc_ctx
|
||||
for axis_ctx in self.axes:
|
||||
yield 'axis' + str(axis_ctx.num), axis_ctx
|
||||
for i in range(1,9):
|
||||
yield ('gpio' + str(i)), getattr(self, 'gpio' + str(i))
|
||||
yield 'can', self.can
|
||||
yield 'spi.sck', self.sck
|
||||
yield 'spi.miso', self.miso
|
||||
yield 'spi.mosi', self.mosi
|
||||
|
||||
def prepare(self, logger: Logger):
|
||||
"""
|
||||
Connects to the ODrive
|
||||
"""
|
||||
if not self.handle is None:
|
||||
return
|
||||
|
||||
logger.debug('waiting for {} ({})'.format(self.yaml['name'], self.yaml['serial-number']))
|
||||
self.handle = odrive.find_any(
|
||||
path="usb", serial_number=self.yaml['serial-number'], timeout=60)#, printer=print)
|
||||
assert(self.handle)
|
||||
#for axis_idx, axis_ctx in enumerate(self.axes):
|
||||
# axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)]
|
||||
for encoder_idx, encoder_ctx in enumerate(self.encoders):
|
||||
encoder_ctx.handle = self.handle.__dict__['axis{}'.format(encoder_idx)].encoder
|
||||
# TODO: distinguish between axis and motor context
|
||||
for axis_idx, axis_ctx in enumerate(self.axes):
|
||||
axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)]
|
||||
|
||||
def unuse_gpios(self):
|
||||
self.handle.config.enable_uart = False
|
||||
self.handle.axis0.config.enable_step_dir = False
|
||||
self.handle.axis1.config.enable_step_dir = False
|
||||
self.handle.config.gpio1_pwm_mapping.endpoint = None
|
||||
self.handle.config.gpio2_pwm_mapping.endpoint = None
|
||||
self.handle.config.gpio3_pwm_mapping.endpoint = None
|
||||
self.handle.config.gpio4_pwm_mapping.endpoint = None
|
||||
self.handle.config.gpio3_analog_mapping.endpoint = None
|
||||
self.handle.config.gpio4_analog_mapping.endpoint = None
|
||||
|
||||
def save_config_and_reboot(self):
|
||||
self.handle.save_configuration()
|
||||
try:
|
||||
self.handle.reboot()
|
||||
except fibre.ChannelBrokenException:
|
||||
pass # this is expected
|
||||
self.handle = None
|
||||
time.sleep(2)
|
||||
self.prepare(logger)
|
||||
|
||||
def erase_config_and_reboot(self):
|
||||
try:
|
||||
self.handle.erase_configuration()
|
||||
except fibre.ChannelBrokenException:
|
||||
pass # this is expected
|
||||
self.handle = None
|
||||
time.sleep(2)
|
||||
self.prepare(logger)
|
||||
|
||||
class MotorComponent(Component):
|
||||
def __init__(self, yaml: dict):
|
||||
self.yaml = yaml
|
||||
|
||||
def prepare(self, logger: Logger):
|
||||
pass
|
||||
|
||||
class ODriveAxisComponent(Component):
|
||||
def __init__(self, parent: ODriveComponent, num: int, yaml: dict):
|
||||
Component.__init__(self, parent)
|
||||
self.handle = None
|
||||
self.yaml = yaml # TODO: this is bad naming
|
||||
self.num = num
|
||||
|
||||
def prepare(self, logger: Logger):
|
||||
self.parent.prepare(logger)
|
||||
|
||||
class ODriveEncoderComponent(Component):
|
||||
def __init__(self, parent: ODriveComponent, num: int, yaml: dict):
|
||||
Component.__init__(self, parent)
|
||||
self.handle = None
|
||||
self.yaml = yaml
|
||||
self.num = num
|
||||
self.z = Component(self)
|
||||
self.a = Component(self)
|
||||
self.b = Component(self)
|
||||
|
||||
def get_subcomponents(self):
|
||||
return [('z', self.z), ('a', self.a), ('b', self.b)]
|
||||
|
||||
def prepare(self, logger: Logger):
|
||||
self.parent.prepare(logger)
|
||||
|
||||
class EncoderComponent(Component):
|
||||
def __init__(self, parent: Component, yaml: dict):
|
||||
Component.__init__(self, parent)
|
||||
self.yaml = yaml
|
||||
self.z = Component(self)
|
||||
self.a = Component(self)
|
||||
self.b = Component(self)
|
||||
|
||||
def get_subcomponents(self):
|
||||
return [('z', self.z), ('a', self.a), ('b', self.b)]
|
||||
|
||||
class GeneralPurposeComponent(Component):
|
||||
def __init__(self, yaml: dict):
|
||||
self.components = {}
|
||||
for component_yaml in yaml.get('components', []):
|
||||
if component_yaml['type'] == 'can':
|
||||
self.components[component_yaml['name']] = CanInterfaceComponent(self, component_yaml)
|
||||
if component_yaml['type'] == 'uart':
|
||||
self.components[component_yaml['name']] = SerialPortComponent(self, component_yaml)
|
||||
if component_yaml['type'] == 'gpio':
|
||||
self.components['gpio' + str(component_yaml['num'])] = LinuxGpioComponent(self, component_yaml)
|
||||
|
||||
def get_subcomponents(self):
|
||||
return self.components.items()
|
||||
|
||||
class LinuxGpioComponent(Component):
|
||||
def __init__(self, parent: Component, yaml: dict):
|
||||
Component.__init__(self, parent)
|
||||
self.num = int(yaml['num'])
|
||||
|
||||
def config(self, output: bool):
|
||||
with open("/sys/class/gpio/gpio{}/direction".format(self.num), "w") as fp:
|
||||
fp.write('out' if output else '0')
|
||||
|
||||
def write(self, state: bool):
|
||||
with open("/sys/class/gpio/gpio{}/value".format(self.num), "w") as fp:
|
||||
fp.write('1' if state else '0')
|
||||
|
||||
|
||||
class SerialPortComponent(Component):
|
||||
def __init__(self, parent: Component, yaml: dict):
|
||||
Component.__init__(self, parent)
|
||||
self.yaml = yaml
|
||||
|
||||
def get_subcomponents(self):
|
||||
yield 'tx', Component(self)
|
||||
yield 'rx', Component(self)
|
||||
|
||||
def open(self, baudrate: int):
|
||||
import serial
|
||||
return serial.Serial(self.yaml['port'], baudrate, timeout=1)
|
||||
|
||||
class CanInterfaceComponent(Component):
|
||||
def __init__(self, parent: Component, yaml: dict):
|
||||
Component.__init__(self, parent)
|
||||
self.handle = None
|
||||
self.yaml = yaml
|
||||
|
||||
def prepare(self, logger: Logger):
|
||||
if not self.handle is None:
|
||||
return
|
||||
|
||||
import can
|
||||
self.handle = can.interface.Bus(bustype='socketcan', channel=self.yaml['interface'], bitrate=250000)
|
||||
|
||||
class TeensyGpio(Component):
|
||||
def __init__(self, parent: Component, num: int):
|
||||
Component.__init__(self, parent)
|
||||
self.num = num
|
||||
|
||||
class TeensyComponent(Component):
|
||||
def __init__(self, testrig, yaml: dict):
|
||||
self.testrig = testrig
|
||||
self.yaml = yaml
|
||||
self.gpios = [TeensyGpio(self, i) for i in range(24)]
|
||||
self.routes = []
|
||||
self.previous_routes = object()
|
||||
|
||||
def get_subcomponents(self):
|
||||
for i, gpio in enumerate(self.gpios):
|
||||
yield ('gpio' + str(i)), gpio
|
||||
yield 'program', Component(self)
|
||||
|
||||
def add_route(self, input: TeensyGpio, output: TeensyGpio, noise_enable: TeensyGpio):
|
||||
self.routes.append((input, output, noise_enable))
|
||||
|
||||
def commit_routing_config(self, logger: Logger):
|
||||
if self.previous_routes == self.routes:
|
||||
self.routes = []
|
||||
return
|
||||
|
||||
code = ''
|
||||
code += 'bool noise = false;\n'
|
||||
code += 'void setup() {\n'
|
||||
for i, o, n in self.routes:
|
||||
code += ' pinMode({}, OUTPUT);\n'.format(o.num)
|
||||
code += '}\n'
|
||||
code += 'void loop() {\n'
|
||||
code += ' noise = !noise;\n'
|
||||
for i, o, n in self.routes:
|
||||
if n:
|
||||
# with noise enable
|
||||
code += ' digitalWrite({}, digitalRead({}) ? noise : digitalRead({}));\n'.format(o.num, n.num, i.num)
|
||||
else:
|
||||
# no noise enable
|
||||
code += ' digitalWrite({}, digitalRead({}));\n'.format(o.num, i.num)
|
||||
code += '}\n'
|
||||
|
||||
self.compile_and_program(code)
|
||||
|
||||
self.previous_routes = self.routes
|
||||
self.routes = []
|
||||
|
||||
def compile(self, sketchfile, hexfile):
|
||||
env = os.environ.copy()
|
||||
env['ARDUINO_COMPILE_DESTINATION'] = hexfile
|
||||
run_shell(
|
||||
['arduino', '--board', 'teensy:avr:teensy40', '--verify', sketchfile],
|
||||
logger, env = env, timeout = 120)
|
||||
|
||||
def program(self, hex_file_path: str, logger: Logger):
|
||||
"""
|
||||
Programs the specified hex file onto the Teensy.
|
||||
To reset the Teensy, a GPIO of the local system must be connected to the
|
||||
Teensy's "Program" pin.
|
||||
"""
|
||||
|
||||
# todo: this should be treated like a regular setup resource
|
||||
program_gpio = self.testrig.get_directly_connected_components(self.testrig.get_component_name(self) + '.program')[0]
|
||||
|
||||
# Put Teensy into program mode by pulling it's program pin down
|
||||
program_gpio.config(output = True)
|
||||
program_gpio.write(False)
|
||||
time.sleep(0.1)
|
||||
program_gpio.write(True)
|
||||
|
||||
run_shell(["teensy-loader-cli", "-mmcu=imxrt1062", "-w", hex_file_path], logger, timeout = 5)
|
||||
time.sleep(0.5) # give it some time to boot
|
||||
|
||||
def compile_and_program(self, code: str):
|
||||
with tempfile.TemporaryDirectory() as temp_dir:
|
||||
with open(os.path.join(temp_dir, 'code.ino'), 'w+') as code_fp:
|
||||
code_fp.write(code)
|
||||
code_fp.flush()
|
||||
code_fp.seek(0)
|
||||
print('Writing code to teensy: ')
|
||||
print(code_fp.read())
|
||||
with tempfile.NamedTemporaryFile(suffix='.hex') as hex_fp:
|
||||
self.compile(code_fp.name, hex_fp.name)
|
||||
self.program(hex_fp.name, logger)
|
||||
|
||||
class LowPassFilterComponent(Component):
|
||||
def __init__(self, parent: Component):
|
||||
Component.__init__(self, parent)
|
||||
self.en = Component(self)
|
||||
|
||||
def get_subcomponents(self):
|
||||
yield 'en', self.en
|
||||
|
||||
|
||||
class ProxiedComponent(Component):
|
||||
def __init__(self, impl, *gpio_tuples):
|
||||
"""
|
||||
Each element in gpio_tuples should be a tuple of the form:
|
||||
(teensy: TeensyComponent, gpio_in, gpio_out, gpio_noise_enable)
|
||||
"""
|
||||
Component.__init__(self, getattr(impl, 'parent', None))
|
||||
self.impl = impl
|
||||
assert(all([len(t) == 4 for t in gpio_tuples]))
|
||||
self.gpio_tuples = list(gpio_tuples)
|
||||
|
||||
def __repr__(self):
|
||||
return testrig.get_component_name(self.impl) + ' (routed via ' + ', '.join((testrig.get_component_name(t) + ': ' + str(i.num) + ' => ' + str(o.num)) for t, i, o, n in self.gpio_tuples) + ')'
|
||||
|
||||
def __eq__(self, obj):
|
||||
return isinstance(obj, ProxiedComponent) and (self.impl == obj.impl) # and (self.gpio_tuples == obj.gpio_tuples)
|
||||
|
||||
def prepare(self):
|
||||
for teensy, gpio_in, gpio_out, gpio_noise_enable in self.gpio_tuples:
|
||||
teensy.add_route(gpio_in, gpio_out, gpio_noise_enable)
|
||||
|
||||
class TestRig():
|
||||
def __init__(self, yaml: dict, logger: Logger):
|
||||
# Contains all components (including subcomponents).
|
||||
# Ports are components too.
|
||||
self.components_by_name = {} # {'name': object, ...}
|
||||
self.names_by_component = {} # {'name': object, ...}
|
||||
|
||||
def add_component(name, component):
|
||||
self.components_by_name[name] = component
|
||||
self.names_by_component[component] = name
|
||||
if hasattr(component, 'get_subcomponents'):
|
||||
for subname, subcomponent in component.get_subcomponents():
|
||||
add_component(name + '.' + subname, subcomponent)
|
||||
|
||||
for component_yaml in yaml['components']:
|
||||
if component_yaml['type'] == 'odrive':
|
||||
add_component(component_yaml['name'], ODriveComponent(component_yaml))
|
||||
elif component_yaml['type'] == 'generalpurpose':
|
||||
add_component(component_yaml['name'], GeneralPurposeComponent(component_yaml))
|
||||
elif component_yaml['type'] == 'teensy':
|
||||
add_component(component_yaml['name'], TeensyComponent(self, component_yaml))
|
||||
elif component_yaml['type'] == 'motor':
|
||||
add_component(component_yaml['name'], MotorComponent(component_yaml))
|
||||
elif component_yaml['type'] == 'encoder':
|
||||
add_component(component_yaml['name'], EncoderComponent(self, component_yaml))
|
||||
elif component_yaml['type'] == 'lpf':
|
||||
add_component(component_yaml['name'], LowPassFilterComponent(self))
|
||||
else:
|
||||
logger.warn('test rig has unsupported component ' + component_yaml['type'])
|
||||
continue
|
||||
|
||||
# List of disjunct sets, where each set holds references of the mutually connected components
|
||||
self.connections = []
|
||||
for connection_yaml in yaml['connections']:
|
||||
self.connections.append(set(self.components_by_name[name] for name in connection_yaml))
|
||||
self.connections = list(disjoint_sets(self.connections))
|
||||
|
||||
# Dict for fast lookup of the connection sets for each port
|
||||
self.net_by_component = {}
|
||||
for s in self.connections:
|
||||
for port in s:
|
||||
self.net_by_component[port] = s
|
||||
|
||||
def get_components(self, t: type):
|
||||
"""Returns a tuple (name, component) for all components that are of the specified type"""
|
||||
return (comp for comp in self.names_by_component.keys() if isinstance(comp, t))
|
||||
|
||||
def get_component_name(self, component: Component):
|
||||
if isinstance(component, ProxiedComponent):
|
||||
return self.names_by_component[component.impl]
|
||||
else:
|
||||
return self.names_by_component[component]
|
||||
|
||||
def get_directly_connected_components(self, component: Union[str, Component]):
|
||||
"""
|
||||
Returns all components that are directly connected to the specified
|
||||
component, excluding the specified component itself.
|
||||
"""
|
||||
if isinstance(component, str):
|
||||
component = self.components_by_name[component]
|
||||
result = self.net_by_component.get(component, set([component]))
|
||||
return [c for c in result if (c != component)]
|
||||
|
||||
def get_connected_components(self, src: Union[dict, Tuple[Union[Component, str], bool]], comp_type: type = None):
|
||||
"""
|
||||
Returns all components that are either directly or indirectly (through a
|
||||
Teensy) connected to the specified component(s).
|
||||
|
||||
component: Either:
|
||||
- A component object.
|
||||
- A component name given as string.
|
||||
- A tuple of the form (comp, dir) where comp is a component object
|
||||
or name and dir specifies the data direction.
|
||||
The direction is required if routing through a Teensy should be
|
||||
considered.
|
||||
- A dict {sumcomponent: val} where subcomponent is a string
|
||||
such as 'tx' or 'rx' and val is of one of the forms described above.
|
||||
|
||||
A type can be specified to filter the connected components.
|
||||
"""
|
||||
|
||||
if isinstance(src, dict):
|
||||
component_list = []
|
||||
for name, subsrc in src.items():
|
||||
component_list.append([c for c in self.get_connected_components(subsrc) if self.get_component_name(c).endswith('.' + name)])
|
||||
|
||||
for combination in itertools.product(*component_list):
|
||||
if len(set(c.parent for c in combination)) != 1:
|
||||
continue # parent of the components don't match
|
||||
proxied_dst = combination[0].parent
|
||||
if comp_type and not isinstance(proxied_dst, comp_type):
|
||||
continue # not the requested type
|
||||
gpio_tuples = [c2 for c in combination for c2 in c.gpio_tuples if isinstance(c, ProxiedComponent)]
|
||||
if len(gpio_tuples):
|
||||
yield ProxiedComponent(proxied_dst, *gpio_tuples)
|
||||
else:
|
||||
yield proxied_dst
|
||||
|
||||
else:
|
||||
|
||||
if isinstance(src, tuple):
|
||||
src, dir = src
|
||||
else:
|
||||
dir = None
|
||||
|
||||
for dst in self.get_directly_connected_components(src):
|
||||
if (not comp_type) or isinstance(dst, comp_type):
|
||||
yield dst
|
||||
|
||||
if (not dir is None) and isinstance(getattr(dst, 'parent', None), TeensyComponent):
|
||||
teensy = dst.parent
|
||||
for gpio2 in teensy.gpios:
|
||||
for proxied_dst in self.get_directly_connected_components(gpio2):
|
||||
if (not comp_type) or isinstance(proxied_dst, comp_type):
|
||||
yield ProxiedComponent(proxied_dst, (teensy, dst if dir else gpio2, gpio2 if dir else dst, None))
|
||||
|
||||
|
||||
# Helper functions ------------------------------------------------------------#
|
||||
|
||||
def request_state(axis_ctx: ODriveAxisComponent, state, expect_success=True):
|
||||
axis_ctx.handle.requested_state = state
|
||||
time.sleep(0.001)
|
||||
if expect_success:
|
||||
test_assert_eq(axis_ctx.handle.current_state, state)
|
||||
else:
|
||||
test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE)
|
||||
test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_INVALID_STATE)
|
||||
axis_ctx.handle.error = AXIS_ERROR_NONE # reset error
|
||||
|
||||
def get_errors(axis_ctx: ODriveAxisComponent):
|
||||
errors = []
|
||||
if axis_ctx.handle.motor.error != 0:
|
||||
errors.append("motor failed with error 0x{:04X}".format(axis_ctx.handle.motor.error))
|
||||
if axis_ctx.handle.encoder.error != 0:
|
||||
errors.append("encoder failed with error 0x{:04X}".format(axis_ctx.handle.encoder.error))
|
||||
if axis_ctx.handle.sensorless_estimator.error != 0:
|
||||
errors.append("sensorless_estimator failed with error 0x{:04X}".format(axis_ctx.handle.sensorless_estimator.error))
|
||||
if axis_ctx.handle.error != 0:
|
||||
errors.append("axis failed with error 0x{:04X}".format(axis_ctx.handle.error))
|
||||
elif len(errors) > 0:
|
||||
errors.append("and by the way: axis reports no error even though there is one")
|
||||
return errors
|
||||
|
||||
def test_assert_no_error(axis_ctx: ODriveAxisComponent):
|
||||
errors = get_errors(axis_ctx)
|
||||
if len(errors) > 0:
|
||||
raise TestFailed("\n".join(errors))
|
||||
|
||||
def run_shell(command_line, logger, env=None, timeout=None):
|
||||
"""
|
||||
Runs a shell command in the current directory
|
||||
"""
|
||||
import shlex
|
||||
import subprocess
|
||||
logger.debug("invoke: " + str(command_line))
|
||||
if isinstance(command_line, list):
|
||||
cmd = command_line
|
||||
else:
|
||||
cmd = shlex.split(command_line)
|
||||
result = subprocess.run(cmd, timeout=timeout,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.STDOUT,
|
||||
env=env)
|
||||
if result.returncode != 0:
|
||||
logger.error(result.stdout.decode(sys.stdout.encoding))
|
||||
raise TestFailed("command {} failed".format(command_line))
|
||||
|
||||
|
||||
def get_combinations(param_options):
|
||||
if isinstance(param_options, tuple):
|
||||
if len(param_options) > 0:
|
||||
for part1, part2 in itertools.product(
|
||||
get_combinations(param_options[0]),
|
||||
get_combinations(param_options[1:]) if (len(param_options) > 1) else [()]):
|
||||
assert(isinstance(part1, tuple))
|
||||
assert(isinstance(part2, tuple))
|
||||
yield part1 + part2
|
||||
elif is_list_like(param_options):
|
||||
for item in param_options:
|
||||
for c in get_combinations(item):
|
||||
yield c
|
||||
else:
|
||||
yield (param_options,)
|
||||
|
||||
def select_params(param_options):
|
||||
# Select parameters from the resource list
|
||||
# (this could be arbitrarily complex to improve parallelization of the tests)
|
||||
for combination in get_combinations(param_options):
|
||||
if all_unique([x for x in combination if isinstance(x, Component)]):
|
||||
return list(combination)
|
||||
|
||||
return None
|
||||
|
||||
def run(tests):
|
||||
if not isinstance(tests, list):
|
||||
tests = [tests]
|
||||
|
||||
for test in tests:
|
||||
# The result of get_test_cases can be described in ABNF grammar:
|
||||
# test-case-list = *arglist
|
||||
# arglist = *flexible-arg
|
||||
# flexible-arg = component / *argvariant
|
||||
# argvariant = component / arglist
|
||||
#
|
||||
# If for a particular test-case, the components are not given plainly
|
||||
# but in some selectable form, the test driver will select exactly one
|
||||
# of those options.
|
||||
# In other words, it will bring arglist from the form *flexible-arg
|
||||
# into the form *component before calling the test.
|
||||
#
|
||||
# All of the provided test-cases are executed. If none is provided,
|
||||
# a warning is reported. A warning is also reported if for a particular
|
||||
# test case no component combination can be resolved.
|
||||
|
||||
test_cases = list(test.get_test_cases(testrig))
|
||||
|
||||
if len(test_cases) == 0:
|
||||
logger.warn('no test cases are available to conduct the test {}'.format(type(test).__name__))
|
||||
continue
|
||||
|
||||
for test_case in test_cases:
|
||||
params = select_params(test_case)
|
||||
if params is None:
|
||||
logger.warn('no resources are available to conduct the test {}'.format(type(test).__name__))
|
||||
continue
|
||||
|
||||
logger.notify('* preparing {} with {}...'.format(type(test).__name__,
|
||||
[(testrig.get_component_name(p) if isinstance(p, Component) else str(p)) for p in params]))
|
||||
|
||||
teensies = set()
|
||||
for param in params:
|
||||
if isinstance(param, ProxiedComponent):
|
||||
param.prepare()
|
||||
for teensy, _, _, _ in param.gpio_tuples:
|
||||
teensies.add(teensy)
|
||||
|
||||
for teensy in teensies:
|
||||
teensy.commit_routing_config(logger)
|
||||
|
||||
# prepare all components
|
||||
teensies = set()
|
||||
for param in params:
|
||||
if isinstance(param, ProxiedComponent):
|
||||
continue
|
||||
if hasattr(param, 'prepare'):
|
||||
param.prepare(logger)
|
||||
|
||||
logger.notify('* running {} on {}...'.format(type(test).__name__,
|
||||
[(testrig.get_component_name(p) if isinstance(p, Component) else str(p)) for p in params]))
|
||||
|
||||
# Resolve routed components
|
||||
for i, param in enumerate(params):
|
||||
if isinstance(param, ProxiedComponent):
|
||||
params[i] = param.impl
|
||||
|
||||
test.run_test(*params, logger)
|
||||
|
||||
|
||||
logger.success('All tests passed!')
|
||||
|
||||
|
||||
# Load test engine ------------------------------------------------------------#
|
||||
|
||||
# Parse arguments
|
||||
parser = argparse.ArgumentParser(description='ODrive automated test tool\n')
|
||||
parser.add_argument("--ignore", metavar='DEVICE', action='store', nargs='+',
|
||||
help="Ignore (disable) one or more components of the test rig")
|
||||
# TODO: implement
|
||||
parser.add_argument("--test-rig-yaml", type=argparse.FileType('r'), required=True,
|
||||
help="test rig YAML file")
|
||||
parser.add_argument("--setup-host", action='store_true', default=False,
|
||||
help="configure operating system functions such as GPIOs (requires root)")
|
||||
parser.set_defaults(ignore=[])
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# Load objects
|
||||
test_rig_yaml = yaml.load(args.test_rig_yaml, Loader=yaml.BaseLoader)
|
||||
logger = Logger()
|
||||
|
||||
testrig = TestRig(test_rig_yaml, logger)
|
||||
|
||||
|
||||
if args.setup_host:
|
||||
for gpio in testrig.get_components(LinuxGpioComponent):
|
||||
num = gpio.num
|
||||
logger.debug('exporting GPIO ' + str(num) + ' to user space...')
|
||||
if not os.path.isdir("/sys/class/gpio/gpio{}".format(num)):
|
||||
with open("/sys/class/gpio/export", "w") as fp:
|
||||
fp.write(str(num))
|
||||
os.chmod("/sys/class/gpio/gpio{}/value".format(num), stat.S_IRWXU | stat.S_IRWXG | stat.S_IRWXO)
|
||||
os.chmod("/sys/class/gpio/gpio{}/direction".format(num), stat.S_IRWXU | stat.S_IRWXG | stat.S_IRWXO)
|
||||
|
||||
for port in testrig.get_components(SerialPortComponent):
|
||||
logger.debug('changing permissions on ' + port.yaml['port'] + '...')
|
||||
os.chmod(port.yaml['port'], stat.S_IRWXU | stat.S_IRWXG | stat.S_IRWXO)
|
||||
|
||||
if len(list(testrig.get_components(TeensyComponent))):
|
||||
# This breaks the annoying teensy loader that shows up on every compile
|
||||
logger.debug('modifying teensyduino installation...')
|
||||
if not os.path.isfile('/usr/share/arduino/hardware/tools/teensy_post_compile_old'):
|
||||
os.rename('/usr/share/arduino/hardware/tools/teensy_post_compile', '/usr/share/arduino/hardware/tools/teensy_post_compile_old')
|
||||
with open('/usr/share/arduino/hardware/tools/teensy_post_compile', 'w') as scr:
|
||||
scr.write('#!/usr/bin/env bash\n')
|
||||
scr.write('if [ "$ARDUINO_COMPILE_DESTINATION" != "" ]; then\n')
|
||||
scr.write(' cp -r ${2#-path=}/*.ino.hex ${ARDUINO_COMPILE_DESTINATION}\n')
|
||||
scr.write('fi\n')
|
||||
os.chmod('/usr/share/arduino/hardware/tools/teensy_post_compile', stat.S_IRWXU | stat.S_IRGRP | stat.S_IXGRP | stat.S_IROTH | stat.S_IXOTH)
|
||||
|
||||
# Bring up CAN interface(s)
|
||||
for intf in testrig.get_components(CanInterfaceComponent):
|
||||
name = intf.yaml['interface']
|
||||
logger.debug('bringing up {}...'.format(name))
|
||||
run_shell('ip link set dev {} down'.format(name), logger)
|
||||
run_shell('ip link set dev {} type can bitrate 250000'.format(name), logger)
|
||||
run_shell('ip link set dev {} type can loopback off'.format(name), logger)
|
||||
run_shell('ip link set dev {} up'.format(name), logger)
|
||||
|
||||
@@ -0,0 +1,301 @@
|
||||
|
||||
import test_runner
|
||||
|
||||
import struct
|
||||
import time
|
||||
import os
|
||||
import io
|
||||
import functools
|
||||
import operator
|
||||
|
||||
from fibre.utils import Logger
|
||||
from odrive.enums import *
|
||||
from test_runner import *
|
||||
|
||||
|
||||
def append_checksum(command):
|
||||
return command + b'*' + str(functools.reduce(operator.xor, command)).encode('ascii')
|
||||
|
||||
def strip_checksum(command):
|
||||
command, _, checksum = command.partition(b'*')
|
||||
test_assert_eq(int(checksum.strip()), functools.reduce(operator.xor, command))
|
||||
return command
|
||||
|
||||
def reset_state(ser):
|
||||
"""Resets the state of the ASCII protocol by flushing all buffers"""
|
||||
ser.flushOutput() # ensure that all previous bytes are sent
|
||||
time.sleep(0.1) # wait for ODrive to handle last input (buffer might be full)
|
||||
ser.write(b'\n') # terminate line
|
||||
ser.flushOutput() # ensure that end-of-line is sent
|
||||
time.sleep(0.1) # wait for any response that this may generate
|
||||
ser.flushInput() # discard response
|
||||
|
||||
class TestUartAscii():
|
||||
"""
|
||||
Tests the most important functions of the ASCII protocol.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
ports = list(testrig.get_connected_components({
|
||||
'rx': (odrive.gpio1, True),
|
||||
'tx': (odrive.gpio2, False)
|
||||
}, SerialPortComponent))
|
||||
yield (odrive, ports)
|
||||
|
||||
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 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
|
||||
|
||||
|
||||
# TODO: test cases for 't', 'ss', 'se', 'sr' commands
|
||||
|
||||
|
||||
class TestUartBaudrate():
|
||||
"""
|
||||
Tests if the UART baudrate setting works as intended.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
ports = list(testrig.get_connected_components({
|
||||
'rx': (odrive.gpio1, True),
|
||||
'tx': (odrive.gpio2, False)
|
||||
}, SerialPortComponent))
|
||||
yield (odrive, ports)
|
||||
|
||||
def run_test(self, odrive: ODriveComponent, port: SerialPortComponent, logger: Logger):
|
||||
odrive.handle.axis0.config.enable_step_dir = False
|
||||
odrive.handle.config.enable_uart = True
|
||||
|
||||
odrive.handle.config.uart_baudrate = 9600
|
||||
odrive.save_config_and_reboot()
|
||||
|
||||
# Control test: talk to the ODrive with the wrong baudrate
|
||||
with port.open(115200) as ser:
|
||||
# reset port to known state
|
||||
reset_state(ser)
|
||||
|
||||
ser.write(b'r vbus_voltage\n')
|
||||
test_assert_eq(ser.readline().strip(), b'')
|
||||
|
||||
with port.open(9600) as ser:
|
||||
# reset port to known state
|
||||
reset_state(ser)
|
||||
|
||||
# Check if protocol works
|
||||
ser.write(b'r vbus_voltage\n')
|
||||
response = float(ser.readline().strip())
|
||||
test_assert_eq(response, odrive.handle.vbus_voltage, accuracy=0.1)
|
||||
|
||||
odrive.handle.config.uart_baudrate = 115200
|
||||
odrive.save_config_and_reboot()
|
||||
|
||||
|
||||
class TestUartBurnIn():
|
||||
"""
|
||||
Tests if the ASCII protocol can handle 64kB of random data being thrown at it.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
ports = list(testrig.get_connected_components({
|
||||
'rx': (odrive.gpio1, True),
|
||||
'tx': (odrive.gpio2, False)
|
||||
}, SerialPortComponent))
|
||||
yield (odrive, ports)
|
||||
|
||||
def run_test(self, odrive: ODriveComponent, port: SerialPortComponent, logger: Logger):
|
||||
odrive.handle.axis0.config.enable_step_dir = False
|
||||
odrive.handle.config.enable_uart = True
|
||||
|
||||
with port.open(115200) as ser:
|
||||
with open('/dev/random', 'rb') as rand:
|
||||
buf = rand.read(65536)
|
||||
ser.write(buf)
|
||||
|
||||
# reset port to known state
|
||||
reset_state(ser)
|
||||
|
||||
# Check if protocol still works
|
||||
ser.write(b'r vbus_voltage\n')
|
||||
response = float(ser.readline().strip())
|
||||
test_assert_eq(response, odrive.handle.vbus_voltage, accuracy=0.1)
|
||||
|
||||
|
||||
class TestUartNoise():
|
||||
"""
|
||||
Tests if the UART can handle invalid signals.
|
||||
"""
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
# For every ODrive, find a connected serial port which has a teensy
|
||||
# in between, so that we can inject noise,
|
||||
|
||||
ports = list(testrig.get_connected_components({
|
||||
'rx': (odrive.gpio1, True),
|
||||
'tx': (odrive.gpio2, False)
|
||||
}, SerialPortComponent))
|
||||
|
||||
# Hack the bus objects to enable noise_enable functionality on the TX line.
|
||||
|
||||
def get_noise_gpio(bus):
|
||||
teensy = bus.gpio_tuples[1][0]
|
||||
for teensy_gpio in teensy.gpios:
|
||||
for other_gpio in testrig.get_directly_connected_components(teensy_gpio):
|
||||
if isinstance(other_gpio, LinuxGpioComponent):
|
||||
return teensy_gpio, other_gpio
|
||||
return None
|
||||
|
||||
for idx, bus in enumerate(ports):
|
||||
noise_gpio_on_teensy, noise_gpio_on_rpi = get_noise_gpio(bus)
|
||||
assert(noise_gpio_on_rpi)
|
||||
t, i, o, _ = bus.gpio_tuples[1]
|
||||
bus.gpio_tuples[1] = (t, i, o, noise_gpio_on_teensy)
|
||||
ports[idx] = (bus, noise_gpio_on_rpi)
|
||||
|
||||
yield (odrive, ports)
|
||||
|
||||
def run_test(self, odrive: ODriveComponent, port: SerialPortComponent, noise_enable: LinuxGpioComponent, logger: Logger):
|
||||
noise_enable.config(output=True)
|
||||
noise_enable.write(False)
|
||||
time.sleep(0.1)
|
||||
|
||||
odrive.handle.axis0.config.enable_step_dir = False
|
||||
odrive.handle.config.enable_uart = True
|
||||
|
||||
with port.open(115200) as ser:
|
||||
# reset port to known state
|
||||
reset_state(ser)
|
||||
|
||||
# Enable square wave of ~1.6MHz on the ODrive's RX line
|
||||
noise_enable.write(True)
|
||||
|
||||
time.sleep(0.1)
|
||||
reset_state(ser)
|
||||
|
||||
time.sleep(1.0)
|
||||
|
||||
# Read an attribute (should fail because the command is not passed through)
|
||||
ser.write(b'r vbus_voltage\n')
|
||||
test_assert_eq(ser.readline(), b'')
|
||||
|
||||
# Disable square wave
|
||||
noise_enable.write(False)
|
||||
|
||||
# Give receiver some time to recover
|
||||
time.sleep(0.1)
|
||||
|
||||
# reset port to known state
|
||||
reset_state(ser)
|
||||
|
||||
# Try again
|
||||
ser.write(b'r vbus_voltage\n')
|
||||
response = float(ser.readline().strip())
|
||||
test_assert_eq(response, odrive.handle.vbus_voltage, accuracy=0.1)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run([
|
||||
TestUartAscii(),
|
||||
TestUartBaudrate(),
|
||||
TestUartBurnIn(),
|
||||
TestUartNoise(),
|
||||
])
|
||||
Reference in New Issue
Block a user