pbio/dcmotor: Implement close() to re-use device.

Fixes: pybricks/support#904

Author: laurensvalk

CHANGELOG.md

@@ -4,6 +4,12 @@
 
 ## [Unreleased]
 
+### Added
+- Added `close()` method to `DCMotor` and `Motor` so they can be closed and
+  re-initialized later.
+
+[support#904]: https://github.com/pybricks/support/issues/904
+
 ## [3.2.2] - 2023-01-06
 
 ### Fixed

lib/pbio/include/pbio/dcmotor.h

@@ -65,6 +65,7 @@ void pbio_dcmotor_stop_all(bool clear_parents);
 
 // Setup and status:
 
+pbio_error_t pbio_dcmotor_close(pbio_dcmotor_t *dcmotor);
 pbio_error_t pbio_dcmotor_get_dcmotor(pbio_port_id_t port, pbio_dcmotor_t **dcmotor);
 pbio_error_t pbio_dcmotor_setup(pbio_dcmotor_t *dcmotor, pbio_direction_t direction);
 void pbio_dcmotor_get_state(pbio_dcmotor_t *dcmotor, pbio_dcmotor_actuation_t *actuation, int32_t *voltage_now);

lib/pbio/src/dcmotor.c

@@ -60,6 +60,30 @@ void pbio_dcmotor_stop_all(bool clear_parents) {
     }
 }
 
+/**
+ * Stops and closes DC motor instance so it can be used in another application.
+ *
+ * @param [in]  dcmotor     The DC motor instance.
+ * @return                  Error code.
+ */
+pbio_error_t pbio_dcmotor_close(pbio_dcmotor_t *dcmotor) {
+
+    // Coast the motor and remember error.
+    pbio_error_t stop_err = pbio_dcmotor_coast(dcmotor);
+
+    // Stop its parents and reset parent objects to free up this motor
+    // for use in new objects, even if stopping has failed.
+    pbio_error_t parent_err = pbio_parent_stop(&dcmotor->parent, true);
+
+    // Return original error corresponding to stopping the motor.
+    if (stop_err != PBIO_SUCCESS) {
+        return stop_err;
+    }
+
+    // Return error of trying to stop parent structure.
+    return parent_err;
+}
+
 /**
  * Sets up the DC motor instance to be used in an application.
  *

lib/pbio/src/servo.c

@@ -59,6 +59,12 @@ static void pbio_servo_update_loop_set_state(pbio_servo_t *srv, bool update) {
  * @return                  True if up and running, false if not.
  */
 bool pbio_servo_update_loop_is_running(pbio_servo_t *srv) {
+
+    // Servo must be the parent of its dc motor.
+    if (!pbio_parent_equals(&srv->dcmotor->parent, srv)) {
+        pbio_servo_update_loop_set_state(srv, false);
+    }
+
     return srv->run_update_loop;
 }
 

pybricks/common.h

@@ -107,6 +107,7 @@ extern const mp_obj_type_t pb_type_DCMotor;
 
 // Nonstatic objects shared between Motor and DCMotor
 void common_DCMotor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind);
+MP_DECLARE_CONST_FUN_OBJ_1(common_DCMotor_close_obj);
 MP_DECLARE_CONST_FUN_OBJ_KW(common_DCMotor_duty_obj);
 MP_DECLARE_CONST_FUN_OBJ_1(common_DCMotor_stop_obj);
 MP_DECLARE_CONST_FUN_OBJ_1(common_DCMotor_brake_obj);

pybricks/common/pb_type_dcmotor.c

@@ -125,8 +125,17 @@ STATIC mp_obj_t common_DCMotor_dc_settings(size_t n_args, const mp_obj_t *pos_ar
 }
 MP_DEFINE_CONST_FUN_OBJ_KW(common_DCMotor_dc_settings_obj, 1, common_DCMotor_dc_settings);
 
+// pybricks._common.DCMotor.close
+// pybricks._common.Motor.close
+STATIC mp_obj_t common_DCMotor_close(mp_obj_t self_in) {
+    pb_assert(pbio_dcmotor_close(get_dcmotor_from_object(self_in)));
+    return mp_const_none;
+}
+MP_DEFINE_CONST_FUN_OBJ_1(common_DCMotor_close_obj, common_DCMotor_close);
+
 // dir(pybricks.builtins.DCMotor)
 STATIC const mp_rom_map_elem_t common_DCMotor_locals_dict_table[] = {
+    { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&common_DCMotor_close_obj) },
     { MP_ROM_QSTR(MP_QSTR_dc), MP_ROM_PTR(&common_DCMotor_duty_obj) },
     { MP_ROM_QSTR(MP_QSTR_stop), MP_ROM_PTR(&common_DCMotor_stop_obj) },
     { MP_ROM_QSTR(MP_QSTR_brake), MP_ROM_PTR(&common_DCMotor_brake_obj) },

pybricks/common/pb_type_motor.c

@@ -376,6 +376,7 @@ STATIC const mp_rom_map_elem_t common_Motor_locals_dict_table[] = {
     //
     // Methods common to DC motors and encoded motors
     //
+    { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&common_DCMotor_close_obj) },
     { MP_ROM_QSTR(MP_QSTR_dc), MP_ROM_PTR(&common_DCMotor_duty_obj) },
     { MP_ROM_QSTR(MP_QSTR_stop), MP_ROM_PTR(&common_DCMotor_stop_obj) },
     { MP_ROM_QSTR(MP_QSTR_brake), MP_ROM_PTR(&common_DCMotor_brake_obj) },