From fee592555d9c56e5cb61705349a5ff36458b4772 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 20 Sep 2024 20:19:26 +0200 Subject: [PATCH] pbio/drivebase: Stop drivebase on gyro reset. --- CHANGELOG.md | 11 +++++++++-- lib/pbio/include/pbio/drivebase.h | 1 + lib/pbio/src/drivebase.c | 25 ++++++++++++++++++++----- lib/pbio/src/imu.c | 8 ++++++++ 4 files changed, 38 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5b58aaef2..6f580f78a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,8 @@ - Added one byte program identifier to the hub status report to the host. - Added interface and implementation for storing and selecting multiple code 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]). ### Changed @@ -36,9 +38,11 @@ the Bluetooth light. Only warning lights will be shown on the main button light. See ([support#1716]) and ([pybricks-micropython#261]). - Allow gyro calibration only while all motors are coasting ([support#1840]) to - prevent recalibration during very steady moves. + prevent recalibration during very steady moves ([support#1687]) - Reduced default angular velocity stationary threshold from an undocumented - 5 deg/s to 3 deg/s to reduce unwanted calibration while moving ([support#1105]). + 5 deg/s to 3 deg/s to reduce unwanted calibration while moving ([support#1105]). +- If `imu.reset_heading()` is called while a drive base is actively using the + gyro, the drive base will stop to avoid confusion ([support#1818]). ### Fixed - Fixed not able to connect to new Technic Move hub with `LWP3Device()`. @@ -58,9 +62,12 @@ [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 +[support#1617]: https://github.com/pybricks/support/issues/1617 [support#1622]: https://github.com/pybricks/support/issues/1622 [support#1678]: https://github.com/pybricks/support/issues/1678 +[support#1687]: https://github.com/pybricks/support/issues/1687 [support#1716]: https://github.com/pybricks/support/issues/1716 +[support#1818]: https://github.com/pybricks/support/issues/1818 [support#1840]: https://github.com/pybricks/support/issues/1840 ## [3.5.0] - 2024-04-11 diff --git a/lib/pbio/include/pbio/drivebase.h b/lib/pbio/include/pbio/drivebase.h index e0d5f2237..b3784c987 100644 --- a/lib/pbio/include/pbio/drivebase.h +++ b/lib/pbio/include/pbio/drivebase.h @@ -55,6 +55,7 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se void pbio_drivebase_update_all(void); bool pbio_drivebase_update_loop_is_running(pbio_drivebase_t *db); +void pbio_drivebase_stop_all_when_gyro_used(void); bool pbio_drivebase_is_done(const pbio_drivebase_t *db); pbio_error_t pbio_drivebase_is_stalled(pbio_drivebase_t *db, bool *stalled, uint32_t *stall_duration); diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index bc92a5b52..032459ebb 100644 --- a/lib/pbio/src/drivebase.c +++ b/lib/pbio/src/drivebase.c @@ -725,8 +725,8 @@ pbio_error_t pbio_drivebase_get_state_user(pbio_drivebase_t *db, int32_t *distan * 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. + * @param [in] distance Distance traveled in mm. + * @param [in] angle Angle turned in degrees. * @return Error code. */ pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_t angle) { @@ -756,13 +756,28 @@ pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_ 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); + // Synchronize heading and drivebase angle state if gyro in use. + if (db->use_gyro) { + pbio_imu_set_heading(angle); + } return PBIO_SUCCESS; } +/** + * Stops all drivebases that use the gyro. Called by the imu module when the + * imu heading is reset. Resetting it would throw off ongoing drivebase + * controls, so we should stop them. + */ +void pbio_drivebase_stop_all_when_gyro_used(void) { + for (uint8_t i = 0; i < PBIO_CONFIG_NUM_DRIVEBASES; i++) { + pbio_drivebase_t *db = &drivebases[i]; + if (pbio_drivebase_update_loop_is_running(db) && db->use_gyro) { + // Let errors pass. + pbio_drivebase_stop(db, PBIO_CONTROL_ON_COMPLETION_COAST); + } + } +} /** * Gets the drivebase settings in user units. diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index 62d7c1f09..9c2a18734 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -267,6 +268,13 @@ float pbio_imu_get_heading(void) { */ void pbio_imu_set_heading(float desired_heading) { heading_offset = pbio_imu_get_heading() + heading_offset - desired_heading; + + // Callbacks to other resources to inform that the gyro has been (re)set. + + // REVISIT: At the moment, only drivebases use the gyro. If more resources + // need it, we can enable subscribing to the imu and have a callback + // called here on resets. + pbio_drivebase_stop_all_when_gyro_used(); } /**