From 71d640e562a275cbe633eb60e851e55d4a276e3f Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Thu, 23 Apr 2020 11:10:24 +0200 Subject: [PATCH] add sin/cos encoder test, improve sawtooth fitting --- tools/odrive/tests/analog_input_test.py | 34 ++--- tools/odrive/tests/encoder_test.py | 187 +++++++++++++++++------- tools/odrive/tests/pwm_input_test.py | 2 +- tools/odrive/tests/test_runner.py | 97 ++++++++++-- 4 files changed, 239 insertions(+), 81 deletions(-) diff --git a/tools/odrive/tests/analog_input_test.py b/tools/odrive/tests/analog_input_test.py index 6a3c73e7..d1f83774 100644 --- a/tools/odrive/tests/analog_input_test.py +++ b/tools/odrive/tests/analog_input_test.py @@ -5,7 +5,6 @@ import time import math import os import numpy as np -import scipy.optimize from odrive.enums import errors from test_runner import * @@ -36,16 +35,6 @@ void loop() { """ -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. @@ -92,6 +81,7 @@ class TestAnalogInput(): 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, @@ -108,24 +98,24 @@ class TestAnalogInput(): odrive.save_config_and_reboot() - logger.debug("Log test_property...") - log_x = [] - log_y = [] + logger.debug("Recording log...") + data = [] 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) + data.append(( + time.monotonic() - start, + odrive.handle.axis0.controller.input_pos + )) + data = np.array(data) # 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. + # 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) - 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) + slope, offset, fitted_curve = fit_sawtooth(data, min_val, max_val) + 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) diff --git a/tools/odrive/tests/encoder_test.py b/tools/odrive/tests/encoder_test.py index d1ffa061..53335716 100644 --- a/tools/odrive/tests/encoder_test.py +++ b/tools/odrive/tests/encoder_test.py @@ -6,6 +6,7 @@ from math import pi import os from fibre.utils import Logger +from odrive.enums import * from test_runner import * @@ -36,8 +37,100 @@ void loop() { """ +teensy_code_template2 = """ +void setup() { + analogWriteResolution(10); + int freq = 150000000/1024; // ~146.5kHz PWM frequency + analogWriteFrequency({enc_sin}, freq); + analogWriteFrequency({enc_cos}, freq); +} -class TestIncrementalEncoder(): +int rpm = 60; +float pos = 0; + +void loop() { + pos += 0.001f * ((float)rpm / 60.0f); + 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 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): + encoder.config.cpr = true_cpr + true_cps = true_cpr * true_rps + + logger.debug("Recording log...") + data = [] + start = time.monotonic() + encoder.set_linear_count(0) # prevent numerical errors + while time.monotonic() - start < 5.0: + data.append(( + time.monotonic() - start, + encoder.shadow_count, + encoder.count_in_cpr, + encoder.phase, + encoder.pos_estimate, + encoder.pos_cpr, + encoder.vel_estimate, + )) + + data = np.array(data) + + 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.01, inlier_range = true_cpr * 0.01, 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.01, inlier_range = true_cpr * 0.01, max_outliers = len(data[:,0]) * 0.02) + + # encoder.phase + slope, offset, fitted_curve = fit_sawtooth(data[:,(0,3)], -pi, pi, sigma=5) + test_assert_eq(slope / 7, 2*pi*abs(true_rps), accuracy=0.01) + test_curve_fit(data[:,(0,3)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.01, 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.01, inlier_range = true_cpr * 0.01, 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.005) + test_assert_eq(offset, true_cpr * true_rps, accuracy = 0.005) + test_curve_fit(data[:,(0,6)], fitted_curve, max_mean_err = true_cpr * 0.05, inlier_range = true_cpr * 0.05, max_outliers = len(data[:,0]) * 0.02) + + + + +class TestIncrementalEncoder(TestEncoderBase): def get_test_cases(self, testrig: TestRig): for odrive in testrig.get_components(ODriveComponent): @@ -50,41 +143,13 @@ class TestIncrementalEncoder(): ] valid_combinations = [ - [combination[0].parent] + list(combination) + (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_delta_test(self, encoder, true_cps, with_cpr): - encoder.config.cpr = with_cpr - - for i in range(100): - now = time.monotonic() - new_shadow_count = encoder.shadow_count - new_count_in_cpr = encoder.count_in_cpr - new_phase = encoder.phase - new_pos_estimate = encoder.pos_estimate - new_pos_cpr = encoder.pos_cpr - - if i > 0: - dt = now - before - test_assert_eq((new_shadow_count - last_shadow_count) / dt, true_cps, accuracy = 0.05) - test_assert_eq(modpm(new_count_in_cpr - last_count_in_cpr, with_cpr) / dt, true_cps, accuracy = 0.3) - #test_assert_eq(modpm(new_phase - last_phase, 2*pi) / dt, 2*pi*true_rps, accuracy = 0.1) - test_assert_eq((new_pos_estimate - last_pos_estimate) / dt, true_cps, accuracy = 0.3) - test_assert_eq(modpm(new_pos_cpr - last_pos_cpr, with_cpr) / dt, true_cps, accuracy = 0.3) - test_assert_eq(encoder.vel_estimate, true_cps, accuracy = 0.05) - - before = now - last_shadow_count = new_shadow_count - last_count_in_cpr = new_count_in_cpr - last_phase = new_phase - last_pos_estimate = new_pos_estimate - last_pos_cpr = new_pos_cpr - - time.sleep(0.01) def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: int, teensy_gpio_b: int, logger: Logger): true_cps = 8192*-0.5 # counts per second generated by the virtual encoder @@ -92,32 +157,56 @@ class TestIncrementalEncoder(): code = teensy_code_template.replace("{enc_a}", str(teensy_gpio_a.num)).replace("{enc_b}", str(teensy_gpio_b.num)) teensy.compile_and_program(code) - time.sleep(1.0) # wait for PLLs to stabilize + 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 encoder = enc.handle - # The true encoder count and PLL output should be roughly the same. - # At 8192 CPR and 0.5 RPM, the delta because of sequential reading is - # around 3.25 counts. The exact value depends on the connection. - # The tracking error of the PLL is below 1 count. - - #logger.debug("check if count_in_cpr == pos_cpr") - #configured_cpr = 8192 - #encoder.config.cpr = configured_cpr - #expected_delta = true_cps/1200 - #for _ in range(1000): - # first = enc.handle.axis0.encoder.count_in_cpr - # second = enc.handle.axis0.encoder.pos_cpr - # test_assert_eq(modpm(second - first, configured_cpr), expected_delta, range=abs(true_cps/500)) - # time.sleep(0.001) - logger.debug("check if variables move at the correct velocity (8192 CPR)...") - self.run_delta_test(encoder, true_cps, 8192) + self.run_generic_encoder_test(enc.handle, 8192, true_cps / 8192) logger.debug("check if variables move at the correct velocity (65536 CPR)...") - self.run_delta_test(encoder, true_cps, 65536) + self.run_generic_encoder_test(enc.handle, 65536, true_cps / 65536) encoder.config.cpr = 8192 +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: EncoderComponent, teensy: TeensyComponent, teensy_gpio_sin: TeensyGpio, teensy_gpio_cos: TeensyGpio, logger: Logger): + code = teensy_code_template2.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 + + self.run_generic_encoder_test(enc.handle, 6283, 1.0) + + + if __name__ == '__main__': - test_runner.run(TestIncrementalEncoder()) + test_runner.run([ + TestIncrementalEncoder(), + TestSinCosEncoder(), + ]) diff --git a/tools/odrive/tests/pwm_input_test.py b/tools/odrive/tests/pwm_input_test.py index e1dbddd9..e601bd72 100644 --- a/tools/odrive/tests/pwm_input_test.py +++ b/tools/odrive/tests/pwm_input_test.py @@ -61,7 +61,7 @@ class TestPwmInput(): ] valid_combinations = [ - [combination[0].parent] + list(combination) + (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)) ] diff --git a/tools/odrive/tests/test_runner.py b/tools/odrive/tests/test_runner.py index 82298f3f..c3787641 100644 --- a/tools/odrive/tests/test_runner.py +++ b/tools/odrive/tests/test_runner.py @@ -17,6 +17,11 @@ 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 ----------------------------------------------------------------# @@ -68,6 +73,79 @@ def all_unique(lst): def modpm(val, range): return ((val + (range / 2)) % range) - (range / 2) +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): + def save(): + import json + filename = '/tmp/log.json' + print('saving data to ' + filename) + with open(filename, 'w+') as fp: + json.dump(np.concatenate([data, np.array([fitted_curve]).transpose()], 1).tolist(), fp, indent=2) + + diffs = data[:,1] - fitted_curve + + mean_err = np.abs(diffs).mean() + if mean_err > max_mean_err: + save() + 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() + raise TestFailed("curve fit has too many outliers (err > {}): {} > {}".format(inlier_range, outliers, max_outliers)) + + # Test Components -------------------------------------------------------------# class Component(object): @@ -307,15 +385,16 @@ class TeensyComponent(Component): time.sleep(0.5) # give it some time to boot def compile_and_program(self, code: str): - with io.TextIOWrapper(tempfile.NamedTemporaryFile(suffix='.ino')) 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) + 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):