pbio/test/drivebase: Add stall test.

Catches bug found in #294

Author: laurensvalk

CHANGELOG.md

@@ -7,7 +7,12 @@
 ### Fixed
 
 - Fixed overflow in saturation and value for ambient color measurement. This
-  can occur with very bright screens.
+  can occur with very bright screens ([pybricks-micropython#295]).
+- Fixed drive base stall condition not being raised if the right motor is
+  individually controlled and then stalled ([pybricks-micropython#294]).
+
+[pybricks-micropython#294]: https://github.com/pybricks/pybricks-micropython/pull/294
+[pybricks-micropython#295]: https://github.com/pybricks/pybricks-micropython/pull/295
 
 ## [3.6.0] - 2024-03-02
 

lib/pbio/test/src/test_drivebase.c

@@ -105,7 +105,7 @@ static PT_THREAD(test_drivebase_basics(struct pt *pt)) {
         turn_acceleration,
         turn_deceleration), ==, PBIO_ERROR_INVALID_ARG);
 
-    // Try to set invalid settings.
+    // Try to set valid settings.
     tt_uint_op(pbio_drivebase_set_drive_settings(db,
         200,
         drive_acceleration * 2,
@@ -183,7 +183,92 @@ static PT_THREAD(test_drivebase_basics(struct pt *pt)) {
     PT_END(pt);
 }
 
+/**
+ * Creates a drivebase pair where one of the motors (C) is physically stalled at
+ * about +/- 150 degrees. Motor F can spin freely.
+ */
+static PT_THREAD(test_drivebase_stalling(struct pt *pt)) {
+
+    static struct timer timer;
+
+    static pbio_servo_t *srv_free;
+    static pbio_servo_t *srv_blocked;
+    static pbdrv_legodev_dev_t *legodev_left;
+    static pbdrv_legodev_dev_t *legodev_right;
+    static pbio_drivebase_t *db;
+
+    static int32_t drive_distance;
+    static int32_t drive_speed;
+    static int32_t turn_angle_start;
+    static int32_t turn_rate;
+
+    static bool stalled;
+    static uint32_t stall_duration;
+
+    // Start motor driver simulation process.
+    pbdrv_motor_driver_init_manual();
+
+    PT_BEGIN(pt);
+
+    // Wait for motor simulation process to be ready.
+    while (pbdrv_init_busy()) {
+        PT_YIELD(pt);
+    }
+
+    // Start motor control process manually.
+    pbio_motor_process_start();
+
+    // Initialize the servos.
+
+    pbdrv_legodev_type_id_t id = PBDRV_LEGODEV_TYPE_ID_ANY_ENCODED_MOTOR;
+    tt_uint_op(pbdrv_legodev_get_device(PBIO_PORT_ID_F, &id, &legodev_left), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_servo_get_servo(legodev_left, &srv_free), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_servo_setup(srv_free, id, PBIO_DIRECTION_COUNTERCLOCKWISE, 1000, true, 0), ==, PBIO_SUCCESS);
+    id = PBDRV_LEGODEV_TYPE_ID_ANY_ENCODED_MOTOR;
+    tt_uint_op(pbdrv_legodev_get_device(PBIO_PORT_ID_C, &id, &legodev_right), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_servo_get_servo(legodev_right, &srv_blocked), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_servo_setup(srv_blocked, id, PBIO_DIRECTION_CLOCKWISE, 1000, true, 0), ==, PBIO_SUCCESS);
+
+    // Set up the drivebase.
+    tt_uint_op(pbio_drivebase_get_drivebase(&db, srv_free, srv_blocked, 56000, 112000), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_drivebase_get_state_user(db, &drive_distance, &drive_speed, &turn_angle_start, &turn_rate), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_drivebase_is_stalled(db, &stalled, &stall_duration), ==, PBIO_SUCCESS);
+    tt_want(!stalled);
+
+    // Try to drive straight. Should get stalled.
+    tt_uint_op(pbio_drivebase_drive_straight(db, 1000, PBIO_CONTROL_ON_COMPLETION_COAST_SMART), ==, PBIO_SUCCESS);
+    pbio_test_sleep_until(pbio_drivebase_is_stalled(db, &stalled, &stall_duration) == PBIO_SUCCESS && stalled);
+
+    // Stop. Now we should not be stalled any more.
+    tt_uint_op(pbio_drivebase_stop(db, PBIO_CONTROL_ON_COMPLETION_COAST), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_drivebase_is_stalled(db, &stalled, &stall_duration), ==, PBIO_SUCCESS);
+    tt_want(!stalled);
+
+    // Run the individual servos back to the start position.
+    tt_uint_op(pbio_servo_run_target(srv_free, 500, 0, PBIO_CONTROL_ON_COMPLETION_COAST), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_servo_run_target(srv_blocked, 500, 0, PBIO_CONTROL_ON_COMPLETION_COAST), ==, PBIO_SUCCESS);
+    pbio_test_sleep_until(pbio_control_is_done(&srv_free->control) && pbio_control_is_done(&srv_blocked->control));
+
+    // Stall the individual servos.
+    tt_uint_op(pbio_servo_run_target(srv_free, 500, 360, PBIO_CONTROL_ON_COMPLETION_COAST), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_servo_run_target(srv_blocked, 500, 360, PBIO_CONTROL_ON_COMPLETION_COAST), ==, PBIO_SUCCESS);
+    pbio_test_sleep_ms(&timer, 2000);
+
+    // The blocked motor should be stalled, the free motor not stalled, and the drivebase stalled.
+    tt_uint_op(pbio_servo_is_stalled(srv_blocked, &stalled, &stall_duration), ==, PBIO_SUCCESS);
+    tt_want(stalled);
+    tt_uint_op(pbio_servo_is_stalled(srv_free, &stalled, &stall_duration), ==, PBIO_SUCCESS);
+    tt_want(!stalled);
+    tt_uint_op(pbio_drivebase_is_stalled(db, &stalled, &stall_duration), ==, PBIO_SUCCESS);
+    tt_want(stalled);
+
+end:
+
+    PT_END(pt);
+}
+
 struct testcase_t pbio_drivebase_tests[] = {
     PBIO_PT_THREAD_TEST(test_drivebase_basics),
+    PBIO_PT_THREAD_TEST(test_drivebase_stalling),
     END_OF_TESTCASES
 };