add analog input tests (still require manual intervention to enable low pass filter)

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

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

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

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

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

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