From 4d6d172405b55807e492ca3c2c325a36d5120807 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 13 Jul 2021 09:42:55 +0200 Subject: [PATCH] pbio/servo: Make angle reset optional. This adds a `reset_angle=True` keyword argument to the Motor class initializer. Setting it to false allows the encoder angle to be preserved between consecutive scripts. This way, you'd need to reset robot mechanisms only once instead of every time you run a program. https://github.com/pybricks/support/issues/389 --- CHANGELOG.md | 6 ++++++ lib/pbio/include/pbio/servo.h | 2 +- lib/pbio/include/pbio/tacho.h | 2 +- lib/pbio/src/motor_process.c | 2 +- lib/pbio/src/servo.c | 4 ++-- lib/pbio/src/tacho.c | 11 ++++++++--- lib/pbio/test/src/servo.c | 2 +- pybricks/common/pb_type_motor.c | 6 ++++-- 8 files changed, 24 insertions(+), 11 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index bb48b5e14..7e1239192 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,11 @@ ## [Unreleased] +### Added +- Added `reset_angle=False` keyword argument to `Motor()` class. + This makes resetting the angle optional, allowing to maintain absolute + positioning for robots with gears ([support#389]). + ### Fixed - Fixed City hub not always powering off on shutdown ([support#385]). @@ -86,6 +91,7 @@ Prerelease changes are documented at [support#48]. [support#361]: https://github.com/pybricks/support/issues/361 [support#379]: https://github.com/pybricks/support/issues/379 [support#385]: https://github.com/pybricks/support/issues/385 +[support#389]: https://github.com/pybricks/support/issues/389 [Unreleased]: https://github.com/pybricks/pybricks-micropython/compare/v3.1.0a2...HEAD [3.1.0a2]: https://github.com/pybricks/pybricks-micropython/compare/v3.0.0a1...v3.1.0a2 diff --git a/lib/pbio/include/pbio/servo.h b/lib/pbio/include/pbio/servo.h index 97a3d7132..98e5ca21a 100644 --- a/lib/pbio/include/pbio/servo.h +++ b/lib/pbio/include/pbio/servo.h @@ -38,7 +38,7 @@ typedef struct _pbio_servo_t { pbio_log_t log; } pbio_servo_t; -pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix16_t gear_ratio); +pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix16_t gear_ratio, bool reset_angle); void pbio_servo_load_settings(pbio_control_settings_t *control_settings, const pbio_observer_settings_t **observer_settings, pbio_iodev_type_id_t id); diff --git a/lib/pbio/include/pbio/tacho.h b/lib/pbio/include/pbio/tacho.h index c200982e7..a18125bd8 100644 --- a/lib/pbio/include/pbio/tacho.h +++ b/lib/pbio/include/pbio/tacho.h @@ -16,7 +16,7 @@ typedef struct _pbio_tacho_t pbio_tacho_t; #if PBIO_CONFIG_TACHO -pbio_error_t pbio_tacho_get(pbio_port_id_t port, pbio_tacho_t **tacho, pbio_direction_t direction, fix16_t gear_ratio); +pbio_error_t pbio_tacho_get(pbio_port_id_t port, pbio_tacho_t **tacho, pbio_direction_t direction, fix16_t gear_ratio, bool reset_angle); pbio_error_t pbio_tacho_get_count(pbio_tacho_t *tacho, int32_t *count); pbio_error_t pbio_tacho_get_angle(pbio_tacho_t *tacho, int32_t *angle); diff --git a/lib/pbio/src/motor_process.c b/lib/pbio/src/motor_process.c index 7e28403f1..349ed5bff 100644 --- a/lib/pbio/src/motor_process.c +++ b/lib/pbio/src/motor_process.c @@ -47,7 +47,7 @@ void pbio_motor_process_reset(void) { pbio_servo_stop_force(srv); // Run setup and set connected flag on success - pbio_servo_set_connected(srv, pbio_servo_setup(srv, PBIO_DIRECTION_CLOCKWISE, fix16_one) == PBIO_SUCCESS); + pbio_servo_set_connected(srv, pbio_servo_setup(srv, PBIO_DIRECTION_CLOCKWISE, fix16_one, false) == PBIO_SUCCESS); } } diff --git a/lib/pbio/src/servo.c b/lib/pbio/src/servo.c index 537639c4c..a3ab33c2a 100644 --- a/lib/pbio/src/servo.c +++ b/lib/pbio/src/servo.c @@ -30,7 +30,7 @@ static pbio_error_t pbio_servo_observer_reset(pbio_servo_t *srv) { return PBIO_SUCCESS; } -pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix16_t gear_ratio) { +pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix16_t gear_ratio, bool reset_angle) { pbio_error_t err; // Return if this servo is already in use by higher level entity @@ -39,7 +39,7 @@ pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix } // Get and reset tacho - err = pbio_tacho_get(srv->port, &srv->tacho, direction, gear_ratio); + err = pbio_tacho_get(srv->port, &srv->tacho, direction, gear_ratio, reset_angle); if (err != PBIO_SUCCESS) { return err; } diff --git a/lib/pbio/src/tacho.c b/lib/pbio/src/tacho.c index c9e3b2561..a5fa5e175 100644 --- a/lib/pbio/src/tacho.c +++ b/lib/pbio/src/tacho.c @@ -56,7 +56,7 @@ static pbio_error_t pbio_tacho_reset_count_to_abs(pbio_tacho_t *tacho, int32_t * return pbio_tacho_reset_count(tacho, *abs_count); } -static pbio_error_t pbio_tacho_setup(pbio_tacho_t *tacho, uint8_t counter_id, pbio_direction_t direction, fix16_t gear_ratio) { +static pbio_error_t pbio_tacho_setup(pbio_tacho_t *tacho, uint8_t counter_id, pbio_direction_t direction, fix16_t gear_ratio, bool reset_angle) { // Assert that scaling factors are positive if (gear_ratio < 0) { return PBIO_ERROR_INVALID_ARG; @@ -73,6 +73,11 @@ static pbio_error_t pbio_tacho_setup(pbio_tacho_t *tacho, uint8_t counter_id, pb return err; } + // If there's no need to reset the angle, we are done here. + if (!reset_angle) { + return PBIO_SUCCESS; + } + // Reset count to absolute value if supported int32_t abs_count; err = pbio_tacho_reset_count_to_abs(tacho, &abs_count); @@ -83,7 +88,7 @@ static pbio_error_t pbio_tacho_setup(pbio_tacho_t *tacho, uint8_t counter_id, pb return err; } -pbio_error_t pbio_tacho_get(pbio_port_id_t port, pbio_tacho_t **tacho, pbio_direction_t direction, fix16_t gear_ratio) { +pbio_error_t pbio_tacho_get(pbio_port_id_t port, pbio_tacho_t **tacho, pbio_direction_t direction, fix16_t gear_ratio, bool reset_angle) { // Validate port if (port < PBDRV_CONFIG_FIRST_MOTOR_PORT || port > PBDRV_CONFIG_LAST_MOTOR_PORT) { return PBIO_ERROR_INVALID_PORT; @@ -96,7 +101,7 @@ pbio_error_t pbio_tacho_get(pbio_port_id_t port, pbio_tacho_t **tacho, pbio_dire uint8_t counter_id = port - PBDRV_CONFIG_FIRST_MOTOR_PORT; // Initialize and set up tacho properties - return pbio_tacho_setup(*tacho, counter_id, direction, gear_ratio); + return pbio_tacho_setup(*tacho, counter_id, direction, gear_ratio, reset_angle); } pbio_error_t pbio_tacho_get_count(pbio_tacho_t *tacho, int32_t *count) { diff --git a/lib/pbio/test/src/servo.c b/lib/pbio/test/src/servo.c index 64302f060..0ab62613f 100644 --- a/lib/pbio/test/src/servo.c +++ b/lib/pbio/test/src/servo.c @@ -89,7 +89,7 @@ static PT_THREAD(test_servo_run_func(struct pt *pt, const char *name, pbio_error tt_want(process_is_running(&pbio_motor_process)); tt_uint_op(pbio_motor_process_get_servo(PBIO_PORT_ID_A, &servo), ==, PBIO_SUCCESS); - tt_uint_op(pbio_servo_setup(servo, PBIO_DIRECTION_CLOCKWISE, F16C(1, 0)), ==, PBIO_SUCCESS); + tt_uint_op(pbio_servo_setup(servo, PBIO_DIRECTION_CLOCKWISE, F16C(1, 0), true), ==, PBIO_SUCCESS); pbio_servo_set_connected(servo, true); // only logging one row since we read it after every iteration diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index d05ee57af..d82604129 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -34,11 +34,13 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, PB_PARSE_ARGS_CLASS(n_args, n_kw, args, PB_ARG_REQUIRED(port), PB_ARG_DEFAULT_OBJ(positive_direction, pb_Direction_CLOCKWISE_obj), - PB_ARG_DEFAULT_NONE(gears)); + PB_ARG_DEFAULT_NONE(gears), + PB_ARG_DEFAULT_TRUE(reset_angle)); // Configure the motor with the selected arguments at pbio level pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); pbio_direction_t positive_direction = pb_type_enum_get_value(positive_direction_in, &pb_enum_type_Direction); + bool reset_angle = mp_obj_is_true(reset_angle_in); pbio_error_t err; pbio_servo_t *srv; @@ -82,7 +84,7 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, pb_assert(pbio_motor_process_get_servo(port, &srv)); // Set up servo - while ((err = pbio_servo_setup(srv, positive_direction, gear_ratio)) == PBIO_ERROR_AGAIN) { + while ((err = pbio_servo_setup(srv, positive_direction, gear_ratio, reset_angle)) == PBIO_ERROR_AGAIN) { mp_hal_delay_ms(1000); } pb_assert(err);