pybricks.robotics.DriveBase: Implement arc method.

See pybricks/support#1157

Author: laurensvalk

CHANGELOG.md

@@ -22,6 +22,8 @@
   slots on the Prime Hub and Inventor Hub.
 - Added ability to set distance and angle in `DriveBase.reset()`. If the
   DriveBase is using the gyro, it will be set to the same angle. ([support#1617]).
+- Added `DriveBase.arc` method with more intuitive parameters to drive along
+  an arc, to eventually replace `DriveBase.curve` ([support#1157]).
 
 ### Changed
 
@@ -59,6 +61,7 @@
 [pybricks-micropython#254]: https://github.com/pybricks/pybricks-micropython/pull/254
 [pybricks-micropython#261]: https://github.com/pybricks/pybricks-micropython/pull/261
 [support#1105]: https://github.com/pybricks/support/issues/1105
+[support#1157]: https://github.com/pybricks/support/issues/1157
 [support#1429]: https://github.com/pybricks/support/issues/1429
 [support#1460]: https://github.com/pybricks/support/issues/1460
 [support#1615]: https://github.com/pybricks/support/issues/1615

lib/pbio/include/pbio/drivebase.h

@@ -63,6 +63,8 @@ pbio_error_t pbio_drivebase_is_stalled(pbio_drivebase_t *db, bool *stalled, uint
 
 pbio_error_t pbio_drivebase_drive_straight(pbio_drivebase_t *db, int32_t distance, pbio_control_on_completion_t on_completion);
 pbio_error_t pbio_drivebase_drive_curve(pbio_drivebase_t *db, int32_t radius, int32_t angle, pbio_control_on_completion_t on_completion);
+pbio_error_t pbio_drivebase_drive_arc_angle(pbio_drivebase_t *db, int32_t radius, int32_t angle, pbio_control_on_completion_t on_completion);
+pbio_error_t pbio_drivebase_drive_arc_distance(pbio_drivebase_t *db, int32_t radius, int32_t distance, pbio_control_on_completion_t on_completion);
 
 // Infinite driving:
 

lib/pbio/src/drivebase.c

@@ -616,6 +616,9 @@ pbio_error_t pbio_drivebase_drive_straight(pbio_drivebase_t *db, int32_t distanc
 /**
  * Starts the drivebase controllers to run by an arc of given radius and angle.
  *
+ * curve() was originally used as a generalization of turn(), but is now
+ * deprecated in favor of the arc methods, which have more practical arguments.
+ *
  * This will use the default speed.
  *
  * @param [in]  db              The drivebase instance.
@@ -636,6 +639,74 @@ pbio_error_t pbio_drivebase_drive_curve(pbio_drivebase_t *db, int32_t radius, in
     return pbio_drivebase_drive_relative(db, arc_length, 0, arc_angle, 0, on_completion);
 }
 
+/**
+ * Starts the drivebase controllers to run by an arc of given radius and angle.
+ *
+ * With a positive radius, the robot drives along a circle to its right.
+ * With a negative radius, the robot drives along a circle to its left.
+ *
+ * A positive angle means driving forward along the circle, negative is reverse.
+ *
+ * This will use the default speed.
+ *
+ * @param [in]  db              The drivebase instance.
+ * @param [in]  radius          Radius of the arc in mm.
+ * @param [in]  angle           The angle to drive along the circle in degrees.
+ * @param [in]  on_completion   What to do when reaching the target.
+ * @return                      Error code.
+ */
+pbio_error_t pbio_drivebase_drive_arc_angle(pbio_drivebase_t *db, int32_t radius, int32_t angle, pbio_control_on_completion_t on_completion) {
+
+    if (pbio_int_math_abs(radius) < 10) {
+        return PBIO_ERROR_INVALID_ARG;
+    }
+
+    // Arc length is radius * angle, with the user angle parameter governing
+    // the drive direction as positive forward.
+    int32_t drive_distance = (10 * angle * pbio_int_math_abs(radius)) / 573;
+
+    // The user angle is positive for going forward, no matter the radius sign.
+    // The internal functions expect positive to mean clockwise for the robot.
+    int32_t direction = (radius > 0) == (angle > 0) ? 1 : -1;
+    int32_t drive_angle = pbio_int_math_abs(angle) * direction;
+
+    // Execute the common drive command at default speed (by passing 0 speed).
+    return pbio_drivebase_drive_relative(db, drive_distance, 0, drive_angle, 0, on_completion);
+}
+
+/**
+ * Starts the drivebase controllers to run by an arc of given radius and arc length.
+ *
+ * With a positive radius, the robot drives along a circle to its right.
+ * With a negative radius, the robot drives along a circle to its left.
+ *
+ * A positive distance means driving forward along the circle, negative is reverse.
+ *
+ * This will use the default speed.
+ *
+ * @param [in]  db              The drivebase instance.
+ * @param [in]  radius          Radius of the arc in mm.
+ * @param [in]  distance        The distance to drive (arc length) in mm.
+ * @param [in]  on_completion   What to do when reaching the target.
+ * @return                      Error code.
+ */
+pbio_error_t pbio_drivebase_drive_arc_distance(pbio_drivebase_t *db, int32_t radius, int32_t distance, pbio_control_on_completion_t on_completion) {
+
+    if (pbio_int_math_abs(radius) < 10) {
+        return PBIO_ERROR_INVALID_ARG;
+    }
+
+    // The internal functions expect positive to mean clockwise for the robot
+    // with respect to the ground, not in relation to any particular circle.
+    int32_t angle = pbio_int_math_abs(distance) * 573 / pbio_int_math_abs(radius) / 10;
+    if ((radius < 0) != (distance < 0)) {
+        angle *= -1;
+    }
+
+    // Execute the common drive command at default speed (by passing 0 speed).
+    return pbio_drivebase_drive_relative(db, distance, 0, angle, 0, on_completion);
+}
+
 /**
  * Starts the drivebase controllers to run for a given duration.
  *

pybricks/robotics/pb_type_drivebase.c

@@ -184,6 +184,39 @@ static mp_obj_t pb_type_DriveBase_curve(size_t n_args, const mp_obj_t *pos_args,
 }
 static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_curve_obj, 1, pb_type_DriveBase_curve);
 
+// pybricks.robotics.DriveBase.arc
+static mp_obj_t pb_type_DriveBase_arc(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
+    PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args,
+        pb_type_DriveBase_obj_t, self,
+        PB_ARG_REQUIRED(radius),
+        PB_ARG_DEFAULT_NONE(distance),
+        PB_ARG_DEFAULT_NONE(angle),
+        PB_ARG_DEFAULT_OBJ(then, pb_Stop_HOLD_obj),
+        PB_ARG_DEFAULT_TRUE(wait));
+
+    // Parse user arguments.
+    mp_int_t radius = pb_obj_get_int(radius_in);
+    if ((distance_in == mp_const_none) == (angle_in == mp_const_none)) {
+        mp_raise_ValueError(MP_ERROR_TEXT("Please specify distance or angle but not both."));
+    }
+
+    pbio_control_on_completion_t then = pb_type_enum_get_value(then_in, &pb_enum_type_Stop);
+
+    if (distance_in != mp_const_none) {
+        pb_assert(pbio_drivebase_drive_arc_distance(self->db, radius, pb_obj_get_int(distance_in), then));
+    } else {
+        pb_assert(pbio_drivebase_drive_arc_angle(self->db, radius, pb_obj_get_int(angle_in), then));
+    }
+
+    // Old way to do parallel movement is to start and not wait on anything.
+    if (!mp_obj_is_true(wait_in)) {
+        return mp_const_none;
+    }
+    // Handle completion by awaiting or blocking.
+    return await_or_wait(self);
+}
+static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_arc_obj, 1, pb_type_DriveBase_arc);
+
 // pybricks.robotics.DriveBase.drive
 static mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
     PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args,
@@ -359,6 +392,7 @@ static const pb_attr_dict_entry_t pb_type_DriveBase_attr_dict[] = {
 
 // dir(pybricks.robotics.DriveBase)
 static const mp_rom_map_elem_t pb_type_DriveBase_locals_dict_table[] = {
+    { MP_ROM_QSTR(MP_QSTR_arc),              MP_ROM_PTR(&pb_type_DriveBase_arc_obj)      },
     { MP_ROM_QSTR(MP_QSTR_curve),            MP_ROM_PTR(&pb_type_DriveBase_curve_obj)    },
     { MP_ROM_QSTR(MP_QSTR_straight),         MP_ROM_PTR(&pb_type_DriveBase_straight_obj) },
     { MP_ROM_QSTR(MP_QSTR_turn),             MP_ROM_PTR(&pb_type_DriveBase_turn_obj)     },