From a34d7cb8aec3ff9449bb1e757f9ffe503fee50b9 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 20 Apr 2023 14:36:47 +0200 Subject: [PATCH] pybricks.robotics: Add GyroDriveBase. This is a variant that uses the gyro for steering. This makes it easier to configure compared to specifying even more kwargs. This also lets you toggle between drive bahaviors by just changing the class name in your script, so you can determine what works best for a robot. Since this can be completely disabled on hubs without a gyro, the reduced number of parameters also reduces move hub build size a bit. --- CHANGELOG.md | 16 +++++++---- bricks/cityhub/mpconfigport.h | 1 + bricks/essentialhub/mpconfigport.h | 1 + bricks/movehub/mpconfigport.h | 1 + bricks/nxt/mpconfigport.h | 1 + bricks/primehub/mpconfigport.h | 1 + bricks/technichub/mpconfigport.h | 1 + pybricks/robotics.h | 8 ++++++ pybricks/robotics/pb_module_robotics.c | 5 +++- pybricks/robotics/pb_type_drivebase.c | 39 +++++++++++++------------- 10 files changed, 49 insertions(+), 25 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4f4d47a0a..d8fa4af92 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,9 +9,8 @@ not calibrate properly ([support#1026]). - Fixed discrepancy in heading value across hubs by accounting for sampling time ([support#1022]). - -[support#1026]: https://github.com/pybricks/support/issues/1022 -[support#1026]: https://github.com/pybricks/support/issues/1026 +- Fixed iterator for `Matrix` objects giving bad values. +- Fixed Bluetooth sometimes locking up on Technic/City hubs ([support#567]). ### Added - Added `pybricks.geometry.cross(a, b)` to get a vector cross product. @@ -21,16 +20,23 @@ required, or even undesired. - Added `hub.imu.ready()` to check that the IMU has been calibrated and is ready for use. +- Added `GyroDriveBase` class to control drivebase steering with the gyro. - Added optional `window` parameter to `Motor.speed` to specify the differentiation window size that determines the average speed. This lets the user choose smaller values to get a more responsive (but noisier) or higher values to get a smoother (but more delayed) speed signal. +### Removed +- Removed `positive_direction` from `DriveBase` initializer. This was + temporarily added in the previous beta release to facilitate gyro support, + but made it more complicated than needed ([support#992]). + ### Fixed -- Fixed iterator for `Matrix` objects giving bad values. -- Fixed Bluetooth sometimes locking up on Technic/City hubs ([support#567]). [support#567]: https://github.com/pybricks/support/issues/567 +[support#992]: https://github.com/pybricks/support/issues/992 +[support#1022]: https://github.com/pybricks/support/issues/1022 +[support#1026]: https://github.com/pybricks/support/issues/1026 ## [3.3.0b3] - 2023-03-28 diff --git a/bricks/cityhub/mpconfigport.h b/bricks/cityhub/mpconfigport.h index 2345571c4..a99141862 100644 --- a/bricks/cityhub/mpconfigport.h +++ b/bricks/cityhub/mpconfigport.h @@ -36,6 +36,7 @@ #define PYBRICKS_PY_PARAMETERS_ICON (0) #define PYBRICKS_PY_PUPDEVICES (1) #define PYBRICKS_PY_ROBOTICS (1) +#define PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO (0) #define PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE (0) #define PYBRICKS_PY_TOOLS (1) diff --git a/bricks/essentialhub/mpconfigport.h b/bricks/essentialhub/mpconfigport.h index e9706ff7b..fa5a535a5 100644 --- a/bricks/essentialhub/mpconfigport.h +++ b/bricks/essentialhub/mpconfigport.h @@ -37,6 +37,7 @@ #define PYBRICKS_PY_PARAMETERS_ICON (0) #define PYBRICKS_PY_PUPDEVICES (1) #define PYBRICKS_PY_ROBOTICS (1) +#define PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO (1) #define PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE (1) #define PYBRICKS_PY_TOOLS (1) diff --git a/bricks/movehub/mpconfigport.h b/bricks/movehub/mpconfigport.h index a6877aece..baf789f61 100644 --- a/bricks/movehub/mpconfigport.h +++ b/bricks/movehub/mpconfigport.h @@ -32,6 +32,7 @@ #define PYBRICKS_PY_PARAMETERS_ICON (0) #define PYBRICKS_PY_PUPDEVICES (1) #define PYBRICKS_PY_ROBOTICS (1) +#define PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO (0) #define PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE (0) #define PYBRICKS_PY_TOOLS (1) diff --git a/bricks/nxt/mpconfigport.h b/bricks/nxt/mpconfigport.h index 4c1e4305a..8cb0c1948 100644 --- a/bricks/nxt/mpconfigport.h +++ b/bricks/nxt/mpconfigport.h @@ -38,6 +38,7 @@ #define PYBRICKS_PY_PARAMETERS_ICON (0) #define PYBRICKS_PY_PUPDEVICES (0) #define PYBRICKS_PY_ROBOTICS (1) +#define PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO (0) #define PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE (0) #define PYBRICKS_PY_TOOLS (1) diff --git a/bricks/primehub/mpconfigport.h b/bricks/primehub/mpconfigport.h index 9b10955ea..5d05abff0 100644 --- a/bricks/primehub/mpconfigport.h +++ b/bricks/primehub/mpconfigport.h @@ -38,6 +38,7 @@ #define PYBRICKS_PY_PARAMETERS_ICON (1) #define PYBRICKS_PY_PUPDEVICES (1) #define PYBRICKS_PY_ROBOTICS (1) +#define PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO (1) #define PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE (1) #define PYBRICKS_PY_TOOLS (1) diff --git a/bricks/technichub/mpconfigport.h b/bricks/technichub/mpconfigport.h index 58b98195a..d9c2597f7 100644 --- a/bricks/technichub/mpconfigport.h +++ b/bricks/technichub/mpconfigport.h @@ -36,6 +36,7 @@ #define PYBRICKS_PY_PARAMETERS_ICON (0) #define PYBRICKS_PY_PUPDEVICES (1) #define PYBRICKS_PY_ROBOTICS (1) +#define PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO (1) #define PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE (0) #define PYBRICKS_PY_TOOLS (1) diff --git a/pybricks/robotics.h b/pybricks/robotics.h index 5b3fd821c..6cfa4242f 100644 --- a/pybricks/robotics.h +++ b/pybricks/robotics.h @@ -15,7 +15,15 @@ #include "pybricks/util_mp/pb_obj_helper.h" extern const mp_obj_type_t pb_type_drivebase; + +#if PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO +extern const mp_obj_type_t pb_type_gyrodrivebase; +#endif + +#if PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE extern const mp_obj_type_t pb_type_spikebase; +#endif + #endif // PYBRICKS_PY_ROBOTICS diff --git a/pybricks/robotics/pb_module_robotics.c b/pybricks/robotics/pb_module_robotics.c index 6abcbde25..972307c28 100644 --- a/pybricks/robotics/pb_module_robotics.c +++ b/pybricks/robotics/pb_module_robotics.c @@ -11,7 +11,10 @@ STATIC const mp_rom_map_elem_t robotics_globals_table[] = { { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_robotics) }, #if PYBRICKS_PY_COMMON_MOTORS { MP_ROM_QSTR(MP_QSTR_DriveBase), MP_ROM_PTR(&pb_type_drivebase) }, - #if (PYBRICKS_HUB_PRIMEHUB || PYBRICKS_HUB_ESSENTIALHUB) + #if PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO + { MP_ROM_QSTR(MP_QSTR_GyroDriveBase), MP_ROM_PTR(&pb_type_gyrodrivebase) }, + #endif + #if PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE { MP_ROM_QSTR(MP_QSTR_SpikeBase), MP_ROM_PTR(&pb_type_spikebase) }, #endif #endif diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index d777c7451..8d16c5547 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -53,36 +54,22 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a PB_ARG_REQUIRED(left_motor), PB_ARG_REQUIRED(right_motor), PB_ARG_REQUIRED(wheel_diameter), - PB_ARG_REQUIRED(axle_track), - PB_ARG_DEFAULT_OBJ(positive_direction, pb_Direction_CLOCKWISE_obj), - PB_ARG_DEFAULT_FALSE(use_gyro)); + PB_ARG_REQUIRED(axle_track)); pb_type_DriveBase_obj_t *self = mp_obj_malloc(pb_type_DriveBase_obj_t, type); - // REVISIT: Allow angle getter callable on any platform. - bool use_gyro = mp_obj_is_true(use_gyro_in); - #if !PBIO_CONFIG_IMU - if (use_gyro) { - pb_assert(PBIO_ERROR_NOT_SUPPORTED); - } - #endif - pbio_direction_t positive_direction = pb_type_enum_get_value(positive_direction_in, &pb_enum_type_Direction); - // REVISIT: Gyro is currently only compatible with counterclockwise drivebase. - if (use_gyro && positive_direction != PBIO_DIRECTION_COUNTERCLOCKWISE) { - pb_assert(PBIO_ERROR_INVALID_ARG); - } - // Pointers to servos pbio_servo_t *srv_left = ((common_Motor_obj_t *)pb_obj_get_base_class_obj(left_motor_in, &pb_type_Motor))->srv; pbio_servo_t *srv_right = ((common_Motor_obj_t *)pb_obj_get_base_class_obj(right_motor_in, &pb_type_Motor))->srv; // Create drivebase pb_assert(pbio_drivebase_get_drivebase(&self->db, - positive_direction == PBIO_DIRECTION_CLOCKWISE ? srv_left : srv_right, - positive_direction == PBIO_DIRECTION_CLOCKWISE ? srv_right : srv_left, + srv_left, + srv_right, pb_obj_get_scaled_int(wheel_diameter_in, 1000), pb_obj_get_scaled_int(axle_track_in, 1000), - use_gyro)); + // Use gyro if creating instance of GyroDriveBase. + type != &pb_type_drivebase)); #if PYBRICKS_PY_COMMON_CONTROL // Create instances of the Control class @@ -339,4 +326,18 @@ const mp_obj_type_t pb_type_drivebase = { .locals_dict = (mp_obj_dict_t *)&pb_type_DriveBase_locals_dict, }; +#if PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO +// type(pybricks.robotics.GyroDriveBase) +const mp_obj_type_t pb_type_gyrodrivebase = { + { &mp_type_type }, + .name = MP_QSTR_GyroDriveBase, + .make_new = pb_type_DriveBase_make_new, + #if PYBRICKS_PY_COMMON_CONTROL + .attr = pb_attribute_handler, + .protocol = pb_type_DriveBase_attr_dict, + #endif + .locals_dict = (mp_obj_dict_t *)&pb_type_DriveBase_locals_dict, +}; +#endif // PYBRICKS_PY_ROBOTICS_DRIVEBASE_GYRO + #endif // PYBRICKS_PY_ROBOTICS && PYBRICKS_PY_COMMON_MOTORS