From 63de1b54b0bbdc9b3aa37b37b9c4ea801c724feb Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 24 Sep 2024 15:33:54 +0200 Subject: [PATCH] pybricks.robotics.DriveBase: Implement arc method. See https://github.com/pybricks/support/issues/1157 --- CHANGELOG.md | 3 ++ lib/pbio/include/pbio/drivebase.h | 2 + lib/pbio/src/drivebase.c | 71 +++++++++++++++++++++++++++ pybricks/robotics/pb_type_drivebase.c | 34 +++++++++++++ 4 files changed, 110 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 015c06c69..cc384b861 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/lib/pbio/include/pbio/drivebase.h b/lib/pbio/include/pbio/drivebase.h index 696645f77..39082c2e2 100644 --- a/lib/pbio/include/pbio/drivebase.h +++ b/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: diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index 68ca18373..7581ae21d 100644 --- a/lib/pbio/src/drivebase.c +++ b/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. * diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index c573e818a..57549ec93 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/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) },