diff --git a/CHANGELOG.md b/CHANGELOG.md index b77c78436..c2d8e3154 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,18 @@ ## [Unreleased] +### Changed + +- The method `DriveBase.angle()` now returns a float ([support#1844]). This + makes it properly equivalent to `hub.imu.heading`. + +### Fixed +- Fixed `DriveBase.angle()` getting an incorrectly rounded gyro value, which + could cause `turn(360)` to be off by a degree ([support#1844]). + +[support#1844]: https://github.com/pybricks/support/issues/1844 +[support#1886]: https://github.com/pybricks/support/issues/1886 + ## [3.6.0b2] - 2024-10-15 ### Added diff --git a/lib/pbio/include/pbio/angle.h b/lib/pbio/include/pbio/angle.h index a23b82029..d9639f5b0 100644 --- a/lib/pbio/include/pbio/angle.h +++ b/lib/pbio/include/pbio/angle.h @@ -34,6 +34,7 @@ typedef struct _pbio_angle_t { // Conversion to and from basic types: int32_t pbio_angle_to_low_res(const pbio_angle_t *a, int32_t scale); +float pbio_angle_to_low_res_float(const pbio_angle_t *a, float scale); void pbio_angle_from_low_res(pbio_angle_t *a, int32_t input, int32_t scale); // Inplace operations on an angle: diff --git a/lib/pbio/include/pbio/control_settings.h b/lib/pbio/include/pbio/control_settings.h index 63e6dc817..dc6866c6d 100644 --- a/lib/pbio/include/pbio/control_settings.h +++ b/lib/pbio/include/pbio/control_settings.h @@ -130,6 +130,7 @@ uint32_t pbio_control_time_ms_to_ticks(uint32_t ms); uint32_t pbio_control_time_ticks_to_ms(uint32_t ticks); int32_t pbio_control_settings_ctl_to_app(const pbio_control_settings_t *s, int32_t input); int32_t pbio_control_settings_ctl_to_app_long(const pbio_control_settings_t *s, const pbio_angle_t *input); +float pbio_control_settings_ctl_to_app_long_float(const pbio_control_settings_t *s, const pbio_angle_t *input); int32_t pbio_control_settings_app_to_ctl(const pbio_control_settings_t *s, int32_t input); void pbio_control_settings_app_to_ctl_long(const pbio_control_settings_t *s, int32_t input, pbio_angle_t *output); int32_t pbio_control_settings_actuation_ctl_to_app(int32_t input); diff --git a/lib/pbio/include/pbio/drivebase.h b/lib/pbio/include/pbio/drivebase.h index 39082c2e2..0a117dd7a 100644 --- a/lib/pbio/include/pbio/drivebase.h +++ b/lib/pbio/include/pbio/drivebase.h @@ -75,6 +75,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_get_state_user_angle(pbio_drivebase_t *db, float *angle); 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); diff --git a/lib/pbio/src/angle.c b/lib/pbio/src/angle.c index 0e03ddb9f..97c3514a2 100644 --- a/lib/pbio/src/angle.c +++ b/lib/pbio/src/angle.c @@ -141,6 +141,22 @@ int32_t pbio_angle_to_low_res(const pbio_angle_t *a, int32_t scale) { return rotations_component + millidegree_component; } +/** + * Scales down high resolution angle to floating point value. + * + * @param [out] a Angle a. + * @param [in] scale Ratio between high resolution angle and input. + */ +float pbio_angle_to_low_res_float(const pbio_angle_t *a, float scale) { + + // Fail safely on zero division. + if (scale < 1) { + return 0; + } + + return a->rotations * (MDEG_PER_ROT / scale) + a->millidegrees / scale; +} + /** * Populates object from scaled-up integer value. * diff --git a/lib/pbio/src/control_settings.c b/lib/pbio/src/control_settings.c index 2f53aba4a..803c63450 100644 --- a/lib/pbio/src/control_settings.c +++ b/lib/pbio/src/control_settings.c @@ -48,7 +48,7 @@ int32_t pbio_control_settings_ctl_to_app(const pbio_control_settings_t *s, int32 } /** - * Converts position-like control units to application-specific units. + * Converts position-like control units to application-specific units (integer). * * This can be used with large inputs but there is more overhead. * @@ -60,6 +60,17 @@ int32_t pbio_control_settings_ctl_to_app_long(const pbio_control_settings_t *s, return pbio_angle_to_low_res(input, s->ctl_steps_per_app_step); } +/** + * Converts position-like control units to application-specific units (float). + * + * @param [in] s Control settings containing the scale. + * @param [in] input Signal in control units. + * @return Signal in application units. + */ +float pbio_control_settings_ctl_to_app_long_float(const pbio_control_settings_t *s, const pbio_angle_t *input) { + return pbio_angle_to_low_res_float(input, s->ctl_steps_per_app_step); +} + /** * Converts application-specific units to position-like control units. * diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index a298dc600..36a31508c 100644 --- a/lib/pbio/src/drivebase.c +++ b/lib/pbio/src/drivebase.c @@ -800,6 +800,28 @@ pbio_error_t pbio_drivebase_get_state_user(pbio_drivebase_t *db, int32_t *distan return PBIO_SUCCESS; } +/** + * Gets the drivebase angle in user units. This is the same as the angle + * returned by ::pbio_drivebase_get_state_user, but as a floating point value. + * + * @param [in] db The drivebase instance. + * @param [out] angle Angle turned in degrees, floating point. + * @return Error code. + */ +pbio_error_t pbio_drivebase_get_state_user_angle(pbio_drivebase_t *db, float *angle) { + + // Get drive base state + pbio_control_state_t state_distance; + pbio_control_state_t state_heading; + pbio_error_t err = pbio_drivebase_get_state_control(db, &state_distance, &state_heading); + if (err != PBIO_SUCCESS) { + return err; + } + + *angle = pbio_control_settings_ctl_to_app_long_float(&db->control_heading.settings, &state_heading.position); + return PBIO_SUCCESS; +} + /** * Stops the drivebase and resets the accumulated drivebase state in user units. * diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 57549ec93..e89a61290 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -279,10 +279,15 @@ MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_distance_obj, pb_type_DriveBase_dist static mp_obj_t pb_type_DriveBase_angle(mp_obj_t self_in) { pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); + #if MICROPY_PY_BUILTINS_FLOAT + float angle; + pb_assert(pbio_drivebase_get_state_user_angle(self->db, &angle)); + return mp_obj_new_float_from_f(angle); + #else int32_t heading, _; pb_assert(pbio_drivebase_get_state_user(self->db, &_, &_, &heading, &_)); - return mp_obj_new_int(heading); + #endif } MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_angle_obj, pb_type_DriveBase_angle);