Merge remote-tracking branch 'origin/sam_tests' into fw4

Firmware/MotorControl/controller.hpp

@@ -18,7 +18,7 @@ struct ControllerConfig_t {
     Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL;  //see: Motor_control_mode_t
     float pos_gain = 20.0f;  // [(counts/s) / counts]
     float vel_gain = 5.0f / 10000.0f;  // [A/(counts/s)]
-    // float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] <sensorless example>
+    // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
     float vel_integrator_gain = 10.0f / 10000.0f;  // [A/(counts/s * s)]
     float vel_limit = 20000.0f;           // [counts/s]
 };

Firmware/communication/ascii_protocol.cpp

@@ -124,7 +124,6 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
             respond(response_channel, use_checksum, "invalid motor %u", motor_number);
         } else {
             axes[motor_number]->controller_.set_current_setpoint(current_setpoint);
-            respond(response_channel, use_checksum, "ok", motor_number);
         }
 
     } else if (cmd[0] == 'i'){ // Dump device info

Firmware/communication/interface_uart.cpp

@@ -95,7 +95,7 @@ void serve_on_uart() {
     dma_last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
 
     // Start UART communication thread
-    osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, 512);
+    osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, 1024 /* the ascii protocol needs considerable stack space */);
     uart_thread = osThreadCreate(osThread(uart_server_thread_def), NULL);
 }
 

tools/odrive/tests.py

@@ -51,12 +51,22 @@ class AxisTestContext():
         self.odrv_ctx = odrv_ctx
 
 def test_assert_eq(observed, expected, range=None, accuracy=None):
-    if range is None and accuracy is None and observed != expected:
-        raise TestFailed("value mismatch: expected {} but observed {}".format(expected, observed))
-    if not range is None and ((observed < expected - range) or (observed > expected + range)):
-        raise TestFailed("value out of range: expected {}+-{} but observed {}".format(expected, range, observed))
-    elif not accuracy is None and ((observed < expected * (1 - accuracy)) or (observed > expected * (1 + accuracy))):
-        raise TestFailed("value out of range: expected {}+-{}% but observed {}".format(expected, accuracy*100.0, observed))
+    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 get_errors(axis_ctx: AxisTestContext):
     errors = []
@@ -154,6 +164,9 @@ def get_max_rpm(axis_ctx: AxisTestContext):
     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
@@ -669,3 +682,86 @@ class TestVelCtrlVsPosCtrl(DualAxisTest):
         #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)))
+
+        # 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.pll_vel, target_vel, range=2000)
+
+        request_state(axis_ctx, AXIS_STATE_IDLE)

tools/run_tests.py

@@ -87,6 +87,12 @@ else:
     all_tests.append(TestDiscoverAndGotoIdle())
     all_tests.append(TestEncoderOffsetCalibration(pass_if_ready=True))
 
+all_tests.append(TestAsciiProtocol())
+all_tests.append(TestSensorlessControl())
+
+#all_tests.append(TestStepDirInput())
+#all_tests.append(TestPWMInput())
+
 if test_rig_yaml['type'] == 'parallel':
     #all_tests.append(TestHighVelocity())
     all_tests.append(TestHighVelocityInViscousFluid(load_current=35, driver_current=45))