pbio/drivebase: Implement distance and angle state offsets.

This moves the setting and getting of the user state to pbio, which gives us more control over resetting the control state when this is done.

Also implements setting nonzero values for these states.

Author: laurensvalk

.vscode/launch.json

@@ -105,7 +105,7 @@
             "request": "launch",
             "program": "${workspaceFolder}/bricks/virtualhub/build-debug/virtualhub-micropython",
             "args": [
-                "${workspaceFolder}/tests/virtualhub/motor/car.py"
+                "${workspaceFolder}/tests/virtualhub/motor/drivebase.py"
             ],
             "stopAtEntry": false,
             "cwd": "${workspaceFolder}",

lib/pbio/include/pbio/drivebase.h

@@ -29,7 +29,23 @@ typedef struct _pbio_drivebase_t {
     bool control_paused;
     pbio_servo_t *left;
     pbio_servo_t *right;
+    /**
+     * Offset of the reported heading angle with respect to the measured value.
+     *
+     * Not used if gyro control is active. Then the gyro module handles offset.
+     */
+    pbio_angle_t heading_offset;
+    /**
+     * Heading controller.
+     */
     pbio_control_t control_heading;
+    /**
+     * Offset of the reported distance with respect to the measured value.
+     */
+    pbio_angle_t distance_offset;
+    /**
+     * Distance controller.
+     */
     pbio_control_t control_distance;
 } pbio_drivebase_t;
 
@@ -56,6 +72,7 @@ pbio_error_t pbio_drivebase_stop(pbio_drivebase_t *db, pbio_control_on_completio
 // Measuring and settings:
 
 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_reset(pbio_drivebase_t *db, int32_t distance, int32_t angle);
 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);

lib/pbio/src/drivebase.c

@@ -21,7 +21,7 @@ static pbio_drivebase_t drivebases[PBIO_CONFIG_NUM_DRIVEBASES];
 /**
  * Gets the state of the drivebase update loop.
  *
- * This becomes true after a successful call to pbio_drivebase_setup and
+ * This becomes true after a successful call to pbio_drivebase_get_drivebase and
  * becomes false when there is an error. Such as when the cable is unplugged.
  *
  * @param [in]  db          The drivebase instance
@@ -117,11 +117,11 @@ static void drivebase_adopt_settings(pbio_control_settings_t *s_distance, pbio_c
  * Get the physical and estimated state of a drivebase in units of control.
  *
  * @param [in]  db              The drivebase instance
- * @param [out] state_distance  Physical and estimated state of the distance.
- * @param [out] state_heading   Physical and estimated state of the heading.
+ * @param [out] state_distance  Physical and estimated state of the distance based on motor angles.
+ * @param [out] state_heading   Physical and estimated state of the heading based on motor angles.
  * @return                      Error code.
  */
-static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_control_state_t *state_distance, pbio_control_state_t *state_heading) {
+static pbio_error_t pbio_drivebase_get_state_via_motors(pbio_drivebase_t *db, pbio_control_state_t *state_distance, pbio_control_state_t *state_heading) {
 
     // Get left servo state
     pbio_control_state_t state_left;
@@ -150,7 +150,35 @@ static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_
     state_heading->speed_estimate = state_distance->speed_estimate - state_right.speed_estimate;
     state_heading->speed = state_distance->speed - state_right.speed;
 
+    return PBIO_SUCCESS;
+}
+
+/**
+ * Get the physical and estimated state of a drivebase in units of control.
+ *
+ * @param [in]  db              The drivebase instance
+ * @param [out] state_distance  Physical and estimated state of the distance.
+ * @param [out] state_heading   Physical and estimated state of the heading.
+ * @return                      Error code.
+ */
+static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_control_state_t *state_distance, pbio_control_state_t *state_heading) {
+
+    // Gets the "measured" state according to the driver motors.
+    pbio_error_t err = pbio_drivebase_get_state_via_motors(db, state_distance, state_heading);
+    if (err != PBIO_SUCCESS) {
+        return err;
+    }
+
+    // Subtract distance offset.
+    pbio_angle_diff(&state_distance->position, &db->distance_offset, &state_distance->position);
+    pbio_angle_diff(&state_distance->position_estimate, &db->distance_offset, &state_distance->position_estimate);
+
+    // Subtract heading offset
+    pbio_angle_diff(&state_heading->position, &db->heading_offset, &state_heading->position);
+    pbio_angle_diff(&state_heading->position_estimate, &db->heading_offset, &state_heading->position_estimate);
+
     // Optionally use gyro to override the heading source for more accuracy.
+    // The gyro manages its own offset, so we don't need to subtract it here.
     if (db->use_gyro) {
         pbio_imu_get_heading_scaled(&state_heading->position, &state_heading->speed, db->control_heading.settings.ctl_steps_per_app_step);
     }
@@ -235,7 +263,7 @@ static pbio_error_t pbio_drivebase_stop_from_servo(void *drivebase, bool clear_p
 #define ROT_MDEG_OVER_PI (114592) // 360 000 / pi
 
 /**
- * Gets drivebase instance from two servo instances.
+ * Gets and sets up drivebase instance from two servo instances.
  *
  * @param [out] db_address       Drivebase instance if available.
  * @param [in]  left             Left servo instance.
@@ -329,6 +357,10 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se
         return PBIO_ERROR_INVALID_ARG;
     }
 
+    // Reset offsets so current distance and angle are 0. This relies on the
+    // geometry, so it is done after the scaling is set.
+    pbio_drivebase_reset(db, 0, 0);
+
     // Finish setup. By default, don't use gyro.
     return pbio_drivebase_set_use_gyro(db, false);
 }
@@ -687,6 +719,50 @@ pbio_error_t pbio_drivebase_get_state_user(pbio_drivebase_t *db, int32_t *distan
     return PBIO_SUCCESS;
 }
 
+/**
+ * Stops the drivebase and resets the accumulated drivebase state in user units.
+ *
+ * If the gyro is being used for control, it will be reset to the same angle.
+ *
+ * @param [in]  db          The drivebase instance.
+ * @param [out] distance    Distance traveled in mm.
+ * @param [out] angle       Angle turned in degrees.
+ * @return                  Error code.
+ */
+pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_t angle) {
+
+    // Physically stops motors and stops the ongoing controllers, simplifying
+    // the state reset since we won't need to restart ongoing motion.
+    pbio_error_t err = pbio_drivebase_stop(db, PBIO_CONTROL_ON_COMPLETION_COAST);
+    if (err != PBIO_SUCCESS) {
+        return err;
+    }
+
+    // Get measured state according to motor encoders.
+    pbio_control_state_t measured_distance;
+    pbio_control_state_t measured_heading;
+    err = pbio_drivebase_get_state_via_motors(db, &measured_distance, &measured_heading);
+    if (err != PBIO_SUCCESS) {
+        return err;
+    }
+
+    // We want to get: reported_new = measured - offset_new
+    // So we can do:     offset_new = measured - reported_new
+    pbio_angle_t reported_new;
+
+    pbio_angle_from_low_res(&reported_new, distance, db->control_distance.settings.ctl_steps_per_app_step);
+    pbio_angle_diff(&measured_distance.position, &reported_new, &db->distance_offset);
+
+    pbio_angle_from_low_res(&reported_new, angle, db->control_heading.settings.ctl_steps_per_app_step);
+    pbio_angle_diff(&measured_heading.position, &reported_new, &db->heading_offset);
+
+    // Whether or not the gyro is being used, synchronize heading and drivebase
+    // angle state.
+    pbio_imu_set_heading(angle);
+
+    return PBIO_SUCCESS;
+}
+
 
 /**
  * Gets the drivebase settings in user units.

pybricks/robotics/pb_type_drivebase.c

@@ -29,8 +29,6 @@ typedef struct _pb_type_DriveBase_obj_t pb_type_DriveBase_obj_t;
 struct _pb_type_DriveBase_obj_t {
     mp_obj_base_t base;
     pbio_drivebase_t *db;
-    int32_t initial_distance;
-    int32_t initial_heading;
     #if PYBRICKS_PY_COMMON_CONTROL
     mp_obj_t heading_control;
     mp_obj_t distance_control;
@@ -39,18 +37,18 @@ struct _pb_type_DriveBase_obj_t {
 };
 
 // pybricks.robotics.DriveBase.reset
-static mp_obj_t pb_type_DriveBase_reset(mp_obj_t self_in) {
-    pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in);
+static mp_obj_t pb_type_DriveBase_reset(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
 
-    int32_t distance, drive_speed, angle, turn_rate;
-    pb_assert(pbio_drivebase_get_state_user(self->db, &distance, &drive_speed, &angle, &turn_rate));
+    PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args,
+        pb_type_DriveBase_obj_t, self,
+        PB_ARG_DEFAULT_INT(distance, 0),
+        PB_ARG_DEFAULT_INT(angle, 0));
 
-    self->initial_distance = distance;
-    self->initial_heading = angle;
+    pb_assert(pbio_drivebase_reset(self->db, pb_obj_get_int(distance_in), pb_obj_get_int(angle_in)));
 
     return mp_const_none;
 }
-MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_reset_obj, pb_type_DriveBase_reset);
+static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_reset_obj, 1, pb_type_DriveBase_reset);
 
 // pybricks.robotics.DriveBase.__init__
 static mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) {
@@ -80,9 +78,6 @@ static mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a
     self->distance_control = pb_type_Control_obj_make_new(&self->db->control_distance);
     #endif
 
-    // Reset drivebase state
-    pb_type_DriveBase_reset(MP_OBJ_FROM_PTR(self));
-
     // List of awaitables associated with this drivebase. By keeping track,
     // we can cancel them as needed when a new movement is started.
     self->awaitables = mp_obj_new_list(0, NULL);
@@ -243,7 +238,7 @@ static mp_obj_t pb_type_DriveBase_distance(mp_obj_t self_in) {
     int32_t distance, _;
     pb_assert(pbio_drivebase_get_state_user(self->db, &distance, &_, &_, &_));
 
-    return mp_obj_new_int(distance - self->initial_distance);
+    return mp_obj_new_int(distance);
 }
 MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_distance_obj, pb_type_DriveBase_distance);
 
@@ -254,7 +249,7 @@ static mp_obj_t pb_type_DriveBase_angle(mp_obj_t self_in) {
     int32_t heading, _;
     pb_assert(pbio_drivebase_get_state_user(self->db, &_, &_, &heading, &_));
 
-    return mp_obj_new_int(heading - self->initial_heading);
+    return mp_obj_new_int(heading);
 }
 MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_angle_obj, pb_type_DriveBase_angle);
 
@@ -266,9 +261,9 @@ static mp_obj_t pb_type_DriveBase_state(mp_obj_t self_in) {
     pb_assert(pbio_drivebase_get_state_user(self->db, &distance, &drive_speed, &heading, &turn_rate));
 
     mp_obj_t ret[4];
-    ret[0] = mp_obj_new_int(distance - self->initial_distance);
+    ret[0] = mp_obj_new_int(distance);
     ret[1] = mp_obj_new_int(drive_speed);
-    ret[2] = mp_obj_new_int(heading - self->initial_heading);
+    ret[2] = mp_obj_new_int(heading);
     ret[3] = mp_obj_new_int(turn_rate);
 
     return mp_obj_new_tuple(4, ret);

tests/virtualhub/motor/drivebase.py

@@ -2,19 +2,67 @@
 from pybricks.tools import wait
 from pybricks.parameters import Port, Direction
 from pybricks.robotics import DriveBase
+from umath import pi
 
 # Initialize default "Driving Base" with medium motors and wheels.
 left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
 right_motor = Motor(Port.B)
 drive_base = DriveBase(left_motor, right_motor, wheel_diameter=56, axle_track=112)
 
-drive_base.settings(
-    straight_speed=500, straight_acceleration=1000, turn_rate=500, turn_acceleration=2000
-)
+
+def expect_state(
+    expected_distance, expected_drive_speed, expected_angle, expected_angular_velocity
+):
+    expected_state = (
+        expected_distance,
+        expected_drive_speed,
+        expected_angle,
+        expected_angular_velocity,
+    )
+    distance, drive_speed, angle, angular_velocity = drive_base.state()
+    if (
+        abs(expected_distance - distance) > 10
+        or abs(expected_drive_speed - drive_speed) > 30
+        or abs(expected_angle - angle) > 10
+        or abs(expected_angular_velocity - angular_velocity) > 30
+    ):
+        raise ValueError("Expected {0} but got {1}".format(expected_state, drive_base.state()))
+
+
+# Expect zeroed state on startup.
+expect_state(0, 0, 0, 0)
 
 # Drive straight forward and back again.
 drive_base.straight(500)
+expect_state(500, 0, 0, 0)
 drive_base.straight(-500)
+expect_state(0, 0, 0, 0)
+
+# Curve and back again.
+drive_base.curve(100, 90)
+expect_state(pi / 2 * 100, 0, 90, 0)
+drive_base.curve(-100, 90)
+expect_state(0, 0, 0, 0)
 
-wait(100)
-drive_base.stop()
+# Test setting of distance and angle state.
+drive_base.reset(1000, 360)
+expect_state(1000, 0, 360, 0)
+drive_base.straight(-1000)
+drive_base.turn(-360)
+expect_state(0, 0, 0, 0)
+
+# Expect to reach target speed.
+drive_base.drive(200, 25)
+wait(500)
+expect_state(drive_base.distance(), 200, drive_base.angle(), 25)
+
+# Expect reset to stop the robot
+drive_base.reset()
+wait(500)
+expect_state(drive_base.distance(), 0, drive_base.angle(), 0)
+
+# Drive fast.
+drive_base.settings(
+    straight_speed=500, straight_acceleration=1000, turn_rate=500, turn_acceleration=2000
+)
+drive_base.straight(500)