From 7e2434f7d84b1085b106e320a429d192c84900ec Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 21 Apr 2020 20:11:33 +0200 Subject: [PATCH] add analog input tests (still require manual intervention to enable low pass filter) --- tools/odrive/tests/analog_input_test.py | 133 ++++++++++++++++++++++++ tools/odrive/tests/calibration_test.py | 3 - tools/odrive/tests/encoder_test.py | 3 - tools/odrive/tests/pwm_input_test.py | 6 +- tools/odrive/tests/test_runner.py | 51 +++++++-- tools/test-rig-rpi.yaml | 10 +- 6 files changed, 184 insertions(+), 22 deletions(-) create mode 100644 tools/odrive/tests/analog_input_test.py diff --git a/tools/odrive/tests/analog_input_test.py b/tools/odrive/tests/analog_input_test.py new file mode 100644 index 00000000..6a3c73e7 --- /dev/null +++ b/tools/odrive/tests/analog_input_test.py @@ -0,0 +1,133 @@ + +import test_runner + +import time +import math +import os +import numpy as np +import scipy.optimize + +from odrive.enums import errors +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); +} +""" + + +def fit_sawtooth(data, min_val, max_val, period, range): + """ + Returns the average absolute error and the number of outliers + """ + func = lambda x, a: np.mod((max_val - min_val) / a * (x - a/2) - min_val, max_val - min_val) + min_val + params = scipy.optimize.curve_fit(func, data[:,0], data[:,1], [period])[0] + diffs = data[:,1] - func(data[:,0], *params) + return np.abs(diffs).mean(), np.count_nonzero((diffs > range) | (diffs < -range)) + + +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 + + 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() + + + logger.debug("Log test_property...") + log_x = [] + log_y = [] + start = time.monotonic() + analog_reset_gpio.write(False) + while time.monotonic() - start < 5.0: + log_x.append(time.monotonic() - start) + log_y.append(odrive.handle.axis0.controller.input_pos) + + + # Expect mean error to be at most 2% (of the full scale). + # Expect there to be less than 1% 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) + data = np.array([log_x, log_y]).transpose() + mean_error, n_outliers = fit_sawtooth(data, min_val, max_val, 1.05, full_range * 0.05) + + test_assert_eq(mean_error, 0, range = full_range * 0.02) + test_assert_eq(n_outliers, 0, range = len(log_x) * 0.01) + + + +if __name__ == '__main__': + test_runner.run(TestAnalogInput()) diff --git a/tools/odrive/tests/calibration_test.py b/tools/odrive/tests/calibration_test.py index 595b3b92..3cd60740 100644 --- a/tools/odrive/tests/calibration_test.py +++ b/tools/odrive/tests/calibration_test.py @@ -9,9 +9,6 @@ from fibre.utils import Logger from test_runner import * from odrive.enums import * -def modpm(val, range): - return ((val + (range / 2)) % range) - (range / 2) - class TestMotorCalibration(): """ diff --git a/tools/odrive/tests/encoder_test.py b/tools/odrive/tests/encoder_test.py index 016f39da..d1ffa061 100644 --- a/tools/odrive/tests/encoder_test.py +++ b/tools/odrive/tests/encoder_test.py @@ -37,9 +37,6 @@ void loop() { """ -def modpm(val, range): - return ((val + (range / 2)) % range) - (range / 2) - class TestIncrementalEncoder(): def get_test_cases(self, testrig: TestRig): diff --git a/tools/odrive/tests/pwm_input_test.py b/tools/odrive/tests/pwm_input_test.py index fc603eb3..e1dbddd9 100644 --- a/tools/odrive/tests/pwm_input_test.py +++ b/tools/odrive/tests/pwm_input_test.py @@ -37,10 +37,6 @@ void loop() { """ - -def modpm(val, range): - return ((val + (range / 2)) % range) - (range / 2) - class TestPwmInput(): """ Verifies the PWM input. @@ -125,7 +121,7 @@ class TestPwmInput(): test_assert_eq(min_val, with_min, range = step_size) test_assert_eq(max_val, with_max, range = step_size) - def run_test(self, odrive: ODriveComponent, teensy: TeensyComponent, teensy_gpio1: int, teensy_gpio2: int, teensy_gpio3: int, teensy_gpio4: int, logger: Logger): + def run_test(self, odrive: ODriveComponent, teensy: TeensyComponent, teensy_gpio1: Component, teensy_gpio2: Component, teensy_gpio3: Component, teensy_gpio4: Component, logger: Logger): # TODO: test each GPIO separately setup_code = "\n".join(" pinMode(" + str(gpio.num) + ", OUTPUT);" for gpio in [teensy_gpio1, teensy_gpio2, teensy_gpio3, teensy_gpio4]) diff --git a/tools/odrive/tests/test_runner.py b/tools/odrive/tests/test_runner.py index d76b32ae..82298f3f 100644 --- a/tools/odrive/tests/test_runner.py +++ b/tools/odrive/tests/test_runner.py @@ -65,6 +65,9 @@ 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) + # Test Components -------------------------------------------------------------# class Component(object): @@ -100,7 +103,8 @@ class ODriveComponent(Component): 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=30)#, printer=print) + 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): @@ -109,6 +113,17 @@ class ODriveComponent(Component): 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: @@ -302,6 +317,15 @@ class TeensyComponent(Component): 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): """ @@ -348,6 +372,8 @@ class TestRig(): 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 @@ -490,15 +516,20 @@ def run_shell(command_line, logger, env=None, timeout=None): def get_combinations(param_options): - if len(param_options) > 0: - param = param_options[0] - if not is_list_like(param): - param = [param] - - for part1, part2 in itertools.product(param, get_combinations(param_options[1:]) if (len(param_options) > 1) else [()]): - if not isinstance(part1, tuple): - part1 = (part1,) - yield part1 + part2 + 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 diff --git a/tools/test-rig-rpi.yaml b/tools/test-rig-rpi.yaml index 3321263f..33588274 100644 --- a/tools/test-rig-rpi.yaml +++ b/tools/test-rig-rpi.yaml @@ -17,7 +17,9 @@ components: name: can0 interface: can0 connected-to: odrive.can - - {type: gpio, num: 19} # need to specify GPIOs explicitly for the generalpurpose type + # need to specify GPIOs explicitly for the generalpurpose type + - {type: gpio, num: 16} + - {type: gpio, num: 19} - {type: gpio, num: 20} - {type: gpio, num: 26} @@ -57,6 +59,9 @@ components: - type: teensy name: teensy + - {type: lpf, name: lpf0} + - {type: lpf, name: lpf1} + connections: - ['odrive.can', 'rpi.can0'] - ['teensy.program', 'rpi.gpio26'] @@ -83,3 +88,6 @@ connections: - ['teensy.gpio2', 'real_encoder.b'] - ['odrive.axis0', 'D5065-270KV_0'] - ['D5065-270KV_0', 'real_encoder'] + - ['odrive.gpio3', 'lpf0'] + - ['odrive.gpio4', 'lpf1'] + - ['lpf0.en', 'lpf1.en', 'rpi.gpio16']