pbio/drivebase: Separate use_gyro setter.

This allows it to be toggled during runtime.

Author: laurensvalk

lib/pbio/include/pbio/drivebase.h

@@ -33,7 +33,7 @@ typedef struct _pbio_drivebase_t {
     pbio_control_t control_distance;
 } pbio_drivebase_t;
 
-pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_servo_t *left, pbio_servo_t *right, int32_t wheel_diameter, int32_t axle_track, bool use_gyro);
+pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_servo_t *left, pbio_servo_t *right, int32_t wheel_diameter, int32_t axle_track);
 
 // Drive base status:
 
@@ -58,6 +58,7 @@ pbio_error_t pbio_drivebase_stop(pbio_drivebase_t *db, pbio_control_on_completio
 pbio_error_t pbio_drivebase_get_state_user(pbio_drivebase_t *db, int32_t *distance, int32_t *drive_speed, int32_t *angle, int32_t *turn_rate);
 pbio_error_t pbio_drivebase_get_drive_settings(const pbio_drivebase_t *db, int32_t *drive_speed, int32_t *drive_acceleration, int32_t *drive_deceleration, int32_t *turn_rate, int32_t *turn_acceleration, int32_t *turn_deceleration);
 pbio_error_t pbio_drivebase_set_drive_settings(pbio_drivebase_t *db, int32_t drive_speed, int32_t drive_acceleration, int32_t drive_deceleration, int32_t turn_rate, int32_t turn_acceleration, int32_t turn_deceleration);
+pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, bool use_gyro);
 
 #if PBIO_CONFIG_DRIVEBASE_SPIKE
 

lib/pbio/src/drivebase.c

@@ -242,10 +242,9 @@ static pbio_error_t pbio_drivebase_stop_from_servo(void *drivebase, bool clear_p
  * @param [in]  right            Right servo instance.
  * @param [in]  wheel_diameter   Wheel diameter in um.
  * @param [in]  axle_track       Distance between wheel-ground contact points in um.
- * @param [in]  use_gyro         Whether to use a gyro for heading (true) or the builtin rotation sensors (false).
  * @return                       Error code.
  */
-pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_servo_t *left, pbio_servo_t *right, int32_t wheel_diameter, int32_t axle_track, bool use_gyro) {
+pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_servo_t *left, pbio_servo_t *right, int32_t wheel_diameter, int32_t axle_track) {
 
     // Can't build a drive base with just one motor.
     if (left == right) {
@@ -287,8 +286,8 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se
     db->left = left;
     db->right = right;
 
-    // Whether to use the gyro for steering and driving straight.
-    db->use_gyro = use_gyro;
+    // By default, don't use gyro.
+    db->use_gyro = false;
 
     // Set parents of both servos, so they can stop this drivebase.
     pbio_parent_set(&left->parent, db, pbio_drivebase_stop_from_servo);
@@ -336,6 +335,28 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se
     return PBIO_SUCCESS;
 }
 
+/**
+ * Makes the drivebase use gyro or motor rotation sensors for heading control.
+ *
+ * This function will stop the drivebase if it is running.
+ *
+ * @param [in]  db               Drivebase instance.
+ * @param [in]  use_gyro         Whether to use the gyro for heading control.
+ * @return                       Error code.
+ */
+pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, bool use_gyro) {
+
+    // We stop so that new commands will reinitialize the state using the
+    // newly selected input for heading control.
+    pbio_error_t err = pbio_drivebase_stop(db, PBIO_CONTROL_ON_COMPLETION_COAST);
+    if (err != PBIO_SUCCESS) {
+        return err;
+    }
+
+    db->use_gyro = use_gyro;
+    return PBIO_SUCCESS;
+}
+
 /**
  * Stops a drivebase.
  *
@@ -821,7 +842,7 @@ pbio_error_t pbio_drivebase_is_stalled(pbio_drivebase_t *db, bool *stalled, uint
  * @return                       Error code.
  */
 pbio_error_t pbio_drivebase_get_drivebase_spike(pbio_drivebase_t **db_address, pbio_servo_t *left, pbio_servo_t *right) {
-    pbio_error_t err = pbio_drivebase_get_drivebase(db_address, left, right, 1000, 1000, false);
+    pbio_error_t err = pbio_drivebase_get_drivebase(db_address, left, right, 1000, 1000);
 
     // The application input for spike bases is degrees per second average
     // between both wheels, so in millidegrees this is x1000.

lib/pbio/test/src/test_drivebase.c

@@ -82,7 +82,7 @@ static PT_THREAD(test_drivebase_basics(struct pt *pt)) {
     tt_uint_op(pbio_servo_setup(srv_right, id, PBIO_DIRECTION_CLOCKWISE, 1000, true, 0), ==, PBIO_SUCCESS);
 
     // Set up the drivebase.
-    tt_uint_op(pbio_drivebase_get_drivebase(&db, srv_left, srv_right, 56000, 112000, false), ==, PBIO_SUCCESS);
+    tt_uint_op(pbio_drivebase_get_drivebase(&db, srv_left, srv_right, 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);

pybricks/robotics/pb_type_drivebase.c

@@ -72,9 +72,10 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a
         srv_left,
         srv_right,
         pb_obj_get_scaled_int(wheel_diameter_in, 1000),
-        pb_obj_get_scaled_int(axle_track_in, 1000),
-        // Use gyro if creating instance of GyroDriveBase.
-        type != &pb_type_drivebase));
+        pb_obj_get_scaled_int(axle_track_in, 1000)));
+
+    // Use gyro if creating instance of GyroDriveBase.
+    pb_assert(pbio_drivebase_set_use_gyro(self->db, type != &pb_type_drivebase));
 
     #if PYBRICKS_PY_COMMON_CONTROL
     // Create instances of the Control class