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. pybricks/support#389
pybricks · Jul 13, 2021 · 4d6d172 · 4d6d172
1 parent de27858
commit 4d6d172
Show file tree

Hide file tree

Showing 8 changed files with 24 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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);