add sin/cos encoder test, improve sawtooth fitting

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)
 
 
 

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(),
+    ])

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))
             ]

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):