From 541f34982b7f9a087cba66efa0ef1cd6cfeb0328 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 10 Jan 2023 15:31:56 +0100 Subject: [PATCH 01/59] pybricks.tools: Add _set_run_loop_active(). This function can be called from the pure-Python task module to inform blocking methods that they must return an awaitable generator. --- pybricks/tools.h | 2 ++ pybricks/tools/pb_module_tools.c | 20 ++++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/pybricks/tools.h b/pybricks/tools.h index f5bf85de4..ea3a961aa 100644 --- a/pybricks/tools.h +++ b/pybricks/tools.h @@ -10,6 +10,8 @@ #include "py/obj.h" +bool pb_module_tools_run_loop_is_active(); + extern const mp_obj_type_t pb_type_StopWatch; #endif // PYBRICKS_PY_TOOLS diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 1ce45823b..361861c00 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -28,8 +28,28 @@ STATIC mp_obj_t tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(tools_wait_obj, 0, tools_wait); +STATIC bool _pb_module_tools_run_loop_is_active; + +bool pb_module_tools_run_loop_is_active() { + return _pb_module_tools_run_loop_is_active; +} + +STATIC mp_obj_t pb_module_tools___init__(void) { + _pb_module_tools_run_loop_is_active = false; + return mp_const_none; +} +MP_DEFINE_CONST_FUN_OBJ_0(pb_module_tools___init___obj, pb_module_tools___init__); + +STATIC mp_obj_t pb_module_tools_set_run_loop_active(mp_obj_t self_in) { + _pb_module_tools_run_loop_is_active = mp_obj_is_true(self_in); + return mp_const_none; +} +MP_DEFINE_CONST_FUN_OBJ_1(pb_module_tools_set_run_loop_active_obj, pb_module_tools_set_run_loop_active); + STATIC const mp_rom_map_elem_t tools_globals_table[] = { { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_tools) }, + { MP_ROM_QSTR(MP_QSTR___init__), MP_ROM_PTR(&pb_module_tools___init___obj)}, + { MP_ROM_QSTR(MP_QSTR__set_run_loop_active), MP_ROM_PTR(&pb_module_tools_set_run_loop_active_obj)}, { MP_ROM_QSTR(MP_QSTR_wait), MP_ROM_PTR(&tools_wait_obj) }, { MP_ROM_QSTR(MP_QSTR_StopWatch), MP_ROM_PTR(&pb_type_StopWatch) }, #if MICROPY_PY_BUILTINS_FLOAT From 49f453e0ebf6f309a62c56c0fb5031da81e8938c Mon Sep 17 00:00:00 2001 From: David Lechner Date: Mon, 31 May 2021 16:21:55 -0500 Subject: [PATCH 02/59] pybricks.common.Motor: Return generator object in run loop. This adds a new MotorWait object that is generator-like (send() and throw() are omitted since we shouldn't need them) to provide a non- blocking way to wait for completion of a motor operation. --- pybricks/common/pb_type_motor.c | 88 +++++++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 3bbbe81d1..0309bb8ff 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -17,12 +17,82 @@ #include #include #include +#include #include #include #include #include +/** + * A generator-like type for waiting on a motor operation to complete. + */ +typedef struct { + mp_obj_base_t base; + /** + * Servo object that this object is awaiting on. + */ + pbio_servo_t *srv; +} pb_type_MotorWait_obj_t; + +// The __next__() method should raise a StopIteration if the operation is +// complete. +STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { + pb_type_MotorWait_obj_t *self = MP_OBJ_TO_PTR(self_in); + + if (0 /*cancelled*/) { + // TODO: Handle cancelled. + return MP_OBJ_STOP_ITERATION; + } + + // Handle I/O exceptions. + if (!pbio_servo_update_loop_is_running(self->srv)) { + pb_assert(PBIO_ERROR_NO_DEV); + } + + // If not done yet, keep going. + if (!pbio_control_is_done(&self->srv->control)) { + return mp_const_none; + } + + // Otherwise we are completing a normal operation. + return MP_OBJ_STOP_ITERATION; +} + +// The close() method is used to cancel an operation before it completes. If +// the operation is already complete, it should do nothing. It can be called +// more than once. +STATIC mp_obj_t pb_type_MotorWait_close(mp_obj_t self_in) { + pb_type_MotorWait_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // REVISIT: We only need to call an explicit stop if a task is cancelled + // by something other than a motor. + pb_assert(pbio_dcmotor_user_command(self->srv->dcmotor, true, 0)); + + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_MotorWait_close_obj, pb_type_MotorWait_close); + +STATIC const mp_rom_map_elem_t pb_type_MotorWait_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_MotorWait_close_obj) }, +}; +MP_DEFINE_CONST_DICT(pb_type_MotorWait_locals_dict, pb_type_MotorWait_locals_dict_table); + +// This is a partial implementation of the Python generator type. It is missing +// send(value) and throw(type[, value[, traceback]]) +MP_DEFINE_CONST_OBJ_TYPE(pb_type_MotorWait, + MP_QSTR_MotorWait, + MP_TYPE_FLAG_ITER_IS_ITERNEXT, + iter, pb_type_MotorWait_iternext, + locals_dict, &pb_type_MotorWait_locals_dict); + +STATIC mp_obj_t pb_type_MotorWait_new(pbio_servo_t *srv) { + pb_type_MotorWait_obj_t *self = m_new_obj(pb_type_MotorWait_obj_t); + self->base.type = &pb_type_MotorWait; + self->srv = srv; + return MP_OBJ_FROM_PTR(self); +} + /* Wait for servo maneuver to complete */ STATIC void wait_for_completion(pbio_servo_t *srv) { @@ -215,6 +285,12 @@ STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, m // Call pbio with parsed user/default arguments pb_assert(pbio_servo_run_time(self->srv, speed, time, then)); + // Handle async case, return a generator. + if (pb_module_tools_run_loop_is_active()) { + return pb_type_MotorWait_new(self->srv); + } + + // Otherwise, handle default blocking wait. if (mp_obj_is_true(wait_in)) { wait_for_completion(self->srv); } @@ -311,6 +387,12 @@ STATIC mp_obj_t common_Motor_run_angle(size_t n_args, const mp_obj_t *pos_args, // Call pbio with parsed user/default arguments pb_assert(pbio_servo_run_angle(self->srv, speed, angle, then)); + // Handle async case, return a generator. + if (pb_module_tools_run_loop_is_active()) { + return pb_type_MotorWait_new(self->srv); + } + + // Otherwise, handle default blocking wait. if (mp_obj_is_true(wait_in)) { wait_for_completion(self->srv); } @@ -335,6 +417,12 @@ STATIC mp_obj_t common_Motor_run_target(size_t n_args, const mp_obj_t *pos_args, // Call pbio with parsed user/default arguments pb_assert(pbio_servo_run_target(self->srv, speed, target_angle, then)); + // Handle async case, return a generator. + if (pb_module_tools_run_loop_is_active()) { + return pb_type_MotorWait_new(self->srv); + } + + // Otherwise, handle default blocking wait. if (mp_obj_is_true(wait_in)) { wait_for_completion(self->srv); } From 8981245508f27dfcde56b9ae45e0c6a313a09937 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 12 Jan 2023 13:30:25 +0100 Subject: [PATCH 03/59] pybricks.common.Motor: Async run_until_stalled. --- pybricks/common/pb_type_motor.c | 56 ++++++++++++++++++++++++++++++--- 1 file changed, 52 insertions(+), 4 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 0309bb8ff..fdac46bc5 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -33,6 +33,16 @@ typedef struct { * Servo object that this object is awaiting on. */ pbio_servo_t *srv; + /** + * Voltage cap prior to starting. Only applicable for run_until_stalled. + * + * Set to -1 to indicate that this is not used. + */ + int32_t stall_voltage_restore_value; + /** + * What to do once stalled. Only applicable for run_until_stalled. + */ + pbio_control_on_completion_t stall_stop_type; } pb_type_MotorWait_obj_t; // The __next__() method should raise a StopIteration if the operation is @@ -50,13 +60,34 @@ STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { pb_assert(PBIO_ERROR_NO_DEV); } - // If not done yet, keep going. - if (!pbio_control_is_done(&self->srv->control)) { + // First, handle normal case without stalling. + if (self->stall_voltage_restore_value == -1) { + return pbio_control_is_done(&self->srv->control) ? + MP_OBJ_STOP_ITERATION : mp_const_none; + } + + // The remainder handles run_until_stalled. First, check stall state. + bool stalled; + uint32_t stall_duration; + pb_assert(pbio_servo_is_stalled(self->srv, &stalled, &stall_duration)); + + // Keep going if not stalled. + if (!stalled) { return mp_const_none; } - // Otherwise we are completing a normal operation. - return MP_OBJ_STOP_ITERATION; + // Read the angle upon completion of the stall maneuver. + int32_t stall_angle, stall_speed; + pb_assert(pbio_servo_get_state_user(self->srv, &stall_angle, &stall_speed)); + + // Stop moving. + pb_assert(pbio_servo_stop(self->srv, self->stall_stop_type)); + + // Restore original voltage limit. + pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, self->stall_voltage_restore_value)); + + // Raise stop iteration with angle to return the final position. + return mp_make_stop_iteration(mp_obj_new_int(stall_angle)); } // The close() method is used to cancel an operation before it completes. If @@ -90,6 +121,16 @@ STATIC mp_obj_t pb_type_MotorWait_new(pbio_servo_t *srv) { pb_type_MotorWait_obj_t *self = m_new_obj(pb_type_MotorWait_obj_t); self->base.type = &pb_type_MotorWait; self->srv = srv; + self->stall_voltage_restore_value = -1; + return MP_OBJ_FROM_PTR(self); +} + +STATIC mp_obj_t pb_type_MotorWait_new_stalled(pbio_servo_t *srv, int32_t stall_voltage_restore_value, int32_t stall_stop_type) { + pb_type_MotorWait_obj_t *self = m_new_obj(pb_type_MotorWait_obj_t); + self->base.type = &pb_type_MotorWait; + self->srv = srv; + self->stall_stop_type = stall_stop_type; + self->stall_voltage_restore_value = stall_voltage_restore_value; return MP_OBJ_FROM_PTR(self); } @@ -328,6 +369,13 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, max_voltage)); } + // Handle async await. + if (pb_module_tools_run_loop_is_active()) { + pb_assert(pbio_servo_run_forever(self->srv, speed)); + return pb_type_MotorWait_new_stalled(self->srv, max_voltage_old, on_completion); + } + + // The remainder handles blocking case. mp_obj_t ex = MP_OBJ_NULL; nlr_buf_t nlr; if (nlr_push(&nlr) == 0) { From 07f0f9c706e8b0056025a6947b709a7187c12079 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 12 Jan 2023 14:15:06 +0100 Subject: [PATCH 04/59] pybricks.common.Motor: Use generator for blocking run_until_stalled. This reworks the blocking case of run_until_stalled to just use the generator approach as well. This reduces code duplication. This would get even simpler if we didn't have handle the exception here but restored the voltage on cancellation in general. --- pybricks/common/pb_type_motor.c | 70 ++++++++++++--------------------- 1 file changed, 26 insertions(+), 44 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index fdac46bc5..357181eaf 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -351,14 +351,12 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po mp_int_t speed = pb_obj_get_int(speed_in); pbio_control_on_completion_t on_completion = pb_type_enum_get_value(then_in, &pb_enum_type_Stop); - // If duty_limit argument given, limit duty during this maneuver. - bool override_max_voltage = duty_limit_in != mp_const_none; + // Read original voltage limit so we can restore it when we're done. int32_t max_voltage_old; + pbio_dcmotor_get_settings(self->srv->dcmotor, &max_voltage_old); - if (override_max_voltage) { - // Read original value so we can restore it when we're done - pbio_dcmotor_get_settings(self->srv->dcmotor, &max_voltage_old); - + // If duty_limit argument given, limit duty during this maneuver. + if (duty_limit_in != mp_const_none) { // Internally, the use of a duty cycle limit has been deprecated and // replaced by a voltage limit. Since we can't break the user API, we // convert the user duty limit (0--100) to a voltage by scaling it with @@ -369,53 +367,37 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, max_voltage)); } - // Handle async await. + // Start moving. + pb_assert(pbio_servo_run_forever(self->srv, speed)); + + // Make generator for checking completion. + mp_obj_t gen = pb_type_MotorWait_new_stalled(self->srv, max_voltage_old, on_completion); + + // Within run loop, just return the generator. if (pb_module_tools_run_loop_is_active()) { - pb_assert(pbio_servo_run_forever(self->srv, speed)); - return pb_type_MotorWait_new_stalled(self->srv, max_voltage_old, on_completion); + return gen; } - // The remainder handles blocking case. - mp_obj_t ex = MP_OBJ_NULL; + // The remainder handles blocking case. It is wrapped in an nlr so + // we can restore the voltage limit if an exception occurs. nlr_buf_t nlr; if (nlr_push(&nlr) == 0) { - // Start moving forever. - pb_assert(pbio_servo_run_forever(self->srv, speed)); - - // Wait until the motor stalls or stops on failure. - uint32_t stall_duration; - while (!pbio_control_is_stalled(&self->srv->control, &stall_duration)) { + // Await the generator. + while (true) { + mp_obj_t next = pb_type_MotorWait_iternext(gen); + if (next != mp_const_none) { + mp_obj_t ret_val = MP_STATE_THREAD(stop_iteration_arg); + nlr_pop(); + return ret_val; + } + // Not complete, keep waiting. mp_hal_delay_ms(5); } - - // Assert that no errors happened in the update loop. - if (!pbio_servo_update_loop_is_running(self->srv)) { - pb_assert(PBIO_ERROR_NO_DEV); - } - - nlr_pop(); } else { - ex = MP_OBJ_FROM_PTR(nlr.ret_val); + pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, + ((pb_type_MotorWait_obj_t *)MP_OBJ_TO_PTR(gen))->stall_voltage_restore_value)); + nlr_raise(MP_OBJ_FROM_PTR(nlr.ret_val)); } - - // Restore original settings - if (override_max_voltage) { - pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, max_voltage_old)); - } - - if (ex != MP_OBJ_NULL) { - nlr_raise(ex); - } - - // Read the angle upon completion of the stall maneuver - int32_t stall_angle, stall_speed; - pb_assert(pbio_servo_get_state_user(self->srv, &stall_angle, &stall_speed)); - - // Stop moving. - pb_assert(pbio_servo_stop(self->srv, on_completion)); - - // Return angle at which the motor stalled - return mp_obj_new_int(stall_angle); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_until_stalled_obj, 1, common_Motor_run_until_stalled); From 02fe7fcf15be664710bc8b8b15d7380a750638b7 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 12 Jan 2023 14:39:02 +0100 Subject: [PATCH 05/59] pybricks.common.Motor: Move pb_type_MotorWait_obj_t. This prepares for refactoring to reduce allocation of generator objects. --- pybricks/common.h | 29 ++++++++++++++-- pybricks/common/pb_type_motor.c | 59 ++++++++++----------------------- 2 files changed, 44 insertions(+), 44 deletions(-) diff --git a/pybricks/common.h b/pybricks/common.h index 28db44c59..659624d71 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -95,8 +95,33 @@ mp_obj_t pb_type_MotorModel_obj_make_new(pbio_observer_t *observer); mp_obj_t common_Logger_obj_make_new(pbio_log_t *log, uint8_t num_values); #endif +typedef struct _common_Motor_obj_t common_Motor_obj_t; + +typedef struct _pb_type_MotorWait_obj_t pb_type_MotorWait_obj_t; + +/** + * A generator-like type for waiting on a motor operation to complete. + */ +struct _pb_type_MotorWait_obj_t { + mp_obj_base_t base; + /** + * Motor object whose move this generator is awaiting on. + */ + common_Motor_obj_t *motor_obj; + /** + * Voltage cap prior to starting. Only applicable for run_until_stalled. + * + * Set to -1 to indicate that this is not used. + */ + int32_t stall_voltage_restore_value; + /** + * What to do once stalled. Only applicable for run_until_stalled. + */ + pbio_control_on_completion_t stall_stop_type; +}; + // pybricks._common.Motor() -typedef struct _common_Motor_obj_t { +struct _common_Motor_obj_t { mp_obj_base_t base; pbio_servo_t *srv; #if PYBRICKS_PY_COMMON_MOTOR_MODEL @@ -109,7 +134,7 @@ typedef struct _common_Motor_obj_t { mp_obj_t logger; #endif pbio_port_id_t port; -} common_Motor_obj_t; +}; extern const mp_obj_type_t pb_type_Motor; diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 357181eaf..470424afd 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -24,27 +24,6 @@ #include #include -/** - * A generator-like type for waiting on a motor operation to complete. - */ -typedef struct { - mp_obj_base_t base; - /** - * Servo object that this object is awaiting on. - */ - pbio_servo_t *srv; - /** - * Voltage cap prior to starting. Only applicable for run_until_stalled. - * - * Set to -1 to indicate that this is not used. - */ - int32_t stall_voltage_restore_value; - /** - * What to do once stalled. Only applicable for run_until_stalled. - */ - pbio_control_on_completion_t stall_stop_type; -} pb_type_MotorWait_obj_t; - // The __next__() method should raise a StopIteration if the operation is // complete. STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { @@ -56,20 +35,20 @@ STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { } // Handle I/O exceptions. - if (!pbio_servo_update_loop_is_running(self->srv)) { + if (!pbio_servo_update_loop_is_running(self->motor_obj->srv)) { pb_assert(PBIO_ERROR_NO_DEV); } // First, handle normal case without stalling. if (self->stall_voltage_restore_value == -1) { - return pbio_control_is_done(&self->srv->control) ? + return pbio_control_is_done(&self->motor_obj->srv->control) ? MP_OBJ_STOP_ITERATION : mp_const_none; } // The remainder handles run_until_stalled. First, check stall state. bool stalled; uint32_t stall_duration; - pb_assert(pbio_servo_is_stalled(self->srv, &stalled, &stall_duration)); + pb_assert(pbio_servo_is_stalled(self->motor_obj->srv, &stalled, &stall_duration)); // Keep going if not stalled. if (!stalled) { @@ -78,13 +57,13 @@ STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { // Read the angle upon completion of the stall maneuver. int32_t stall_angle, stall_speed; - pb_assert(pbio_servo_get_state_user(self->srv, &stall_angle, &stall_speed)); + pb_assert(pbio_servo_get_state_user(self->motor_obj->srv, &stall_angle, &stall_speed)); // Stop moving. - pb_assert(pbio_servo_stop(self->srv, self->stall_stop_type)); + pb_assert(pbio_servo_stop(self->motor_obj->srv, self->stall_stop_type)); // Restore original voltage limit. - pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, self->stall_voltage_restore_value)); + pb_assert(pbio_dcmotor_set_settings(self->motor_obj->srv->dcmotor, self->stall_voltage_restore_value)); // Raise stop iteration with angle to return the final position. return mp_make_stop_iteration(mp_obj_new_int(stall_angle)); @@ -98,7 +77,7 @@ STATIC mp_obj_t pb_type_MotorWait_close(mp_obj_t self_in) { // REVISIT: We only need to call an explicit stop if a task is cancelled // by something other than a motor. - pb_assert(pbio_dcmotor_user_command(self->srv->dcmotor, true, 0)); + pb_assert(pbio_dcmotor_user_command(self->motor_obj->srv->dcmotor, true, 0)); return mp_const_none; } @@ -117,23 +96,19 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_MotorWait, iter, pb_type_MotorWait_iternext, locals_dict, &pb_type_MotorWait_locals_dict); -STATIC mp_obj_t pb_type_MotorWait_new(pbio_servo_t *srv) { - pb_type_MotorWait_obj_t *self = m_new_obj(pb_type_MotorWait_obj_t); - self->base.type = &pb_type_MotorWait; - self->srv = srv; - self->stall_voltage_restore_value = -1; - return MP_OBJ_FROM_PTR(self); -} - -STATIC mp_obj_t pb_type_MotorWait_new_stalled(pbio_servo_t *srv, int32_t stall_voltage_restore_value, int32_t stall_stop_type) { +STATIC mp_obj_t pb_type_MotorWait_new_stalled(common_Motor_obj_t *motor_obj, int32_t stall_voltage_restore_value, int32_t stall_stop_type) { pb_type_MotorWait_obj_t *self = m_new_obj(pb_type_MotorWait_obj_t); self->base.type = &pb_type_MotorWait; - self->srv = srv; + self->motor_obj = motor_obj; self->stall_stop_type = stall_stop_type; self->stall_voltage_restore_value = stall_voltage_restore_value; return MP_OBJ_FROM_PTR(self); } +STATIC mp_obj_t pb_type_MotorWait_new(common_Motor_obj_t *motor_obj) { + return pb_type_MotorWait_new_stalled(motor_obj, -1, PBIO_CONTROL_ON_COMPLETION_COAST); +} + /* Wait for servo maneuver to complete */ STATIC void wait_for_completion(pbio_servo_t *srv) { @@ -328,7 +303,7 @@ STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, m // Handle async case, return a generator. if (pb_module_tools_run_loop_is_active()) { - return pb_type_MotorWait_new(self->srv); + return pb_type_MotorWait_new(self); } // Otherwise, handle default blocking wait. @@ -371,7 +346,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po pb_assert(pbio_servo_run_forever(self->srv, speed)); // Make generator for checking completion. - mp_obj_t gen = pb_type_MotorWait_new_stalled(self->srv, max_voltage_old, on_completion); + mp_obj_t gen = pb_type_MotorWait_new_stalled(self, max_voltage_old, on_completion); // Within run loop, just return the generator. if (pb_module_tools_run_loop_is_active()) { @@ -419,7 +394,7 @@ STATIC mp_obj_t common_Motor_run_angle(size_t n_args, const mp_obj_t *pos_args, // Handle async case, return a generator. if (pb_module_tools_run_loop_is_active()) { - return pb_type_MotorWait_new(self->srv); + return pb_type_MotorWait_new(self); } // Otherwise, handle default blocking wait. @@ -449,7 +424,7 @@ STATIC mp_obj_t common_Motor_run_target(size_t n_args, const mp_obj_t *pos_args, // Handle async case, return a generator. if (pb_module_tools_run_loop_is_active()) { - return pb_type_MotorWait_new(self->srv); + return pb_type_MotorWait_new(self); } // Otherwise, handle default blocking wait. From ec7353457681889bdfc1a99ef1a4c67a87c336ba Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 12 Jan 2023 15:05:55 +0100 Subject: [PATCH 06/59] pybricks.common.Motor: Recycle free generators. This only allocates a new generator if all previous generators are in use. In a "good" program with no conflicting motor tasks, this means nothing is allocated outside the motor object at all. --- pybricks/common.h | 9 +++++++++ pybricks/common/pb_type_motor.c | 25 ++++++++++++++++++++++--- 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/pybricks/common.h b/pybricks/common.h index 659624d71..8a68402cb 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -118,6 +118,14 @@ struct _pb_type_MotorWait_obj_t { * What to do once stalled. Only applicable for run_until_stalled. */ pbio_control_on_completion_t stall_stop_type; + /** + * Whether this generator object is done (and thus can be recycled.) + */ + bool has_ended; + /** + * Linked list of awaitables. + */ + pb_type_MotorWait_obj_t *next_awaitable; }; // pybricks._common.Motor() @@ -134,6 +142,7 @@ struct _common_Motor_obj_t { mp_obj_t logger; #endif pbio_port_id_t port; + pb_type_MotorWait_obj_t awaitable; }; extern const mp_obj_type_t pb_type_Motor; diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 470424afd..23eeac40a 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -31,6 +31,7 @@ STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { if (0 /*cancelled*/) { // TODO: Handle cancelled. + self->has_ended = true; return MP_OBJ_STOP_ITERATION; } @@ -41,8 +42,12 @@ STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { // First, handle normal case without stalling. if (self->stall_voltage_restore_value == -1) { - return pbio_control_is_done(&self->motor_obj->srv->control) ? - MP_OBJ_STOP_ITERATION : mp_const_none; + if (pbio_control_is_done(&self->motor_obj->srv->control)) { + self->has_ended = true; + return MP_OBJ_STOP_ITERATION; + } + // Not done, so keep going. + return mp_const_none; } // The remainder handles run_until_stalled. First, check stall state. @@ -66,6 +71,7 @@ STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { pb_assert(pbio_dcmotor_set_settings(self->motor_obj->srv->dcmotor, self->stall_voltage_restore_value)); // Raise stop iteration with angle to return the final position. + self->has_ended = true; return mp_make_stop_iteration(mp_obj_new_int(stall_angle)); } @@ -97,9 +103,19 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_MotorWait, locals_dict, &pb_type_MotorWait_locals_dict); STATIC mp_obj_t pb_type_MotorWait_new_stalled(common_Motor_obj_t *motor_obj, int32_t stall_voltage_restore_value, int32_t stall_stop_type) { - pb_type_MotorWait_obj_t *self = m_new_obj(pb_type_MotorWait_obj_t); + // Find next available previously allocated awaitable. + pb_type_MotorWait_obj_t *self = &motor_obj->awaitable; + while (!self->has_ended && self->next_awaitable != MP_OBJ_NULL) { + self = self->next_awaitable; + } + // No free awaitable available, so allocate one. + if (!self->has_ended) { + self = m_new_obj(pb_type_MotorWait_obj_t); + } self->base.type = &pb_type_MotorWait; self->motor_obj = motor_obj; + self->next_awaitable = MP_OBJ_NULL; + self->has_ended = false; self->stall_stop_type = stall_stop_type; self->stall_voltage_restore_value = stall_voltage_restore_value; return MP_OBJ_FROM_PTR(self); @@ -204,6 +220,9 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, self->srv = srv; self->port = port; + // Indicate that awaitable singleton is ready for re-use. + self->awaitable.has_ended = true; + #if PYBRICKS_PY_COMMON_CONTROL // Create an instance of the Control class self->control = pb_type_Control_obj_make_new(&self->srv->control); From 235d90142f51eec7275a1aa1a17460d6aa8f5772 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 12 Jan 2023 15:36:32 +0100 Subject: [PATCH 07/59] pybricks.common.Motor: Cancel ongoing generators on new task. If a motor is running and another task calls the same motor, the old generator gracefully stops waiting. This matches the low-level implementation of having the latest command always win. --- pybricks/common.h | 7 ++++++- pybricks/common/pb_type_motor.c | 16 +++++++++++++--- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/pybricks/common.h b/pybricks/common.h index 8a68402cb..5e93fc952 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -119,7 +119,12 @@ struct _pb_type_MotorWait_obj_t { */ pbio_control_on_completion_t stall_stop_type; /** - * Whether this generator object is done (and thus can be recycled.) + * Whether this generator object was cancelled. + */ + bool was_cancelled; + /** + * Whether this generator object is done and thus can be recycled. This is + * when motion is complete or cancellation has been handled. */ bool has_ended; /** diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 23eeac40a..ac2d0ae91 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -29,8 +29,9 @@ STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { pb_type_MotorWait_obj_t *self = MP_OBJ_TO_PTR(self_in); - if (0 /*cancelled*/) { - // TODO: Handle cancelled. + if (self->was_cancelled) { + // Gracefully handle cancellation: Allow generator to complete + // and clean up but don't raise any exceptions. self->has_ended = true; return MP_OBJ_STOP_ITERATION; } @@ -103,8 +104,16 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_MotorWait, locals_dict, &pb_type_MotorWait_locals_dict); STATIC mp_obj_t pb_type_MotorWait_new_stalled(common_Motor_obj_t *motor_obj, int32_t stall_voltage_restore_value, int32_t stall_stop_type) { - // Find next available previously allocated awaitable. + + // Cancel all generators belonging to this motor. pb_type_MotorWait_obj_t *self = &motor_obj->awaitable; + do { + self->was_cancelled = true; + self = self->next_awaitable; + } while (self != MP_OBJ_NULL); + + // Find next available previously allocated awaitable. + self = &motor_obj->awaitable; while (!self->has_ended && self->next_awaitable != MP_OBJ_NULL) { self = self->next_awaitable; } @@ -116,6 +125,7 @@ STATIC mp_obj_t pb_type_MotorWait_new_stalled(common_Motor_obj_t *motor_obj, int self->motor_obj = motor_obj; self->next_awaitable = MP_OBJ_NULL; self->has_ended = false; + self->was_cancelled = false; self->stall_stop_type = stall_stop_type; self->stall_voltage_restore_value = stall_voltage_restore_value; return MP_OBJ_FROM_PTR(self); From b0c230577a534cdd5cf74a7e1a5262b657de2f21 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 12 Jan 2023 16:32:34 +0100 Subject: [PATCH 08/59] pybricks.common.Motor: Cancel ongoing generators on new non-blocking task. We also want to cancel ongoing generators if we run new non-blocking operations. Also fix initialization/allocation of new awaitables. --- pybricks/common.h | 2 +- pybricks/common/pb_type_motor.c | 56 +++++++++++++++++++++++---------- 2 files changed, 41 insertions(+), 17 deletions(-) diff --git a/pybricks/common.h b/pybricks/common.h index 5e93fc952..f9d70afc5 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -147,7 +147,7 @@ struct _common_Motor_obj_t { mp_obj_t logger; #endif pbio_port_id_t port; - pb_type_MotorWait_obj_t awaitable; + pb_type_MotorWait_obj_t first_awaitable; }; extern const mp_obj_type_t pb_type_Motor; diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index ac2d0ae91..b60cf6e16 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -76,16 +76,22 @@ STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { return mp_make_stop_iteration(mp_obj_new_int(stall_angle)); } +// Cancel all generators belonging to this motor. +STATIC void pb_type_MotorWait_cancel_all(common_Motor_obj_t *motor_obj) { + pb_type_MotorWait_obj_t *self = &motor_obj->first_awaitable; + do { + self->was_cancelled = true; + self = self->next_awaitable; + } while (self != MP_OBJ_NULL); +} + // The close() method is used to cancel an operation before it completes. If // the operation is already complete, it should do nothing. It can be called // more than once. STATIC mp_obj_t pb_type_MotorWait_close(mp_obj_t self_in) { pb_type_MotorWait_obj_t *self = MP_OBJ_TO_PTR(self_in); - - // REVISIT: We only need to call an explicit stop if a task is cancelled - // by something other than a motor. + pb_type_MotorWait_cancel_all(self->motor_obj); pb_assert(pbio_dcmotor_user_command(self->motor_obj->srv->dcmotor, true, 0)); - return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_MotorWait_close_obj, pb_type_MotorWait_close); @@ -105,29 +111,33 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_MotorWait, STATIC mp_obj_t pb_type_MotorWait_new_stalled(common_Motor_obj_t *motor_obj, int32_t stall_voltage_restore_value, int32_t stall_stop_type) { - // Cancel all generators belonging to this motor. - pb_type_MotorWait_obj_t *self = &motor_obj->awaitable; - do { - self->was_cancelled = true; - self = self->next_awaitable; - } while (self != MP_OBJ_NULL); + // Cancel everything for this motor. + pb_type_MotorWait_cancel_all(motor_obj); // Find next available previously allocated awaitable. - self = &motor_obj->awaitable; + pb_type_MotorWait_obj_t *self = &motor_obj->first_awaitable; while (!self->has_ended && self->next_awaitable != MP_OBJ_NULL) { self = self->next_awaitable; } // No free awaitable available, so allocate one. if (!self->has_ended) { - self = m_new_obj(pb_type_MotorWait_obj_t); + // Attach to the previous one. + self->next_awaitable = m_new_obj(pb_type_MotorWait_obj_t); + + // Initialize the new awaitable. + self = self->next_awaitable; + self->next_awaitable = MP_OBJ_NULL; + self->base.type = &pb_type_MotorWait; } - self->base.type = &pb_type_MotorWait; + + // Initialize what to await on. self->motor_obj = motor_obj; - self->next_awaitable = MP_OBJ_NULL; self->has_ended = false; self->was_cancelled = false; self->stall_stop_type = stall_stop_type; self->stall_voltage_restore_value = stall_voltage_restore_value; + + // Return the awaitable where the user can await it. return MP_OBJ_FROM_PTR(self); } @@ -230,8 +240,10 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, self->srv = srv; self->port = port; - // Indicate that awaitable singleton is ready for re-use. - self->awaitable.has_ended = true; + // Initialize first awaitable singleton. + self->first_awaitable.base.type = &pb_type_MotorWait; + self->first_awaitable.has_ended = true; + self->first_awaitable.next_awaitable = MP_OBJ_NULL; #if PYBRICKS_PY_COMMON_CONTROL // Create an instance of the Control class @@ -276,6 +288,9 @@ STATIC mp_obj_t common_Motor_reset_angle(size_t n_args, const mp_obj_t *pos_args // Set the new angle pb_assert(pbio_servo_reset_angle(self->srv, reset_angle, reset_to_abs)); + // Cancel user-level motor wait operations. + pb_type_MotorWait_cancel_all(self); + return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_reset_angle_obj, 1, common_Motor_reset_angle); @@ -301,6 +316,9 @@ STATIC mp_obj_t common_Motor_run(size_t n_args, const mp_obj_t *pos_args, mp_map mp_int_t speed = pb_obj_get_int(speed_in); pb_assert(pbio_servo_run_forever(self->srv, speed)); + // Cancel user-level motor wait operations in parallel tasks. + pb_type_MotorWait_cancel_all(self); + return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); @@ -309,6 +327,9 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); STATIC mp_obj_t common_Motor_hold(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_HOLD)); + + // Cancel user-level motor wait operations in parallel tasks. + pb_type_MotorWait_cancel_all(self); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_hold_obj, common_Motor_hold); @@ -474,6 +495,9 @@ STATIC mp_obj_t common_Motor_track_target(size_t n_args, const mp_obj_t *pos_arg mp_int_t target_angle = pb_obj_get_int(target_angle_in); pb_assert(pbio_servo_track_target(self->srv, target_angle)); + // Cancel user-level motor wait operations in parallel tasks. + pb_type_MotorWait_cancel_all(self); + return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_track_target_obj, 1, common_Motor_track_target); From 451115ec24d735605f6bbd8cec7a441b86ad2e37 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 13 Jan 2023 10:15:03 +0100 Subject: [PATCH 09/59] pybricks.tools: Make wait awaitable in run loop. In the run loop, wait(time) returns a generator that can be awaited. --- pybricks/tools/pb_module_tools.c | 102 +++++++++++++++++++++++++++++++ 1 file changed, 102 insertions(+) diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 361861c00..af00877b8 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -9,6 +9,7 @@ #include "py/runtime.h" #include +#include #include #include @@ -16,11 +17,111 @@ #include #include +/** + * A generator-like type for waiting on a motor operation to complete. + */ +typedef struct _pb_type_tools_wait_obj_t pb_type_tools_wait_obj_t; + +struct _pb_type_tools_wait_obj_t { + mp_obj_base_t base; + /** + * When to stop waiting. + */ + uint32_t end_time; + /** + * Whether this generator object is done and thus can be recycled. + */ + bool has_ended; + /** + * Linked list of awaitables. + */ + pb_type_tools_wait_obj_t *next_awaitable; +}; + +STATIC mp_obj_t pb_type_tools_wait_iternext(mp_obj_t self_in) { + pb_type_tools_wait_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // Stop on reaching target time or if externally cancelled. + if (mp_hal_ticks_ms() - self->end_time < (uint32_t)INT32_MAX || self->has_ended) { + self->has_ended = true; + return MP_OBJ_STOP_ITERATION; + } + // Not done, so keep going. + return mp_const_none; +} + +// close() cancels the awaitable. +STATIC mp_obj_t pb_type_tools_wait_close(mp_obj_t self_in) { + pb_type_tools_wait_obj_t *self = MP_OBJ_TO_PTR(self_in); + self->has_ended = true; + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_tools_wait_close_obj, pb_type_tools_wait_close); + +STATIC const mp_rom_map_elem_t pb_type_tools_wait_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_tools_wait_close_obj) }, +}; +MP_DEFINE_CONST_DICT(pb_type_tools_wait_locals_dict, pb_type_tools_wait_locals_dict_table); + +// This is a partial implementation of the Python generator type. It is missing +// send(value) and throw(type[, value[, traceback]]) +MP_DEFINE_CONST_OBJ_TYPE(pb_type_tools_wait, + MP_QSTR_wait, + MP_TYPE_FLAG_ITER_IS_ITERNEXT, + iter, pb_type_tools_wait_iternext, + locals_dict, &pb_type_tools_wait_locals_dict); + +// Statically allocated awaitable from which all others can be found. +STATIC pb_type_tools_wait_obj_t first_awaitable; + +// Reset first awaitable on initializing MicroPython. +STATIC void pb_type_tools_wait_reset(void) { + first_awaitable.base.type = &pb_type_tools_wait; + first_awaitable.has_ended = true; + first_awaitable.next_awaitable = MP_OBJ_NULL; +} + +STATIC mp_obj_t pb_type_tools_wait_new(mp_int_t duration) { + + // When to stop waiting. + uint32_t end_time = mp_hal_ticks_ms() + (uint32_t)duration; + + // Find next available awaitable. + pb_type_tools_wait_obj_t *awaitable = &first_awaitable; + while (!awaitable->has_ended && awaitable->next_awaitable != MP_OBJ_NULL) { + awaitable = awaitable->next_awaitable; + } + // If the last known awaitable is still in use, allocate another. + if (!awaitable->has_ended) { + // Attach to the previous one. + awaitable->next_awaitable = m_new_obj(pb_type_tools_wait_obj_t); + + // Initialize the new awaitable. + awaitable = awaitable->next_awaitable; + awaitable->next_awaitable = MP_OBJ_NULL; + awaitable->base.type = &pb_type_tools_wait; + } + + // Initialize awaitable with the end time. + awaitable->has_ended = duration < 0 ? true: false; + awaitable->end_time = end_time; + + // Return the awaitable where the user can await it. + return MP_OBJ_FROM_PTR(awaitable); +} + STATIC mp_obj_t tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, PB_ARG_REQUIRED(time)); mp_int_t time = pb_obj_get_int(time_in); + + // In async mode, return awaitable. + if (pb_module_tools_run_loop_is_active()) { + return pb_type_tools_wait_new(time); + } + + // In blocking mode, wait until done. if (time > 0) { mp_hal_delay_ms(time); } @@ -36,6 +137,7 @@ bool pb_module_tools_run_loop_is_active() { STATIC mp_obj_t pb_module_tools___init__(void) { _pb_module_tools_run_loop_is_active = false; + pb_type_tools_wait_reset(); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_0(pb_module_tools___init___obj, pb_module_tools___init__); From e5ca65583c2f8ed0370b20d59a5922e38489785c Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 13 Jan 2023 12:05:01 +0100 Subject: [PATCH 10/59] pybricks.robotics.DriveBase: Support run loop. --- pybricks/robotics/pb_type_drivebase.c | 151 +++++++++++++++++++++++++- 1 file changed, 149 insertions(+), 2 deletions(-) diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index b20daac69..899df99bb 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -16,13 +16,42 @@ #include #include #include +#include #include #include #include +typedef struct _pb_type_DriveBase_obj_t pb_type_DriveBase_obj_t; + +typedef struct _pb_type_DriveBaseWait_obj_t pb_type_DriveBaseWait_obj_t; + +/** + * A generator-like type for waiting on a motor operation to complete. + */ +struct _pb_type_DriveBaseWait_obj_t { + mp_obj_base_t base; + /** + * Motor object whose move this generator is awaiting on. + */ + pb_type_DriveBase_obj_t *drivebase_obj; + /** + * Whether this generator object was cancelled. + */ + bool was_cancelled; + /** + * Whether this generator object is done and thus can be recycled. This is + * when motion is complete or cancellation has been handled. + */ + bool has_ended; + /** + * Linked list of awaitables. + */ + pb_type_DriveBaseWait_obj_t *next_awaitable; +}; + // pybricks.robotics.DriveBase class object -typedef struct _pb_type_DriveBase_obj_t { +struct _pb_type_DriveBase_obj_t { mp_obj_base_t base; pbio_drivebase_t *db; int32_t initial_distance; @@ -31,7 +60,95 @@ typedef struct _pb_type_DriveBase_obj_t { mp_obj_t heading_control; mp_obj_t distance_control; #endif -} pb_type_DriveBase_obj_t; + pb_type_DriveBaseWait_obj_t first_awaitable; +}; + +STATIC mp_obj_t pb_type_DriveBaseWait_iternext(mp_obj_t self_in) { + pb_type_DriveBaseWait_obj_t *self = MP_OBJ_TO_PTR(self_in); + + if (self->was_cancelled) { + // Gracefully handle cancellation: Allow generator to complete + // and clean up but don't raise any exceptions. + self->has_ended = true; + return MP_OBJ_STOP_ITERATION; + } + + // Handle I/O exceptions. + if (!pbio_drivebase_update_loop_is_running(self->drivebase_obj->db)) { + pb_assert(PBIO_ERROR_NO_DEV); + } + + // Check for completion + if (pbio_drivebase_is_done(self->drivebase_obj->db)) { + self->has_ended = true; + return MP_OBJ_STOP_ITERATION; + } + // Not done, so keep going. + return mp_const_none; +} + +// Cancel all generators belonging to this motor. +STATIC void pb_type_DriveBaseWait_cancel_all(pb_type_DriveBase_obj_t *drivebase_obj) { + pb_type_DriveBaseWait_obj_t *self = &drivebase_obj->first_awaitable; + do { + self->was_cancelled = true; + self = self->next_awaitable; + } while (self != MP_OBJ_NULL); +} + +// The close() method is used to cancel an operation before it completes. If +// the operation is already complete, it should do nothing. It can be called +// more than once. +STATIC mp_obj_t pb_type_DriveBaseWait_close(mp_obj_t self_in) { + pb_type_DriveBaseWait_obj_t *self = MP_OBJ_TO_PTR(self_in); + pb_type_DriveBaseWait_cancel_all(self->drivebase_obj); + pb_assert(pbio_drivebase_stop(self->drivebase_obj->db, PBIO_CONTROL_ON_COMPLETION_COAST)); + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBaseWait_close_obj, pb_type_DriveBaseWait_close); + +STATIC const mp_rom_map_elem_t pb_type_DriveBaseWait_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_DriveBaseWait_close_obj) }, +}; +MP_DEFINE_CONST_DICT(pb_type_DriveBaseWait_locals_dict, pb_type_DriveBaseWait_locals_dict_table); + +// This is a partial implementation of the Python generator type. It is missing +// send(value) and throw(type[, value[, traceback]]) +MP_DEFINE_CONST_OBJ_TYPE(pb_type_DriveBaseWait, + MP_QSTR_DriveBaseWait, + MP_TYPE_FLAG_ITER_IS_ITERNEXT, + iter, pb_type_DriveBaseWait_iternext, + locals_dict, &pb_type_DriveBaseWait_locals_dict); + +STATIC mp_obj_t pb_type_DriveBaseWait_new(pb_type_DriveBase_obj_t *drivebase_obj) { + + // Cancel everything for this motor. + pb_type_DriveBaseWait_cancel_all(drivebase_obj); + + // Find next available previously allocated awaitable. + pb_type_DriveBaseWait_obj_t *self = &drivebase_obj->first_awaitable; + while (!self->has_ended && self->next_awaitable != MP_OBJ_NULL) { + self = self->next_awaitable; + } + // No free awaitable available, so allocate one. + if (!self->has_ended) { + // Attach to the previous one. + self->next_awaitable = m_new_obj(pb_type_DriveBaseWait_obj_t); + + // Initialize the new awaitable. + self = self->next_awaitable; + self->next_awaitable = MP_OBJ_NULL; + self->base.type = &pb_type_DriveBaseWait; + } + + // Initialize what to await on. + self->drivebase_obj = drivebase_obj; + self->has_ended = false; + self->was_cancelled = false; + + // Return the awaitable where the user can await it. + return MP_OBJ_FROM_PTR(self); +} // pybricks.robotics.DriveBase.reset STATIC mp_obj_t pb_type_DriveBase_reset(mp_obj_t self_in) { @@ -80,6 +197,11 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a // Reset drivebase state pb_type_DriveBase_reset(MP_OBJ_FROM_PTR(self)); + // Initialize first awaitable singleton. + self->first_awaitable.base.type = &pb_type_DriveBaseWait; + self->first_awaitable.has_ended = true; + self->first_awaitable.next_awaitable = MP_OBJ_NULL; + return MP_OBJ_FROM_PTR(self); } @@ -105,6 +227,12 @@ STATIC mp_obj_t pb_type_DriveBase_straight(size_t n_args, const mp_obj_t *pos_ar pb_assert(pbio_drivebase_drive_straight(self->db, distance, then)); + // Handle async case, return a generator. + if (pb_module_tools_run_loop_is_active()) { + return pb_type_DriveBaseWait_new(self); + } + + // Otherwise, handle default blocking wait. if (mp_obj_is_true(wait_in)) { wait_for_completion_drivebase(self->db); } @@ -127,6 +255,12 @@ STATIC mp_obj_t pb_type_DriveBase_turn(size_t n_args, const mp_obj_t *pos_args, // Turning in place is done as a curve with zero radius and a given angle. pb_assert(pbio_drivebase_drive_curve(self->db, 0, angle, then)); + // Handle async case, return a generator. + if (pb_module_tools_run_loop_is_active()) { + return pb_type_DriveBaseWait_new(self); + } + + // Otherwise, handle default blocking wait. if (mp_obj_is_true(wait_in)) { wait_for_completion_drivebase(self->db); } @@ -150,6 +284,12 @@ STATIC mp_obj_t pb_type_DriveBase_curve(size_t n_args, const mp_obj_t *pos_args, pb_assert(pbio_drivebase_drive_curve(self->db, radius, angle, then)); + // Handle async case, return a generator. + if (pb_module_tools_run_loop_is_active()) { + return pb_type_DriveBaseWait_new(self); + } + + // Otherwise, handle default blocking wait. if (mp_obj_is_true(wait_in)) { wait_for_completion_drivebase(self->db); } @@ -171,6 +311,9 @@ STATIC mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, pb_assert(pbio_drivebase_drive_forever(self->db, speed, turn_rate)); + // Cancel user-level motor wait operations in parallel tasks. + pb_type_DriveBaseWait_cancel_all(self); + return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_drive_obj, 1, pb_type_DriveBase_drive); @@ -179,6 +322,10 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_drive_obj, 1, pb_type_DriveB STATIC mp_obj_t pb_type_DriveBase_stop(mp_obj_t self_in) { pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_drivebase_stop(self->db, PBIO_CONTROL_ON_COMPLETION_COAST)); + + // Cancel user-level motor wait operations in parallel tasks. + pb_type_DriveBaseWait_cancel_all(self); + return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_stop_obj, pb_type_DriveBase_stop); From db707ed380c89383814acd8f7f739c1f914b48af Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 13 Jan 2023 12:23:10 +0100 Subject: [PATCH 11/59] pybricks.tools: Make wait reusable. This can be used for any time-like awaitable. --- bricks/_common/sources.mk | 1 + pybricks/tools.h | 4 ++ pybricks/tools/pb_module_tools.c | 93 -------------------------- pybricks/tools/pb_type_wait.c | 109 +++++++++++++++++++++++++++++++ 4 files changed, 114 insertions(+), 93 deletions(-) create mode 100644 pybricks/tools/pb_type_wait.c diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index b93fadbc1..ec7dc5fd7 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -99,6 +99,7 @@ PYBRICKS_PYBRICKS_SRC_C = $(addprefix pybricks/,\ tools/pb_module_tools.c \ tools/pb_type_matrix.c \ tools/pb_type_stopwatch.c \ + tools/pb_type_wait.c \ util_mp/pb_obj_helper.c \ util_mp/pb_type_enum.c \ util_pb/pb_color_map.c \ diff --git a/pybricks/tools.h b/pybricks/tools.h index ea3a961aa..54adc193a 100644 --- a/pybricks/tools.h +++ b/pybricks/tools.h @@ -12,6 +12,10 @@ bool pb_module_tools_run_loop_is_active(); +void pb_type_tools_wait_reset(void); + +mp_obj_t pb_type_tools_wait_new(mp_int_t duration); + extern const mp_obj_type_t pb_type_StopWatch; #endif // PYBRICKS_PY_TOOLS diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index af00877b8..6970e3a29 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -17,99 +17,6 @@ #include #include -/** - * A generator-like type for waiting on a motor operation to complete. - */ -typedef struct _pb_type_tools_wait_obj_t pb_type_tools_wait_obj_t; - -struct _pb_type_tools_wait_obj_t { - mp_obj_base_t base; - /** - * When to stop waiting. - */ - uint32_t end_time; - /** - * Whether this generator object is done and thus can be recycled. - */ - bool has_ended; - /** - * Linked list of awaitables. - */ - pb_type_tools_wait_obj_t *next_awaitable; -}; - -STATIC mp_obj_t pb_type_tools_wait_iternext(mp_obj_t self_in) { - pb_type_tools_wait_obj_t *self = MP_OBJ_TO_PTR(self_in); - - // Stop on reaching target time or if externally cancelled. - if (mp_hal_ticks_ms() - self->end_time < (uint32_t)INT32_MAX || self->has_ended) { - self->has_ended = true; - return MP_OBJ_STOP_ITERATION; - } - // Not done, so keep going. - return mp_const_none; -} - -// close() cancels the awaitable. -STATIC mp_obj_t pb_type_tools_wait_close(mp_obj_t self_in) { - pb_type_tools_wait_obj_t *self = MP_OBJ_TO_PTR(self_in); - self->has_ended = true; - return mp_const_none; -} -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_tools_wait_close_obj, pb_type_tools_wait_close); - -STATIC const mp_rom_map_elem_t pb_type_tools_wait_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_tools_wait_close_obj) }, -}; -MP_DEFINE_CONST_DICT(pb_type_tools_wait_locals_dict, pb_type_tools_wait_locals_dict_table); - -// This is a partial implementation of the Python generator type. It is missing -// send(value) and throw(type[, value[, traceback]]) -MP_DEFINE_CONST_OBJ_TYPE(pb_type_tools_wait, - MP_QSTR_wait, - MP_TYPE_FLAG_ITER_IS_ITERNEXT, - iter, pb_type_tools_wait_iternext, - locals_dict, &pb_type_tools_wait_locals_dict); - -// Statically allocated awaitable from which all others can be found. -STATIC pb_type_tools_wait_obj_t first_awaitable; - -// Reset first awaitable on initializing MicroPython. -STATIC void pb_type_tools_wait_reset(void) { - first_awaitable.base.type = &pb_type_tools_wait; - first_awaitable.has_ended = true; - first_awaitable.next_awaitable = MP_OBJ_NULL; -} - -STATIC mp_obj_t pb_type_tools_wait_new(mp_int_t duration) { - - // When to stop waiting. - uint32_t end_time = mp_hal_ticks_ms() + (uint32_t)duration; - - // Find next available awaitable. - pb_type_tools_wait_obj_t *awaitable = &first_awaitable; - while (!awaitable->has_ended && awaitable->next_awaitable != MP_OBJ_NULL) { - awaitable = awaitable->next_awaitable; - } - // If the last known awaitable is still in use, allocate another. - if (!awaitable->has_ended) { - // Attach to the previous one. - awaitable->next_awaitable = m_new_obj(pb_type_tools_wait_obj_t); - - // Initialize the new awaitable. - awaitable = awaitable->next_awaitable; - awaitable->next_awaitable = MP_OBJ_NULL; - awaitable->base.type = &pb_type_tools_wait; - } - - // Initialize awaitable with the end time. - awaitable->has_ended = duration < 0 ? true: false; - awaitable->end_time = end_time; - - // Return the awaitable where the user can await it. - return MP_OBJ_FROM_PTR(awaitable); -} - STATIC mp_obj_t tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, PB_ARG_REQUIRED(time)); diff --git a/pybricks/tools/pb_type_wait.c b/pybricks/tools/pb_type_wait.c new file mode 100644 index 000000000..632e3806c --- /dev/null +++ b/pybricks/tools/pb_type_wait.c @@ -0,0 +1,109 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2023 The Pybricks Authors + +#include "py/mpconfig.h" + +#if PYBRICKS_PY_TOOLS + +#include "py/mphal.h" + +#include + +#include +#include +#include + +/** + * A generator-like type for waiting on a motor operation to complete. + */ +typedef struct _pb_type_tools_wait_obj_t pb_type_tools_wait_obj_t; + +struct _pb_type_tools_wait_obj_t { + mp_obj_base_t base; + /** + * When to stop waiting. + */ + uint32_t end_time; + /** + * Whether this generator object is done and thus can be recycled. + */ + bool has_ended; + /** + * Linked list of awaitables. + */ + pb_type_tools_wait_obj_t *next_awaitable; +}; + +STATIC mp_obj_t pb_type_tools_wait_iternext(mp_obj_t self_in) { + pb_type_tools_wait_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // Stop on reaching target time or if externally cancelled. + if (mp_hal_ticks_ms() - self->end_time < (uint32_t)INT32_MAX || self->has_ended) { + self->has_ended = true; + return MP_OBJ_STOP_ITERATION; + } + // Not done, so keep going. + return mp_const_none; +} + +// close() cancels the awaitable. +STATIC mp_obj_t pb_type_tools_wait_close(mp_obj_t self_in) { + pb_type_tools_wait_obj_t *self = MP_OBJ_TO_PTR(self_in); + self->has_ended = true; + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_tools_wait_close_obj, pb_type_tools_wait_close); + +STATIC const mp_rom_map_elem_t pb_type_tools_wait_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_tools_wait_close_obj) }, +}; +MP_DEFINE_CONST_DICT(pb_type_tools_wait_locals_dict, pb_type_tools_wait_locals_dict_table); + +// This is a partial implementation of the Python generator type. It is missing +// send(value) and throw(type[, value[, traceback]]) +MP_DEFINE_CONST_OBJ_TYPE(pb_type_tools_wait, + MP_QSTR_wait, + MP_TYPE_FLAG_ITER_IS_ITERNEXT, + iter, pb_type_tools_wait_iternext, + locals_dict, &pb_type_tools_wait_locals_dict); + +// Statically allocated awaitable from which all others can be found. +STATIC pb_type_tools_wait_obj_t first_awaitable; + +// Reset first awaitable on initializing MicroPython. +void pb_type_tools_wait_reset(void) { + first_awaitable.base.type = &pb_type_tools_wait; + first_awaitable.has_ended = true; + first_awaitable.next_awaitable = MP_OBJ_NULL; +} + +mp_obj_t pb_type_tools_wait_new(mp_int_t duration) { + + // When to stop waiting. + uint32_t end_time = mp_hal_ticks_ms() + (uint32_t)duration; + + // Find next available awaitable. + pb_type_tools_wait_obj_t *awaitable = &first_awaitable; + while (!awaitable->has_ended && awaitable->next_awaitable != MP_OBJ_NULL) { + awaitable = awaitable->next_awaitable; + } + // If the last known awaitable is still in use, allocate another. + if (!awaitable->has_ended) { + // Attach to the previous one. + awaitable->next_awaitable = m_new_obj(pb_type_tools_wait_obj_t); + + // Initialize the new awaitable. + awaitable = awaitable->next_awaitable; + awaitable->next_awaitable = MP_OBJ_NULL; + awaitable->base.type = &pb_type_tools_wait; + } + + // Initialize awaitable with the end time. + awaitable->has_ended = duration < 0 ? true: false; + awaitable->end_time = end_time; + + // Return the awaitable where the user can await it. + return MP_OBJ_FROM_PTR(awaitable); +} + +#endif // PYBRICKS_PY_TOOLS From cd73dd502a307f41035a9a566f1c86ead7090fc8 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 13 Jan 2023 12:37:34 +0100 Subject: [PATCH 12/59] pybricks.tools: Generalize await with callback. --- pybricks/tools.h | 2 +- pybricks/tools/pb_module_tools.c | 4 ++-- pybricks/tools/pb_type_wait.c | 15 +++++++++++++-- 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/pybricks/tools.h b/pybricks/tools.h index 54adc193a..d3e44225b 100644 --- a/pybricks/tools.h +++ b/pybricks/tools.h @@ -14,7 +14,7 @@ bool pb_module_tools_run_loop_is_active(); void pb_type_tools_wait_reset(void); -mp_obj_t pb_type_tools_wait_new(mp_int_t duration); +mp_obj_t pb_type_tools_wait_new(mp_int_t duration, void (*callback)(void)); extern const mp_obj_type_t pb_type_StopWatch; diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 6970e3a29..db8ba9b91 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -23,9 +23,9 @@ STATIC mp_obj_t tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw mp_int_t time = pb_obj_get_int(time_in); - // In async mode, return awaitable. + // Within run loop, return awaitable. if (pb_module_tools_run_loop_is_active()) { - return pb_type_tools_wait_new(time); + return pb_type_tools_wait_new(time, NULL); } // In blocking mode, wait until done. diff --git a/pybricks/tools/pb_type_wait.c b/pybricks/tools/pb_type_wait.c index 632e3806c..2f45252ae 100644 --- a/pybricks/tools/pb_type_wait.c +++ b/pybricks/tools/pb_type_wait.c @@ -28,6 +28,10 @@ struct _pb_type_tools_wait_obj_t { * Whether this generator object is done and thus can be recycled. */ bool has_ended; + /** + * Callback to call on completion or cancellation. + */ + void (*callback)(void); /** * Linked list of awaitables. */ @@ -40,6 +44,9 @@ STATIC mp_obj_t pb_type_tools_wait_iternext(mp_obj_t self_in) { // Stop on reaching target time or if externally cancelled. if (mp_hal_ticks_ms() - self->end_time < (uint32_t)INT32_MAX || self->has_ended) { self->has_ended = true; + if (self->callback) { + self->callback(); + } return MP_OBJ_STOP_ITERATION; } // Not done, so keep going. @@ -50,6 +57,9 @@ STATIC mp_obj_t pb_type_tools_wait_iternext(mp_obj_t self_in) { STATIC mp_obj_t pb_type_tools_wait_close(mp_obj_t self_in) { pb_type_tools_wait_obj_t *self = MP_OBJ_TO_PTR(self_in); self->has_ended = true; + if (self->callback) { + self->callback(); + } return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_tools_wait_close_obj, pb_type_tools_wait_close); @@ -77,7 +87,7 @@ void pb_type_tools_wait_reset(void) { first_awaitable.next_awaitable = MP_OBJ_NULL; } -mp_obj_t pb_type_tools_wait_new(mp_int_t duration) { +mp_obj_t pb_type_tools_wait_new(mp_int_t duration, void (*callback)(void)) { // When to stop waiting. uint32_t end_time = mp_hal_ticks_ms() + (uint32_t)duration; @@ -98,7 +108,8 @@ mp_obj_t pb_type_tools_wait_new(mp_int_t duration) { awaitable->base.type = &pb_type_tools_wait; } - // Initialize awaitable with the end time. + // Initialize awaitable with the end time and callback. + awaitable->callback = callback; awaitable->has_ended = duration < 0 ? true: false; awaitable->end_time = end_time; From aa04f536e4ab929f9700b5d7c24d74ccafe2bf5d Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 13 Jan 2023 12:37:53 +0100 Subject: [PATCH 13/59] pybricks.common.Speaker: Make beep awaitable. --- pybricks/common/pb_type_speaker.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index bd358c1b1..56b5e7989 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -19,6 +19,7 @@ #include "py/obj.h" #include +#include #include #include #include @@ -127,6 +128,12 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp pb_type_Speaker_start_beep(frequency, self->sample_attenuator); + // Within run loop, return awaitable. + if (pb_module_tools_run_loop_is_active()) { + return pb_type_tools_wait_new(duration, pb_type_Speaker_stop_beep); + } + + // In blocking mode, wait until done. if (duration < 0) { return mp_const_none; } From 78b9561b0414f7dcfdddae033742cce3a0fad159 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Sat, 14 Jan 2023 14:20:38 +0100 Subject: [PATCH 14/59] bricks/_common/modules: Add multi-tasking module. --- bricks/_common/modules/.gitignore | 1 + bricks/_common/modules/_task.py | 25 ++++++++++++++++++++++++ lib/pbio/platform/move_hub/pbdrvconfig.h | 2 +- lib/pbio/platform/move_hub/platform.ld | 4 ++-- 4 files changed, 29 insertions(+), 3 deletions(-) create mode 100644 bricks/_common/modules/_task.py diff --git a/bricks/_common/modules/.gitignore b/bricks/_common/modules/.gitignore index f104652b6..1b1b252f4 100644 --- a/bricks/_common/modules/.gitignore +++ b/bricks/_common/modules/.gitignore @@ -1 +1,2 @@ *.py +!_task.py diff --git a/bricks/_common/modules/_task.py b/bricks/_common/modules/_task.py new file mode 100644 index 000000000..02380445c --- /dev/null +++ b/bricks/_common/modules/_task.py @@ -0,0 +1,25 @@ +from pybricks.tools import _set_run_loop_active + + +def all(*tasks): + completed = {t: False for t in tasks} + all_done = False + while not all_done: + all_done = True + for t in tasks: + if not completed[t]: + all_done = False + try: + next(t) + except StopIteration: + completed[t] = True + yield + + +def run(main_task): + _set_run_loop_active(True) + try: + for _ in main_task: + pass + finally: + _set_run_loop_active(False) diff --git a/lib/pbio/platform/move_hub/pbdrvconfig.h b/lib/pbio/platform/move_hub/pbdrvconfig.h index af1db6756..04b7f23a4 100644 --- a/lib/pbio/platform/move_hub/pbdrvconfig.h +++ b/lib/pbio/platform/move_hub/pbdrvconfig.h @@ -20,7 +20,7 @@ #define PBDRV_CONFIG_BLOCK_DEVICE (1) #define PBDRV_CONFIG_BLOCK_DEVICE_FLASH_STM32 (1) -#define PBDRV_CONFIG_BLOCK_DEVICE_FLASH_STM32_SIZE (4 * 1024) // Must match FLASH_USER_0 + FLASH_USER_1 in linker script +#define PBDRV_CONFIG_BLOCK_DEVICE_FLASH_STM32_SIZE (3 * 1024) // Must match FLASH_USER_0 + FLASH_USER_1 in linker script #define PBDRV_CONFIG_BLUETOOTH (1) #define PBDRV_CONFIG_BLUETOOTH_STM32_BLUENRG (1) diff --git a/lib/pbio/platform/move_hub/platform.ld b/lib/pbio/platform/move_hub/platform.ld index f167bd2f6..a9f32e775 100644 --- a/lib/pbio/platform/move_hub/platform.ld +++ b/lib/pbio/platform/move_hub/platform.ld @@ -14,9 +14,9 @@ MEMORY in FLASH_FIRMWARE and FLASH_USER_0 add up to 0*/ FLASH_BOOTLOADER (rx) : ORIGIN = 0x08000000, LENGTH = 20K /* The firmware. Installed via BLE bootloader. */ - FLASH_FIRMWARE (rx) : ORIGIN = 0x08005000, LENGTH = 104K + FLASH_FIRMWARE (rx) : ORIGIN = 0x08005000, LENGTH = 105K /* User data written by the firmware during shutdown. */ - FLASH_USER_0 (rx) : ORIGIN = 0x0801F000, LENGTH = 2K + FLASH_USER_0 (rx) : ORIGIN = 0x0801F400, LENGTH = 1K /* As above, but this part is not counted by bootloader checksum. */ FLASH_USER_1 (rx) : ORIGIN = 0x0801F800, LENGTH = 2K RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 16K From 31145b4b511f712bde0f72c82f63d683ee739eda Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 9 Mar 2023 09:20:57 +0100 Subject: [PATCH 15/59] pybricks.tools: Import task from tools. --- bricks/_common/mpconfigport.h | 1 + pybricks/tools/pb_module_tools.c | 16 ++++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/bricks/_common/mpconfigport.h b/bricks/_common/mpconfigport.h index db350b017..668f9fd0f 100644 --- a/bricks/_common/mpconfigport.h +++ b/bricks/_common/mpconfigport.h @@ -110,6 +110,7 @@ #define MICROPY_ENABLE_SCHEDULER (0) #define MICROPY_PY_INSTANCE_ATTRS (1) +#define MICROPY_MODULE_ATTR_DELEGATION (1) #define MICROPY_PERSISTENT_CODE_LOAD (1) #define MICROPY_ENABLE_EXTERNAL_IMPORT (0) #define MICROPY_HAS_FILE_READER (0) diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index db8ba9b91..20925b972 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -5,7 +5,9 @@ #if PYBRICKS_PY_TOOLS +#include "py/builtin.h" #include "py/mphal.h" +#include "py/objmodule.h" #include "py/runtime.h" #include @@ -55,6 +57,17 @@ STATIC mp_obj_t pb_module_tools_set_run_loop_active(mp_obj_t self_in) { } MP_DEFINE_CONST_FUN_OBJ_1(pb_module_tools_set_run_loop_active_obj, pb_module_tools_set_run_loop_active); +#if MICROPY_MODULE_ATTR_DELEGATION +// pybricks.tools.task is implemented as pure Python code in the frozen _task +// module. This handler makes it available through the pybricks package. +STATIC void pb_module_tools_attr(mp_obj_t self_in, qstr attr, mp_obj_t *dest) { + if (attr == MP_QSTR_task) { + const mp_obj_t args[] = { MP_OBJ_NEW_QSTR(MP_QSTR__task) }; + dest[0] = mp_builtin___import__(MP_ARRAY_SIZE(args), args); + } +} +#endif + STATIC const mp_rom_map_elem_t tools_globals_table[] = { { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_tools) }, { MP_ROM_QSTR(MP_QSTR___init__), MP_ROM_PTR(&pb_module_tools___init___obj)}, @@ -68,6 +81,9 @@ STATIC const mp_rom_map_elem_t tools_globals_table[] = { // backwards compatibility for pybricks.geometry.Axis { MP_ROM_QSTR(MP_QSTR_Axis), MP_ROM_PTR(&pb_enum_type_Axis) }, #endif // MICROPY_PY_BUILTINS_FLOAT + #if MICROPY_MODULE_ATTR_DELEGATION + MP_MODULE_ATTR_DELEGATION_ENTRY(&pb_module_tools_attr), + #endif }; STATIC MP_DEFINE_CONST_DICT(pb_module_tools_globals, tools_globals_table); From b93ca4b77d8b12de8be937db3c8bc7cb9d1b18a4 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 9 Mar 2023 10:38:37 +0100 Subject: [PATCH 16/59] bricks/virtualhub: Add async demo and test. --- tests/virtualhub/multitasking/motor.py | 46 ++++++++++++++++++++++ tests/virtualhub/multitasking/motor.py.exp | 7 ++++ 2 files changed, 53 insertions(+) create mode 100644 tests/virtualhub/multitasking/motor.py create mode 100644 tests/virtualhub/multitasking/motor.py.exp diff --git a/tests/virtualhub/multitasking/motor.py b/tests/virtualhub/multitasking/motor.py new file mode 100644 index 000000000..6568382e2 --- /dev/null +++ b/tests/virtualhub/multitasking/motor.py @@ -0,0 +1,46 @@ +from pybricks.pupdevices import Motor +from pybricks.parameters import Port, Direction +from pybricks.robotics import DriveBase +from pybricks.tools import wait, task + +# Initialize devices as usual. +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B) +drive_base = DriveBase(left_motor, right_motor, wheel_diameter=56, axle_track=112) +stall_motor = Motor(Port.C) + + +# Task to drive in a square. +async def square(): + for side in range(4): + # Drive forward and then turn. + await drive_base.straight(200) + await drive_base.turn(90) + + +# Task to center a motor. +async def center(): + left = await stall_motor.run_until_stalled(-200) + right = await stall_motor.run_until_stalled(200) + stall_motor.reset_angle((right - left) / 2) + await stall_motor.run_target(500, 0) + print("Motor centered!") + + +# Hello world task. +async def hello(name): + print("Hello!") + await wait(2000) + print(name) + + +# This is the main program +async def main(): + print("Running multiple tasks at once!") + await task.all(square(), center(), hello("Pybricks")) + print("You can also just run one task.") + await hello("World!") + + +# Run the main program. +task.run(main()) diff --git a/tests/virtualhub/multitasking/motor.py.exp b/tests/virtualhub/multitasking/motor.py.exp new file mode 100644 index 000000000..9a9c76230 --- /dev/null +++ b/tests/virtualhub/multitasking/motor.py.exp @@ -0,0 +1,7 @@ +Running multiple tasks at once! +Hello! +Pybricks +Motor centered! +You can also just run one task. +Hello! +World! From 63756619d98c553945a86778e05dd40a887bbf1b Mon Sep 17 00:00:00 2001 From: David Lechner Date: Thu, 16 Mar 2023 17:32:21 -0500 Subject: [PATCH 17/59] pybricks.tools.task: implement all() and race() This implements task.all() and task.race() in the style of redux sagas effects. This is similar to, but much more simple than, asyncio.gather() and asyncio.wait(return_when=FIRST_COMPLETED). If any task raises an exception, all other tasks are canceled. The return value of task.all() works like asyncio.gather(). The return value of task.race() is similar except all canceled tasks will have a value of None. The run loop is modified to call the garbage collector after each loop. This helps keep memory from fragmenting and reduces time needed to allocate new memory in many cases. The run loop also has a timer added to call it at a specific interval rather than as fast as possible. --- bricks/_common/modules/_task.py | 145 +++++++++++++++-- tests/virtualhub/multitasking/basic.py | 174 +++++++++++++++++++++ tests/virtualhub/multitasking/basic.py.exp | 20 +++ 3 files changed, 324 insertions(+), 15 deletions(-) create mode 100644 tests/virtualhub/multitasking/basic.py create mode 100644 tests/virtualhub/multitasking/basic.py.exp diff --git a/bricks/_common/modules/_task.py b/bricks/_common/modules/_task.py index 02380445c..b70f8ac33 100644 --- a/bricks/_common/modules/_task.py +++ b/bricks/_common/modules/_task.py @@ -1,25 +1,140 @@ -from pybricks.tools import _set_run_loop_active +def all(*tasks): + """ + Runs each task in parallel and waits for all to complete. + Args: + tasks: one or more tasks/generator functions to run in parallel. -def all(*tasks): - completed = {t: False for t in tasks} - all_done = False - while not all_done: - all_done = True - for t in tasks: - if not completed[t]: - all_done = False + Returns: + A list containing the return value of each task in the same order + as *tasks*. + + Example:: + + # generator syntax + def example1(): + result1, result2 = yield from tasks.all(task1(), task2()) + ... + + # async/await syntax + async def example2(): + result1, result2 = await tasks.all(task1(), task2()) + ... + + # return value can be ignored + async def example3(): + await tasks.all(task1(), task2()) + ... + """ + r = [None] * len(tasks) + pending = {i: t for i, t in enumerate(tasks)} + done = [] + + try: + while pending: + for i, t in pending.items(): try: next(t) - except StopIteration: - completed[t] = True - yield + except StopIteration as ex: + r[i] = ex.value + done.append(i) + + # can't modify pending while we are iterating it so defer to here + for i in done: + del pending[i] + + done.clear() + + yield + finally: + # In case of crash in one task, all other unfinished tasks are + # canceled. Calling close() on finished tasks is a nop. + for t in tasks: + t.close() + + return r + + +def race(*tasks): + """ + Runs each task in parallel and waits for the first one to complete. After + the first task completes, all other tasks will be cleaned up by calling the + generator ``close()`` function. + + Important: if you want to know which task won the race, all tasks must + return a value that is not ``None``. + + Args: + tasks: One or more tasks/generator functions to run in parallel. + + Returns: + A list of values corresponding to each task in the same order as + *tasks*. All values will be ``None`` except for the winner of the + race. + """ + r = [None] * len(tasks) + try: + while True: + for i, t in enumerate(tasks): + next(t) + yield + except StopIteration as ex: + # winner of the race - save the return value + r[i] = ex.value + finally: + for t in tasks: + t.close() + + return r + + +def run(task, loop_time=10): + """ + Runs a task in the main event loop. + + This will run one iteration of the task every *loop_time*. + + Args: + loop_time: The target loop time in milliseconds. + + Basic usage:: + + async def main(): + await ... + ... + + tasks.run(main()) + + To run more than one task, use :meth:`tasks.all`:: + + tasks.run(tasks.all(task_1(), task_2())) + """ + from gc import collect + from pybricks.tools import _set_run_loop_active, StopWatch, wait -def run(main_task): _set_run_loop_active(True) + try: - for _ in main_task: - pass + timer = StopWatch() + start = timer.time() + + # TODO: keep track of loop time stats and provide API to user + + for _ in task: + # needed here so that wait() is blocking + _set_run_loop_active(False) + + # garbage collect to keep GC high watermark to a minimum + collect() + + # GC can take many milliseconds when lots of RAM is used so we need + # to only wait the time remaining after user code and GC. + val = max(0, loop_time - (timer.time() - start)) + wait(val) + + start += loop_time + + _set_run_loop_active(True) finally: _set_run_loop_active(False) diff --git a/tests/virtualhub/multitasking/basic.py b/tests/virtualhub/multitasking/basic.py new file mode 100644 index 000000000..629b86fa0 --- /dev/null +++ b/tests/virtualhub/multitasking/basic.py @@ -0,0 +1,174 @@ +from pybricks.tools import task + + +class CallCounter: + def __init__(self): + self.count = 0 + + def __call__(self): + self.count += 1 + + +def count(): + """ + Count forever. + """ + n = 0 + while True: + yield n + n += 1 + + +def never_return(on_cancel): + try: + for i in count(): + yield i + except GeneratorExit: + on_cancel() + + +def return_after(n, on_cancel): + try: + for i in range(n): + yield i + return i + except GeneratorExit: + on_cancel() + + +def throw_after(n, msg, on_cancel): + try: + for i in range(n): + yield i + raise RuntimeError(msg) + except GeneratorExit: + on_cancel() + + +async def test_all_1(): + print("test_all_1") + + task1_cancel = CallCounter() + task2_cancel = CallCounter() + + task1 = return_after(1, task1_cancel) + task2 = return_after(2, task2_cancel) + + # should return [0, 1] - both allowed to complete + print(await task.all(task1, task2)) + + # neither should be canceled + print(task1_cancel.count, task2_cancel.count) + + # both should raise StopIteration because they are done + + try: + next(task1) + except StopIteration as e: + print(type(e), e.args) + + try: + next(task2) + except StopIteration as e: + print(type(e), e.args) + + +async def test_all_2(): + print("test_all_2") + + task1_cancel = CallCounter() + task2_cancel = CallCounter() + + task1 = return_after(2, task1_cancel) + task2 = throw_after(1, "task2", task2_cancel) + + # should throw RuntimeError("task2") + try: + await task.all(task1, task2) + except Exception as e: + print(type(e), e.args) + + # task1 should be canceled + print(task1_cancel.count, task2_cancel.count) + + # both should raise StopIteration because they are done + + try: + next(task1) + except StopIteration as e: + print(type(e), e.args) + + try: + next(task2) + except StopIteration as e: + print(type(e), e.args) + + +async def test_race_1(): + print("test_race_1") + + task1_cancel = CallCounter() + task2_cancel = CallCounter() + + task1 = return_after(1, task1_cancel) + task2 = return_after(2, task2_cancel) + + # returns [0, None] - only first completes + print(await task.race(task1, task2)) + + # task2 should be canceled + print(task1_cancel.count, task2_cancel.count) + + # both should raise StopIteration because they are done/canceled + + try: + next(task1) + except StopIteration as e: + print(type(e), e.args) + + try: + next(task2) + except StopIteration as e: + print(type(e), e.args) + + +async def test_race_2(): + print("test_race_2") + + task1_cancel = CallCounter() + task2_cancel = CallCounter() + + task1 = return_after(2, task1_cancel) + task2 = throw_after(1, "task2", task2_cancel) + + # should throw RuntimeError("task2") + try: + await task.race(task1, task2) + except Exception as e: + print(type(e), e.args) + + # task1 should be canceled + print(task1_cancel.count, task2_cancel.count) + + # both should raise StopIteration because they are done + + try: + next(task1) + except StopIteration as e: + print(type(e), e.args) + + try: + next(task2) + except StopIteration as e: + print(type(e), e.args) + + +async def main(): + await test_all_1() + await test_all_2() + await test_race_1() + await test_race_2() + + +# run as fast as possible for CI +task.run(main(), loop_time=0) diff --git a/tests/virtualhub/multitasking/basic.py.exp b/tests/virtualhub/multitasking/basic.py.exp new file mode 100644 index 000000000..aa865bb4c --- /dev/null +++ b/tests/virtualhub/multitasking/basic.py.exp @@ -0,0 +1,20 @@ +test_all_1 +[0, 1] +0 0 + () + () +test_all_2 + ('task2',) +1 0 + () + () +test_race_1 +[0, None] +0 1 + () + () +test_race_2 + ('task2',) +1 0 + () + () From 497312fbee573f9cfb0ce7150a449543437cda49 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 10:53:47 +0200 Subject: [PATCH 18/59] pybricks.tools: Generalize awaitable. This can be used for all awaitables to simplify the code and reduce code size. --- bricks/_common/sources.mk | 1 + pybricks/tools.h | 23 ++++ pybricks/tools/pb_type_awaitable.c | 162 +++++++++++++++++++++++++++++ 3 files changed, 186 insertions(+) create mode 100644 pybricks/tools/pb_type_awaitable.c diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index ec7dc5fd7..89b956505 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -97,6 +97,7 @@ PYBRICKS_PYBRICKS_SRC_C = $(addprefix pybricks/,\ robotics/pb_type_drivebase.c \ robotics/pb_type_spikebase.c \ tools/pb_module_tools.c \ + tools/pb_type_awaitable.c \ tools/pb_type_matrix.c \ tools/pb_type_stopwatch.c \ tools/pb_type_wait.c \ diff --git a/pybricks/tools.h b/pybricks/tools.h index d3e44225b..00c42093b 100644 --- a/pybricks/tools.h +++ b/pybricks/tools.h @@ -10,12 +10,35 @@ #include "py/obj.h" +/** + * Tests if awaitable operation is complete. Returns MP_OBJ_STOP_ITERATION if + * done, possibly with return argument, else mp_const_none. + * + * On completion, this function is expected to close/stop hardware + * operations as needed (hold a motor, etc.). This is not the same as cancel + * below, which always stops the relevant hardware (i.e. always coast). + */ +typedef mp_obj_t (*pb_awaitable_test_completion_t)(mp_obj_t obj); + +/** + * Called on cancel/close. Used to stop hardware operation in unhandled + * conditions. + */ +typedef void (*pb_awaitable_cancel_t)(mp_obj_t obj); + + bool pb_module_tools_run_loop_is_active(); void pb_type_tools_wait_reset(void); mp_obj_t pb_type_tools_wait_new(mp_int_t duration, void (*callback)(void)); +void pb_type_tools_awaitable_init(void); + +mp_obj_t pb_type_tools_await_or_wait(mp_obj_t obj, pb_awaitable_test_completion_t test_completion, pb_awaitable_cancel_t cancel); + +mp_obj_t pb_type_tools_await_time(mp_obj_t duration_in); + extern const mp_obj_type_t pb_type_StopWatch; #endif // PYBRICKS_PY_TOOLS diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c new file mode 100644 index 000000000..2d40ff9e5 --- /dev/null +++ b/pybricks/tools/pb_type_awaitable.c @@ -0,0 +1,162 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2023 The Pybricks Authors + +#include "py/mpconfig.h" + +#if PYBRICKS_PY_TOOLS + +#include "py/mphal.h" +#include "py/mpstate.h" + +#include + +/** + * A generator-like type for waiting on some operation to complete. + */ +typedef struct _pb_type_tools_awaitable_obj_t pb_type_tools_awaitable_obj_t; + +struct _pb_type_tools_awaitable_obj_t { + mp_obj_base_t base; + /** + * Object associated with this awaitable, such as the motor we wait on. + */ + mp_obj_t obj; + /** + * End time. Only used for simple waits with no associated object. + */ + uint32_t end_time; + /** + * Tests if operation is complete. + */ + pb_awaitable_test_completion_t test_completion; + /** + * Called on cancellation. + */ + pb_awaitable_cancel_t cancel; + /** + * Linked list of awaitables. + */ + pb_type_tools_awaitable_obj_t *next_awaitable; +}; + +// close() cancels the awaitable. +STATIC mp_obj_t pb_type_tools_awaitable_close(mp_obj_t self_in) { + pb_type_tools_awaitable_obj_t *self = MP_OBJ_TO_PTR(self_in); + self->test_completion = NULL; + // Handle optional clean up/cancelling of hardware operation. + if (self->cancel) { + self->cancel(self->obj); + } + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_tools_awaitable_close_obj, pb_type_tools_awaitable_close); + +STATIC mp_obj_t pb_type_tools_awaitable_iternext(mp_obj_t self_in) { + pb_type_tools_awaitable_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // If completed callback was unset, then we are done. + if (self->test_completion == NULL) { + return MP_OBJ_STOP_ITERATION; + } + + // Test completion status. + mp_obj_t completion = self->test_completion(self->obj); + + // none means keep going, everything else means done so finalize. + if (completion != mp_const_none) { + self->test_completion = NULL; + } + return completion; +} + +STATIC const mp_rom_map_elem_t pb_type_tools_awaitable_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_tools_awaitable_close_obj) }, +}; +MP_DEFINE_CONST_DICT(pb_type_tools_awaitable_locals_dict, pb_type_tools_awaitable_locals_dict_table); + +// This is a partial implementation of the Python generator type. It is missing +// send(value) and throw(type[, value[, traceback]]) +MP_DEFINE_CONST_OBJ_TYPE(pb_type_tools_awaitable, + MP_QSTR_wait, + MP_TYPE_FLAG_ITER_IS_ITERNEXT, + iter, pb_type_tools_awaitable_iternext, + locals_dict, &pb_type_tools_awaitable_locals_dict); + +// Statically allocated awaitable from which all others can be found. +STATIC pb_type_tools_awaitable_obj_t first_awaitable; + +// Reset first awaitable on initializing MicroPython. +void pb_type_tools_awaitable_init(void) { + first_awaitable.base.type = &pb_type_tools_awaitable; + first_awaitable.test_completion = NULL; + first_awaitable.next_awaitable = MP_OBJ_NULL; +} + +STATIC pb_type_tools_awaitable_obj_t *pb_type_tools_awaitable_get(void) { + + // Find next available awaitable. + pb_type_tools_awaitable_obj_t *awaitable = &first_awaitable; + while (awaitable->test_completion != NULL && awaitable->next_awaitable != MP_OBJ_NULL) { + awaitable = awaitable->next_awaitable; + } + // If the last known awaitable is still in use, allocate another. + if (awaitable->test_completion != NULL) { + // Attach to the previous one. + awaitable->next_awaitable = mp_obj_malloc(pb_type_tools_awaitable_obj_t, &pb_type_tools_awaitable); + + // Initialize the new awaitable. + awaitable = awaitable->next_awaitable; + awaitable->next_awaitable = MP_OBJ_NULL; + } + return awaitable; +} + +// Get an available awaitable, add callbacks, and return as object so user can await it. +STATIC mp_obj_t pb_type_tools_awaitable_new(mp_obj_t obj, pb_awaitable_test_completion_t test_completion, pb_awaitable_cancel_t cancel) { + pb_type_tools_awaitable_obj_t *awaitable = pb_type_tools_awaitable_get(); + awaitable->obj = obj; + awaitable->test_completion = test_completion; + awaitable->cancel = cancel; + return MP_OBJ_FROM_PTR(awaitable); +} + +STATIC mp_obj_t pb_tools_wait_test_completion(mp_obj_t obj) { + pb_type_tools_awaitable_obj_t *awaitable = MP_OBJ_TO_PTR(obj); + return (mp_hal_ticks_ms() - awaitable->end_time) < (uint32_t)INT32_MAX ? MP_OBJ_STOP_ITERATION : mp_const_none; +} + +mp_obj_t pb_type_tools_await_time(mp_obj_t duration_in) { + mp_int_t duration = mp_obj_get_int(duration_in); + pb_type_tools_awaitable_obj_t *awaitable = pb_type_tools_awaitable_get(); + awaitable->obj = MP_OBJ_FROM_PTR(awaitable); + awaitable->test_completion = duration > 0 ? pb_tools_wait_test_completion : NULL; + awaitable->end_time = mp_hal_ticks_ms() + duration; + awaitable->cancel = NULL; + return MP_OBJ_FROM_PTR(awaitable); +} + +// Helper functions to simplify the final part of most methods that can be +// either blocking or not, like motor methods. +mp_obj_t pb_type_tools_await_or_wait(mp_obj_t obj, pb_awaitable_test_completion_t test_completion, pb_awaitable_cancel_t cancel) { + + // Make an awaitable object for the given operation. + mp_obj_t generator = pb_type_tools_awaitable_new(obj, test_completion, cancel); + + // Within run loop, just return the generator that user program will iterate. + if (pb_module_tools_run_loop_is_active()) { + return generator; + } + + // Oterwise block and wait for it to complete. + while (true) { + mp_obj_t next = pb_type_tools_awaitable_iternext(generator); + if (next == mp_const_none) { + // Not complete, keep waiting. + mp_hal_delay_ms(5); + continue; + } + return next == MP_OBJ_STOP_ITERATION ? mp_const_none : MP_STATE_THREAD(stop_iteration_arg); + } +} + +#endif // PYBRICKS_PY_TOOLS From 9f23f0a235c8a2a46435ed4eb7029687c57fb8cb Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 12:40:16 +0200 Subject: [PATCH 19/59] pybricks.tools: Use new awaitable for wait. --- bricks/_common/modules/_task.py | 9 ++------- pybricks/tools/pb_module_tools.c | 32 ++++++++++++++++++++------------ 2 files changed, 22 insertions(+), 19 deletions(-) diff --git a/bricks/_common/modules/_task.py b/bricks/_common/modules/_task.py index b70f8ac33..cecd79ee3 100644 --- a/bricks/_common/modules/_task.py +++ b/bricks/_common/modules/_task.py @@ -111,7 +111,7 @@ async def main(): tasks.run(tasks.all(task_1(), task_2())) """ from gc import collect - from pybricks.tools import _set_run_loop_active, StopWatch, wait + from pybricks.tools import _set_run_loop_active, _wait_block, StopWatch _set_run_loop_active(True) @@ -122,19 +122,14 @@ async def main(): # TODO: keep track of loop time stats and provide API to user for _ in task: - # needed here so that wait() is blocking - _set_run_loop_active(False) - # garbage collect to keep GC high watermark to a minimum collect() # GC can take many milliseconds when lots of RAM is used so we need # to only wait the time remaining after user code and GC. val = max(0, loop_time - (timer.time() - start)) - wait(val) + _wait_block(val) start += loop_time - - _set_run_loop_active(True) finally: _set_run_loop_active(False) diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 20925b972..b35274505 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -19,22 +19,28 @@ #include #include +// Implementation of wait that always blocks. Needed for system runloop code +// to briefly wait inside runloop. +STATIC mp_obj_t pb_module_tools__wait_block(mp_obj_t time_in) { + mp_int_t time = pb_obj_get_int(time_in); + if (time > 0) { + mp_hal_delay_ms(time); + } + return mp_const_none; +} +MP_DEFINE_CONST_FUN_OBJ_1(pb_module_tools__wait_block_obj, pb_module_tools__wait_block); + STATIC mp_obj_t tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, PB_ARG_REQUIRED(time)); - mp_int_t time = pb_obj_get_int(time_in); - - // Within run loop, return awaitable. + // Inside run loop, return generator to await time. if (pb_module_tools_run_loop_is_active()) { - return pb_type_tools_wait_new(time, NULL); + return pb_type_tools_await_time(time_in); } - // In blocking mode, wait until done. - if (time > 0) { - mp_hal_delay_ms(time); - } - return mp_const_none; + // Outside of run loop, just block to wait. + return pb_module_tools__wait_block(time_in); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(tools_wait_obj, 0, tools_wait); @@ -47,15 +53,16 @@ bool pb_module_tools_run_loop_is_active() { STATIC mp_obj_t pb_module_tools___init__(void) { _pb_module_tools_run_loop_is_active = false; pb_type_tools_wait_reset(); + pb_type_tools_awaitable_init(); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_0(pb_module_tools___init___obj, pb_module_tools___init__); -STATIC mp_obj_t pb_module_tools_set_run_loop_active(mp_obj_t self_in) { +STATIC mp_obj_t pb_module_tools__set_run_loop_active(mp_obj_t self_in) { _pb_module_tools_run_loop_is_active = mp_obj_is_true(self_in); return mp_const_none; } -MP_DEFINE_CONST_FUN_OBJ_1(pb_module_tools_set_run_loop_active_obj, pb_module_tools_set_run_loop_active); +MP_DEFINE_CONST_FUN_OBJ_1(pb_module_tools__set_run_loop_active_obj, pb_module_tools__set_run_loop_active); #if MICROPY_MODULE_ATTR_DELEGATION // pybricks.tools.task is implemented as pure Python code in the frozen _task @@ -71,7 +78,8 @@ STATIC void pb_module_tools_attr(mp_obj_t self_in, qstr attr, mp_obj_t *dest) { STATIC const mp_rom_map_elem_t tools_globals_table[] = { { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_tools) }, { MP_ROM_QSTR(MP_QSTR___init__), MP_ROM_PTR(&pb_module_tools___init___obj)}, - { MP_ROM_QSTR(MP_QSTR__set_run_loop_active), MP_ROM_PTR(&pb_module_tools_set_run_loop_active_obj)}, + { MP_ROM_QSTR(MP_QSTR__set_run_loop_active), MP_ROM_PTR(&pb_module_tools__set_run_loop_active_obj)}, + { MP_ROM_QSTR(MP_QSTR__wait_block), MP_ROM_PTR(&pb_module_tools__wait_block_obj) }, { MP_ROM_QSTR(MP_QSTR_wait), MP_ROM_PTR(&tools_wait_obj) }, { MP_ROM_QSTR(MP_QSTR_StopWatch), MP_ROM_PTR(&pb_type_StopWatch) }, #if MICROPY_PY_BUILTINS_FLOAT From 4e5706bb521d1bbd7439abf350accb0cf408b8bc Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 13:45:15 +0200 Subject: [PATCH 20/59] pybricks.tools: Use new awaitable for Motor. --- pybricks/common.h | 39 +---- pybricks/common/pb_type_motor.c | 280 ++++++++------------------------ 2 files changed, 72 insertions(+), 247 deletions(-) diff --git a/pybricks/common.h b/pybricks/common.h index f9d70afc5..dc07a2271 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -97,46 +97,12 @@ mp_obj_t common_Logger_obj_make_new(pbio_log_t *log, uint8_t num_values); typedef struct _common_Motor_obj_t common_Motor_obj_t; -typedef struct _pb_type_MotorWait_obj_t pb_type_MotorWait_obj_t; - -/** - * A generator-like type for waiting on a motor operation to complete. - */ -struct _pb_type_MotorWait_obj_t { - mp_obj_base_t base; - /** - * Motor object whose move this generator is awaiting on. - */ - common_Motor_obj_t *motor_obj; - /** - * Voltage cap prior to starting. Only applicable for run_until_stalled. - * - * Set to -1 to indicate that this is not used. - */ - int32_t stall_voltage_restore_value; - /** - * What to do once stalled. Only applicable for run_until_stalled. - */ - pbio_control_on_completion_t stall_stop_type; - /** - * Whether this generator object was cancelled. - */ - bool was_cancelled; - /** - * Whether this generator object is done and thus can be recycled. This is - * when motion is complete or cancellation has been handled. - */ - bool has_ended; - /** - * Linked list of awaitables. - */ - pb_type_MotorWait_obj_t *next_awaitable; -}; - // pybricks._common.Motor() struct _common_Motor_obj_t { mp_obj_base_t base; pbio_servo_t *srv; + int32_t max_voltage_last; // TODO: Move to pbio, used to restore state after run_until_stalled + pbio_control_on_completion_t on_stall; // TODO: move to pbio, used to restore state after run_until_stalled #if PYBRICKS_PY_COMMON_MOTOR_MODEL mp_obj_t model; #endif @@ -147,7 +113,6 @@ struct _common_Motor_obj_t { mp_obj_t logger; #endif pbio_port_id_t port; - pb_type_MotorWait_obj_t first_awaitable; }; extern const mp_obj_type_t pb_type_Motor; diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index b60cf6e16..8fa6652c9 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -24,138 +24,6 @@ #include #include -// The __next__() method should raise a StopIteration if the operation is -// complete. -STATIC mp_obj_t pb_type_MotorWait_iternext(mp_obj_t self_in) { - pb_type_MotorWait_obj_t *self = MP_OBJ_TO_PTR(self_in); - - if (self->was_cancelled) { - // Gracefully handle cancellation: Allow generator to complete - // and clean up but don't raise any exceptions. - self->has_ended = true; - return MP_OBJ_STOP_ITERATION; - } - - // Handle I/O exceptions. - if (!pbio_servo_update_loop_is_running(self->motor_obj->srv)) { - pb_assert(PBIO_ERROR_NO_DEV); - } - - // First, handle normal case without stalling. - if (self->stall_voltage_restore_value == -1) { - if (pbio_control_is_done(&self->motor_obj->srv->control)) { - self->has_ended = true; - return MP_OBJ_STOP_ITERATION; - } - // Not done, so keep going. - return mp_const_none; - } - - // The remainder handles run_until_stalled. First, check stall state. - bool stalled; - uint32_t stall_duration; - pb_assert(pbio_servo_is_stalled(self->motor_obj->srv, &stalled, &stall_duration)); - - // Keep going if not stalled. - if (!stalled) { - return mp_const_none; - } - - // Read the angle upon completion of the stall maneuver. - int32_t stall_angle, stall_speed; - pb_assert(pbio_servo_get_state_user(self->motor_obj->srv, &stall_angle, &stall_speed)); - - // Stop moving. - pb_assert(pbio_servo_stop(self->motor_obj->srv, self->stall_stop_type)); - - // Restore original voltage limit. - pb_assert(pbio_dcmotor_set_settings(self->motor_obj->srv->dcmotor, self->stall_voltage_restore_value)); - - // Raise stop iteration with angle to return the final position. - self->has_ended = true; - return mp_make_stop_iteration(mp_obj_new_int(stall_angle)); -} - -// Cancel all generators belonging to this motor. -STATIC void pb_type_MotorWait_cancel_all(common_Motor_obj_t *motor_obj) { - pb_type_MotorWait_obj_t *self = &motor_obj->first_awaitable; - do { - self->was_cancelled = true; - self = self->next_awaitable; - } while (self != MP_OBJ_NULL); -} - -// The close() method is used to cancel an operation before it completes. If -// the operation is already complete, it should do nothing. It can be called -// more than once. -STATIC mp_obj_t pb_type_MotorWait_close(mp_obj_t self_in) { - pb_type_MotorWait_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_type_MotorWait_cancel_all(self->motor_obj); - pb_assert(pbio_dcmotor_user_command(self->motor_obj->srv->dcmotor, true, 0)); - return mp_const_none; -} -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_MotorWait_close_obj, pb_type_MotorWait_close); - -STATIC const mp_rom_map_elem_t pb_type_MotorWait_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_MotorWait_close_obj) }, -}; -MP_DEFINE_CONST_DICT(pb_type_MotorWait_locals_dict, pb_type_MotorWait_locals_dict_table); - -// This is a partial implementation of the Python generator type. It is missing -// send(value) and throw(type[, value[, traceback]]) -MP_DEFINE_CONST_OBJ_TYPE(pb_type_MotorWait, - MP_QSTR_MotorWait, - MP_TYPE_FLAG_ITER_IS_ITERNEXT, - iter, pb_type_MotorWait_iternext, - locals_dict, &pb_type_MotorWait_locals_dict); - -STATIC mp_obj_t pb_type_MotorWait_new_stalled(common_Motor_obj_t *motor_obj, int32_t stall_voltage_restore_value, int32_t stall_stop_type) { - - // Cancel everything for this motor. - pb_type_MotorWait_cancel_all(motor_obj); - - // Find next available previously allocated awaitable. - pb_type_MotorWait_obj_t *self = &motor_obj->first_awaitable; - while (!self->has_ended && self->next_awaitable != MP_OBJ_NULL) { - self = self->next_awaitable; - } - // No free awaitable available, so allocate one. - if (!self->has_ended) { - // Attach to the previous one. - self->next_awaitable = m_new_obj(pb_type_MotorWait_obj_t); - - // Initialize the new awaitable. - self = self->next_awaitable; - self->next_awaitable = MP_OBJ_NULL; - self->base.type = &pb_type_MotorWait; - } - - // Initialize what to await on. - self->motor_obj = motor_obj; - self->has_ended = false; - self->was_cancelled = false; - self->stall_stop_type = stall_stop_type; - self->stall_voltage_restore_value = stall_voltage_restore_value; - - // Return the awaitable where the user can await it. - return MP_OBJ_FROM_PTR(self); -} - -STATIC mp_obj_t pb_type_MotorWait_new(common_Motor_obj_t *motor_obj) { - return pb_type_MotorWait_new_stalled(motor_obj, -1, PBIO_CONTROL_ON_COMPLETION_COAST); -} - -/* Wait for servo maneuver to complete */ - -STATIC void wait_for_completion(pbio_servo_t *srv) { - while (!pbio_control_is_done(&srv->control)) { - mp_hal_delay_ms(5); - } - if (!pbio_servo_update_loop_is_running(srv)) { - pb_assert(PBIO_ERROR_NO_DEV); - } -} - // Gets the number of millidegrees of the motor, for each whole degree // of rotation at the gear train output. For example, if the gear train // slows the motor down using a 12 teeth and a 36 teeth gear, the result @@ -240,11 +108,6 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, self->srv = srv; self->port = port; - // Initialize first awaitable singleton. - self->first_awaitable.base.type = &pb_type_MotorWait; - self->first_awaitable.has_ended = true; - self->first_awaitable.next_awaitable = MP_OBJ_NULL; - #if PYBRICKS_PY_COMMON_CONTROL // Create an instance of the Control class self->control = pb_type_Control_obj_make_new(&self->srv->control); @@ -288,9 +151,6 @@ STATIC mp_obj_t common_Motor_reset_angle(size_t n_args, const mp_obj_t *pos_args // Set the new angle pb_assert(pbio_servo_reset_angle(self->srv, reset_angle, reset_to_abs)); - // Cancel user-level motor wait operations. - pb_type_MotorWait_cancel_all(self); - return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_reset_angle_obj, 1, common_Motor_reset_angle); @@ -316,9 +176,6 @@ STATIC mp_obj_t common_Motor_run(size_t n_args, const mp_obj_t *pos_args, mp_map mp_int_t speed = pb_obj_get_int(speed_in); pb_assert(pbio_servo_run_forever(self->srv, speed)); - // Cancel user-level motor wait operations in parallel tasks. - pb_type_MotorWait_cancel_all(self); - return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); @@ -327,13 +184,30 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); STATIC mp_obj_t common_Motor_hold(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_HOLD)); - - // Cancel user-level motor wait operations in parallel tasks. - pb_type_MotorWait_cancel_all(self); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_hold_obj, common_Motor_hold); +STATIC mp_obj_t common_Motor_test_completion(mp_obj_t self_in) { + + common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // Handle I/O exceptions like port unplugged. + if (!pbio_servo_update_loop_is_running(self->srv)) { + pb_assert(PBIO_ERROR_NO_DEV); + } + + // Get completion state. + return pbio_control_is_done(&self->srv->control) ? MP_OBJ_STOP_ITERATION : mp_const_none; +} + +STATIC void common_Motor_cancel(mp_obj_t self_in) { + common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); + // TODO: Can drop next line if moved to pbio + pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, self->max_voltage_last)); + pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_COAST)); +} + // pybricks._common.Motor.run_time STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, @@ -351,19 +225,46 @@ STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, m // Call pbio with parsed user/default arguments pb_assert(pbio_servo_run_time(self->srv, speed, time, then)); - // Handle async case, return a generator. - if (pb_module_tools_run_loop_is_active()) { - return pb_type_MotorWait_new(self); + // Old way to do parallel movement is to start and not wait on anything. + if (!mp_obj_is_true(wait_in)) { + return mp_const_none; } + // Handle completion by awaiting or blocking. + return pb_type_tools_await_or_wait(pos_args[0], common_Motor_test_completion, common_Motor_cancel); +} +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_time_obj, 1, common_Motor_run_time); - // Otherwise, handle default blocking wait. - if (mp_obj_is_true(wait_in)) { - wait_for_completion(self->srv); +STATIC mp_obj_t common_Motor_stall_test_completion(mp_obj_t self_in) { + + common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // Restore original voltage limit. + pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, self->max_voltage_last)); + + // Handle I/O exceptions like port unplugged. + if (!pbio_servo_update_loop_is_running(self->srv)) { + pb_assert(PBIO_ERROR_NO_DEV); } - return mp_const_none; + bool stalled; + uint32_t stall_duration; + pb_assert(pbio_servo_is_stalled(self->srv, &stalled, &stall_duration)); + + // Keep going if not stalled. + if (!stalled) { + return mp_const_none; + } + + // Read the angle upon completion of the stall maneuver. + int32_t stall_angle, stall_speed; + pb_assert(pbio_servo_get_state_user(self->srv, &stall_angle, &stall_speed)); + + // Stop moving. + pb_assert(pbio_servo_stop(self->srv, self->on_stall)); + + // Raise stop iteration with angle to return the final position. + return mp_make_stop_iteration(mp_obj_new_int(stall_angle)); } -STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_time_obj, 1, common_Motor_run_time); // pybricks._common.Motor.run_until_stalled STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -374,11 +275,10 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po PB_ARG_DEFAULT_NONE(duty_limit)); mp_int_t speed = pb_obj_get_int(speed_in); - pbio_control_on_completion_t on_completion = pb_type_enum_get_value(then_in, &pb_enum_type_Stop); - // Read original voltage limit so we can restore it when we're done. - int32_t max_voltage_old; - pbio_dcmotor_get_settings(self->srv->dcmotor, &max_voltage_old); + // Read original voltage limit and completion type so we can restore it when we're done. + pbio_dcmotor_get_settings(self->srv->dcmotor, &self->max_voltage_last); + self->on_stall = pb_type_enum_get_value(then_in, &pb_enum_type_Stop); // If duty_limit argument given, limit duty during this maneuver. if (duty_limit_in != mp_const_none) { @@ -395,34 +295,8 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po // Start moving. pb_assert(pbio_servo_run_forever(self->srv, speed)); - // Make generator for checking completion. - mp_obj_t gen = pb_type_MotorWait_new_stalled(self, max_voltage_old, on_completion); - - // Within run loop, just return the generator. - if (pb_module_tools_run_loop_is_active()) { - return gen; - } - - // The remainder handles blocking case. It is wrapped in an nlr so - // we can restore the voltage limit if an exception occurs. - nlr_buf_t nlr; - if (nlr_push(&nlr) == 0) { - // Await the generator. - while (true) { - mp_obj_t next = pb_type_MotorWait_iternext(gen); - if (next != mp_const_none) { - mp_obj_t ret_val = MP_STATE_THREAD(stop_iteration_arg); - nlr_pop(); - return ret_val; - } - // Not complete, keep waiting. - mp_hal_delay_ms(5); - } - } else { - pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, - ((pb_type_MotorWait_obj_t *)MP_OBJ_TO_PTR(gen))->stall_voltage_restore_value)); - nlr_raise(MP_OBJ_FROM_PTR(nlr.ret_val)); - } + // Handle completion by awaiting or blocking. + return pb_type_tools_await_or_wait(pos_args[0], common_Motor_stall_test_completion, common_Motor_cancel); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_until_stalled_obj, 1, common_Motor_run_until_stalled); @@ -442,17 +316,12 @@ STATIC mp_obj_t common_Motor_run_angle(size_t n_args, const mp_obj_t *pos_args, // Call pbio with parsed user/default arguments pb_assert(pbio_servo_run_angle(self->srv, speed, angle, then)); - // Handle async case, return a generator. - if (pb_module_tools_run_loop_is_active()) { - return pb_type_MotorWait_new(self); - } - - // Otherwise, handle default blocking wait. - if (mp_obj_is_true(wait_in)) { - wait_for_completion(self->srv); + // Old way to do parallel movement is to start and not wait on anything. + if (!mp_obj_is_true(wait_in)) { + return mp_const_none; } - - return mp_const_none; + // Handle completion by awaiting or blocking. + return pb_type_tools_await_or_wait(pos_args[0], common_Motor_test_completion, common_Motor_cancel); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_angle_obj, 1, common_Motor_run_angle); @@ -472,17 +341,12 @@ STATIC mp_obj_t common_Motor_run_target(size_t n_args, const mp_obj_t *pos_args, // Call pbio with parsed user/default arguments pb_assert(pbio_servo_run_target(self->srv, speed, target_angle, then)); - // Handle async case, return a generator. - if (pb_module_tools_run_loop_is_active()) { - return pb_type_MotorWait_new(self); - } - - // Otherwise, handle default blocking wait. - if (mp_obj_is_true(wait_in)) { - wait_for_completion(self->srv); + // Old way to do parallel movement is to start and not wait on anything. + if (!mp_obj_is_true(wait_in)) { + return mp_const_none; } - - return mp_const_none; + // Handle completion by awaiting or blocking. + return pb_type_tools_await_or_wait(pos_args[0], common_Motor_test_completion, common_Motor_cancel); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_target_obj, 1, common_Motor_run_target); @@ -494,10 +358,6 @@ STATIC mp_obj_t common_Motor_track_target(size_t n_args, const mp_obj_t *pos_arg mp_int_t target_angle = pb_obj_get_int(target_angle_in); pb_assert(pbio_servo_track_target(self->srv, target_angle)); - - // Cancel user-level motor wait operations in parallel tasks. - pb_type_MotorWait_cancel_all(self); - return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_track_target_obj, 1, common_Motor_track_target); From e287f718b67c7ad952261b47364c69ebff63d72f Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 13:52:51 +0200 Subject: [PATCH 21/59] pybricks.tools: Use new awaitable for DriveBase. --- pybricks/robotics/pb_type_drivebase.c | 193 ++++---------------------- 1 file changed, 30 insertions(+), 163 deletions(-) diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 899df99bb..8a2595ece 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -24,32 +24,6 @@ typedef struct _pb_type_DriveBase_obj_t pb_type_DriveBase_obj_t; -typedef struct _pb_type_DriveBaseWait_obj_t pb_type_DriveBaseWait_obj_t; - -/** - * A generator-like type for waiting on a motor operation to complete. - */ -struct _pb_type_DriveBaseWait_obj_t { - mp_obj_base_t base; - /** - * Motor object whose move this generator is awaiting on. - */ - pb_type_DriveBase_obj_t *drivebase_obj; - /** - * Whether this generator object was cancelled. - */ - bool was_cancelled; - /** - * Whether this generator object is done and thus can be recycled. This is - * when motion is complete or cancellation has been handled. - */ - bool has_ended; - /** - * Linked list of awaitables. - */ - pb_type_DriveBaseWait_obj_t *next_awaitable; -}; - // pybricks.robotics.DriveBase class object struct _pb_type_DriveBase_obj_t { mp_obj_base_t base; @@ -60,96 +34,8 @@ struct _pb_type_DriveBase_obj_t { mp_obj_t heading_control; mp_obj_t distance_control; #endif - pb_type_DriveBaseWait_obj_t first_awaitable; }; -STATIC mp_obj_t pb_type_DriveBaseWait_iternext(mp_obj_t self_in) { - pb_type_DriveBaseWait_obj_t *self = MP_OBJ_TO_PTR(self_in); - - if (self->was_cancelled) { - // Gracefully handle cancellation: Allow generator to complete - // and clean up but don't raise any exceptions. - self->has_ended = true; - return MP_OBJ_STOP_ITERATION; - } - - // Handle I/O exceptions. - if (!pbio_drivebase_update_loop_is_running(self->drivebase_obj->db)) { - pb_assert(PBIO_ERROR_NO_DEV); - } - - // Check for completion - if (pbio_drivebase_is_done(self->drivebase_obj->db)) { - self->has_ended = true; - return MP_OBJ_STOP_ITERATION; - } - // Not done, so keep going. - return mp_const_none; -} - -// Cancel all generators belonging to this motor. -STATIC void pb_type_DriveBaseWait_cancel_all(pb_type_DriveBase_obj_t *drivebase_obj) { - pb_type_DriveBaseWait_obj_t *self = &drivebase_obj->first_awaitable; - do { - self->was_cancelled = true; - self = self->next_awaitable; - } while (self != MP_OBJ_NULL); -} - -// The close() method is used to cancel an operation before it completes. If -// the operation is already complete, it should do nothing. It can be called -// more than once. -STATIC mp_obj_t pb_type_DriveBaseWait_close(mp_obj_t self_in) { - pb_type_DriveBaseWait_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_type_DriveBaseWait_cancel_all(self->drivebase_obj); - pb_assert(pbio_drivebase_stop(self->drivebase_obj->db, PBIO_CONTROL_ON_COMPLETION_COAST)); - return mp_const_none; -} -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBaseWait_close_obj, pb_type_DriveBaseWait_close); - -STATIC const mp_rom_map_elem_t pb_type_DriveBaseWait_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_DriveBaseWait_close_obj) }, -}; -MP_DEFINE_CONST_DICT(pb_type_DriveBaseWait_locals_dict, pb_type_DriveBaseWait_locals_dict_table); - -// This is a partial implementation of the Python generator type. It is missing -// send(value) and throw(type[, value[, traceback]]) -MP_DEFINE_CONST_OBJ_TYPE(pb_type_DriveBaseWait, - MP_QSTR_DriveBaseWait, - MP_TYPE_FLAG_ITER_IS_ITERNEXT, - iter, pb_type_DriveBaseWait_iternext, - locals_dict, &pb_type_DriveBaseWait_locals_dict); - -STATIC mp_obj_t pb_type_DriveBaseWait_new(pb_type_DriveBase_obj_t *drivebase_obj) { - - // Cancel everything for this motor. - pb_type_DriveBaseWait_cancel_all(drivebase_obj); - - // Find next available previously allocated awaitable. - pb_type_DriveBaseWait_obj_t *self = &drivebase_obj->first_awaitable; - while (!self->has_ended && self->next_awaitable != MP_OBJ_NULL) { - self = self->next_awaitable; - } - // No free awaitable available, so allocate one. - if (!self->has_ended) { - // Attach to the previous one. - self->next_awaitable = m_new_obj(pb_type_DriveBaseWait_obj_t); - - // Initialize the new awaitable. - self = self->next_awaitable; - self->next_awaitable = MP_OBJ_NULL; - self->base.type = &pb_type_DriveBaseWait; - } - - // Initialize what to await on. - self->drivebase_obj = drivebase_obj; - self->has_ended = false; - self->was_cancelled = false; - - // Return the awaitable where the user can await it. - return MP_OBJ_FROM_PTR(self); -} - // pybricks.robotics.DriveBase.reset STATIC mp_obj_t pb_type_DriveBase_reset(mp_obj_t self_in) { pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); @@ -197,21 +83,25 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a // Reset drivebase state pb_type_DriveBase_reset(MP_OBJ_FROM_PTR(self)); - // Initialize first awaitable singleton. - self->first_awaitable.base.type = &pb_type_DriveBaseWait; - self->first_awaitable.has_ended = true; - self->first_awaitable.next_awaitable = MP_OBJ_NULL; - return MP_OBJ_FROM_PTR(self); } -STATIC void wait_for_completion_drivebase(pbio_drivebase_t *db) { - while (!pbio_drivebase_is_done(db)) { - mp_hal_delay_ms(5); - } - if (!pbio_drivebase_update_loop_is_running(db)) { +STATIC mp_obj_t pb_type_DriveBase_test_completion(mp_obj_t self_in) { + + pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // Handle I/O exceptions like port unplugged. + if (!pbio_drivebase_update_loop_is_running(self->db)) { pb_assert(PBIO_ERROR_NO_DEV); } + + // Get completion state. + return pbio_drivebase_is_done(self->db) ? MP_OBJ_STOP_ITERATION : mp_const_none; +} + +STATIC void pb_type_DriveBase_cancel(mp_obj_t self_in) { + pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); + pb_assert(pbio_drivebase_stop(self->db, PBIO_CONTROL_ON_COMPLETION_COAST)); } // pybricks.robotics.DriveBase.straight @@ -227,17 +117,12 @@ STATIC mp_obj_t pb_type_DriveBase_straight(size_t n_args, const mp_obj_t *pos_ar pb_assert(pbio_drivebase_drive_straight(self->db, distance, then)); - // Handle async case, return a generator. - if (pb_module_tools_run_loop_is_active()) { - return pb_type_DriveBaseWait_new(self); + // Old way to do parallel movement is to start and not wait on anything. + if (!mp_obj_is_true(wait_in)) { + return mp_const_none; } - - // Otherwise, handle default blocking wait. - if (mp_obj_is_true(wait_in)) { - wait_for_completion_drivebase(self->db); - } - - return mp_const_none; + // Handle completion by awaiting or blocking. + return pb_type_tools_await_or_wait(pos_args[0], pb_type_DriveBase_test_completion, pb_type_DriveBase_cancel); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_straight_obj, 1, pb_type_DriveBase_straight); @@ -255,17 +140,12 @@ STATIC mp_obj_t pb_type_DriveBase_turn(size_t n_args, const mp_obj_t *pos_args, // Turning in place is done as a curve with zero radius and a given angle. pb_assert(pbio_drivebase_drive_curve(self->db, 0, angle, then)); - // Handle async case, return a generator. - if (pb_module_tools_run_loop_is_active()) { - return pb_type_DriveBaseWait_new(self); - } - - // Otherwise, handle default blocking wait. - if (mp_obj_is_true(wait_in)) { - wait_for_completion_drivebase(self->db); + // Old way to do parallel movement is to start and not wait on anything. + if (!mp_obj_is_true(wait_in)) { + return mp_const_none; } - - return mp_const_none; + // Handle completion by awaiting or blocking. + return pb_type_tools_await_or_wait(pos_args[0], pb_type_DriveBase_test_completion, pb_type_DriveBase_cancel); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_turn_obj, 1, pb_type_DriveBase_turn); @@ -284,17 +164,12 @@ STATIC mp_obj_t pb_type_DriveBase_curve(size_t n_args, const mp_obj_t *pos_args, pb_assert(pbio_drivebase_drive_curve(self->db, radius, angle, then)); - // Handle async case, return a generator. - if (pb_module_tools_run_loop_is_active()) { - return pb_type_DriveBaseWait_new(self); - } - - // Otherwise, handle default blocking wait. - if (mp_obj_is_true(wait_in)) { - wait_for_completion_drivebase(self->db); + // Old way to do parallel movement is to start and not wait on anything. + if (!mp_obj_is_true(wait_in)) { + return mp_const_none; } - - return mp_const_none; + // Handle completion by awaiting or blocking. + return pb_type_tools_await_or_wait(pos_args[0], pb_type_DriveBase_test_completion, pb_type_DriveBase_cancel); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_curve_obj, 1, pb_type_DriveBase_curve); @@ -311,21 +186,13 @@ STATIC mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, pb_assert(pbio_drivebase_drive_forever(self->db, speed, turn_rate)); - // Cancel user-level motor wait operations in parallel tasks. - pb_type_DriveBaseWait_cancel_all(self); - return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_drive_obj, 1, pb_type_DriveBase_drive); // pybricks.robotics.DriveBase.stop STATIC mp_obj_t pb_type_DriveBase_stop(mp_obj_t self_in) { - pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_assert(pbio_drivebase_stop(self->db, PBIO_CONTROL_ON_COMPLETION_COAST)); - - // Cancel user-level motor wait operations in parallel tasks. - pb_type_DriveBaseWait_cancel_all(self); - + pb_type_DriveBase_cancel(self_in); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_stop_obj, pb_type_DriveBase_stop); From 4e8505d11f935591c1c43e3d89727d4fb8ac7666 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 14:05:56 +0200 Subject: [PATCH 22/59] pybricks.tools: Use new awaitable for beep. --- pybricks/common/pb_type_speaker.c | 38 ++++++++++++++++--------------- 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 56b5e7989..0e31123c2 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -28,6 +28,9 @@ typedef struct { mp_obj_base_t base; bool initialized; + // Time at which to stop sound. + uint32_t beep_end_time; + // volume in 0..100 range uint8_t volume; @@ -117,6 +120,20 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg return MP_OBJ_FROM_PTR(self); } +STATIC mp_obj_t pb_type_Speaker_test_completion(mp_obj_t self_in) { + pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); + if (mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX) { + pb_type_Speaker_stop_beep(); + return MP_OBJ_STOP_ITERATION; + } + // Not done yet, keep going. + return mp_const_none; +} + +STATIC void pb_type_Speaker_cancel(mp_obj_t self_in) { + pb_type_Speaker_stop_beep(); +} + STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pb_type_Speaker_obj_t, self, @@ -128,27 +145,12 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp pb_type_Speaker_start_beep(frequency, self->sample_attenuator); - // Within run loop, return awaitable. - if (pb_module_tools_run_loop_is_active()) { - return pb_type_tools_wait_new(duration, pb_type_Speaker_stop_beep); - } - - // In blocking mode, wait until done. if (duration < 0) { - return mp_const_none; + duration = 0; } - nlr_buf_t nlr; - if (nlr_push(&nlr) == 0) { - mp_hal_delay_ms(duration); - pb_type_Speaker_stop_beep(); - nlr_pop(); - } else { - pb_type_Speaker_stop_beep(); - nlr_jump(nlr.ret_val); - } - - return mp_const_none; + self->beep_end_time = mp_hal_ticks_ms() + (uint32_t)duration; + return pb_type_tools_await_or_wait(pos_args[0], pb_type_Speaker_test_completion, pb_type_Speaker_cancel); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_beep_obj, 1, pb_type_Speaker_beep); From d674b8d5dca9f322d91f94aa8ad3b852b0d6ea53 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 14:31:08 +0200 Subject: [PATCH 23/59] pybricks.tools: Restore cancel handling on exception. This was temporarily removed when updating the new awaitable. --- pybricks/tools/pb_type_awaitable.c | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 2d40ff9e5..1b90eb211 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -130,7 +130,7 @@ mp_obj_t pb_type_tools_await_time(mp_obj_t duration_in) { pb_type_tools_awaitable_obj_t *awaitable = pb_type_tools_awaitable_get(); awaitable->obj = MP_OBJ_FROM_PTR(awaitable); awaitable->test_completion = duration > 0 ? pb_tools_wait_test_completion : NULL; - awaitable->end_time = mp_hal_ticks_ms() + duration; + awaitable->end_time = mp_hal_ticks_ms() + (uint32_t)duration; awaitable->cancel = NULL; return MP_OBJ_FROM_PTR(awaitable); } @@ -147,16 +147,24 @@ mp_obj_t pb_type_tools_await_or_wait(mp_obj_t obj, pb_awaitable_test_completion_ return generator; } - // Oterwise block and wait for it to complete. - while (true) { - mp_obj_t next = pb_type_tools_awaitable_iternext(generator); - if (next == mp_const_none) { - // Not complete, keep waiting. + // Otherwise block and wait for it to complete. + nlr_buf_t nlr; + mp_obj_t ret = MP_OBJ_NULL; + if (nlr_push(&nlr) == 0) { + while (pb_type_tools_awaitable_iternext(generator) == mp_const_none) { mp_hal_delay_ms(5); - continue; } - return next == MP_OBJ_STOP_ITERATION ? mp_const_none : MP_STATE_THREAD(stop_iteration_arg); + ret = MP_STATE_THREAD(stop_iteration_arg); + nlr_pop(); + } else { + // Cancel the operation if an exception was raised. + pb_type_tools_awaitable_obj_t *self = MP_OBJ_TO_PTR(generator); + if (self->cancel) { + self->cancel(self->obj); + } + nlr_jump(nlr.ret_val); } + return ret == MP_OBJ_NULL ? mp_const_none : ret; } #endif // PYBRICKS_PY_TOOLS From 97d0f9dc065d1bc5b30620eb16b37aaa64462f21 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 15:21:34 +0200 Subject: [PATCH 24/59] pybricks.common.speaker: Make play_notes awaitable. This is a basic demo that the common awaitable is flexible enough for slightly more complex routines. --- pybricks/common/pb_type_speaker.c | 77 ++++++++++++++++++------------- 1 file changed, 46 insertions(+), 31 deletions(-) diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 0e31123c2..24b144b80 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -28,8 +28,11 @@ typedef struct { mp_obj_base_t base; bool initialized; - // Time at which to stop sound. + // State of awaitable sound + mp_obj_t notes_generator; + uint32_t note_duration; uint32_t beep_end_time; + uint32_t release_end_time; // volume in 0..100 range uint8_t volume; @@ -120,18 +123,21 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg return MP_OBJ_FROM_PTR(self); } -STATIC mp_obj_t pb_type_Speaker_test_completion(mp_obj_t self_in) { +STATIC mp_obj_t pb_type_Speaker_beep_test_completion(mp_obj_t self_in) { pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); if (mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX) { pb_type_Speaker_stop_beep(); return MP_OBJ_STOP_ITERATION; } - // Not done yet, keep going. return mp_const_none; } STATIC void pb_type_Speaker_cancel(mp_obj_t self_in) { pb_type_Speaker_stop_beep(); + pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); + self->beep_end_time = mp_hal_ticks_ms(); + self->release_end_time = self->beep_end_time; + self->notes_generator = MP_OBJ_NULL; } STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -150,7 +156,9 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp } self->beep_end_time = mp_hal_ticks_ms() + (uint32_t)duration; - return pb_type_tools_await_or_wait(pos_args[0], pb_type_Speaker_test_completion, pb_type_Speaker_cancel); + self->release_end_time = self->beep_end_time; + self->notes_generator = MP_OBJ_NULL; + return pb_type_tools_await_or_wait(pos_args[0], pb_type_Speaker_beep_test_completion, pb_type_Speaker_cancel); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_beep_obj, 1, pb_type_Speaker_beep); @@ -316,16 +324,37 @@ STATIC void pb_type_Speaker_play_note(pb_type_Speaker_obj_t *self, mp_obj_t obj, pb_type_Speaker_start_beep((uint32_t)freq, self->sample_attenuator); - // Normally, we want there to be a period of no sound (release) so that - // notes are distinct instead of running together. To sound good, the - // release period is made proportional to duration of the note. - if (release) { - mp_hal_delay_ms(7 * duration / 8); + uint32_t time_now = mp_hal_ticks_ms(); + self->release_end_time = time_now + duration; + self->beep_end_time = release ? time_now + 7 * duration / 8 : time_now + duration; +} + +STATIC mp_obj_t pb_type_Speaker_notes_test_completion(mp_obj_t self_in) { + pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); + + bool release_done = mp_hal_ticks_ms() - self->release_end_time < (uint32_t)INT32_MAX; + bool beep_done = mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX; + + if (self->notes_generator != MP_OBJ_NULL && release_done && beep_done) { + // Full note done, so get next note. + mp_obj_t item = mp_iternext(self->notes_generator); + + // If there is no next note, generator is done. + if (item == MP_OBJ_STOP_ITERATION) { + return MP_OBJ_STOP_ITERATION; + } + + // Start the note. + pb_type_Speaker_play_note(self, item, self->note_duration); + return mp_const_none; + } + + if (beep_done) { + // Time to release. pb_type_Speaker_stop_beep(); - mp_hal_delay_ms(duration / 8); - } else { - mp_hal_delay_ms(duration); } + + return mp_const_none; } STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -334,26 +363,12 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar PB_ARG_REQUIRED(notes), PB_ARG_DEFAULT_INT(tempo, 120)); - // length of whole note in milliseconds = 4 quarter/whole * 60 s/min * 1000 ms/s / tempo quarter/min - int duration = 4 * 60 * 1000 / pb_obj_get_int(tempo_in); - - nlr_buf_t nlr; - mp_obj_t item; - mp_obj_t iterable = mp_getiter(notes_in, NULL); - if (nlr_push(&nlr) == 0) { - while ((item = mp_iternext(iterable)) != MP_OBJ_STOP_ITERATION) { - pb_type_Speaker_play_note(self, item, duration); - } - // in case the last note has '_' - pb_type_Speaker_stop_beep(); - nlr_pop(); - } else { - // ensure that sound stops if an exception is raised - pb_type_Speaker_stop_beep(); - nlr_jump(nlr.ret_val); - } + self->notes_generator = mp_getiter(notes_in, NULL); + self->note_duration = 4 * 60 * 1000 / pb_obj_get_int(tempo_in); + self->beep_end_time = mp_hal_ticks_ms(); + self->release_end_time = self->beep_end_time; + return pb_type_tools_await_or_wait(pos_args[0], pb_type_Speaker_notes_test_completion, pb_type_Speaker_cancel); - return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_play_notes_obj, 1, pb_type_Speaker_play_notes); From ee83f9c1b800f6054b5c874fb362316d128b625c Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 16:38:39 +0200 Subject: [PATCH 25/59] pybricks.tools: Delete type_wait. This is no longer used. --- bricks/_common/sources.mk | 1 - pybricks/tools.h | 5 -- pybricks/tools/pb_module_tools.c | 1 - pybricks/tools/pb_type_wait.c | 120 ------------------------------- 4 files changed, 127 deletions(-) delete mode 100644 pybricks/tools/pb_type_wait.c diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index 89b956505..5d2b8eeb0 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -100,7 +100,6 @@ PYBRICKS_PYBRICKS_SRC_C = $(addprefix pybricks/,\ tools/pb_type_awaitable.c \ tools/pb_type_matrix.c \ tools/pb_type_stopwatch.c \ - tools/pb_type_wait.c \ util_mp/pb_obj_helper.c \ util_mp/pb_type_enum.c \ util_pb/pb_color_map.c \ diff --git a/pybricks/tools.h b/pybricks/tools.h index 00c42093b..51d44ab43 100644 --- a/pybricks/tools.h +++ b/pybricks/tools.h @@ -26,13 +26,8 @@ typedef mp_obj_t (*pb_awaitable_test_completion_t)(mp_obj_t obj); */ typedef void (*pb_awaitable_cancel_t)(mp_obj_t obj); - bool pb_module_tools_run_loop_is_active(); -void pb_type_tools_wait_reset(void); - -mp_obj_t pb_type_tools_wait_new(mp_int_t duration, void (*callback)(void)); - void pb_type_tools_awaitable_init(void); mp_obj_t pb_type_tools_await_or_wait(mp_obj_t obj, pb_awaitable_test_completion_t test_completion, pb_awaitable_cancel_t cancel); diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index b35274505..a0caef4d8 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -52,7 +52,6 @@ bool pb_module_tools_run_loop_is_active() { STATIC mp_obj_t pb_module_tools___init__(void) { _pb_module_tools_run_loop_is_active = false; - pb_type_tools_wait_reset(); pb_type_tools_awaitable_init(); return mp_const_none; } diff --git a/pybricks/tools/pb_type_wait.c b/pybricks/tools/pb_type_wait.c deleted file mode 100644 index 2f45252ae..000000000 --- a/pybricks/tools/pb_type_wait.c +++ /dev/null @@ -1,120 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2023 The Pybricks Authors - -#include "py/mpconfig.h" - -#if PYBRICKS_PY_TOOLS - -#include "py/mphal.h" - -#include - -#include -#include -#include - -/** - * A generator-like type for waiting on a motor operation to complete. - */ -typedef struct _pb_type_tools_wait_obj_t pb_type_tools_wait_obj_t; - -struct _pb_type_tools_wait_obj_t { - mp_obj_base_t base; - /** - * When to stop waiting. - */ - uint32_t end_time; - /** - * Whether this generator object is done and thus can be recycled. - */ - bool has_ended; - /** - * Callback to call on completion or cancellation. - */ - void (*callback)(void); - /** - * Linked list of awaitables. - */ - pb_type_tools_wait_obj_t *next_awaitable; -}; - -STATIC mp_obj_t pb_type_tools_wait_iternext(mp_obj_t self_in) { - pb_type_tools_wait_obj_t *self = MP_OBJ_TO_PTR(self_in); - - // Stop on reaching target time or if externally cancelled. - if (mp_hal_ticks_ms() - self->end_time < (uint32_t)INT32_MAX || self->has_ended) { - self->has_ended = true; - if (self->callback) { - self->callback(); - } - return MP_OBJ_STOP_ITERATION; - } - // Not done, so keep going. - return mp_const_none; -} - -// close() cancels the awaitable. -STATIC mp_obj_t pb_type_tools_wait_close(mp_obj_t self_in) { - pb_type_tools_wait_obj_t *self = MP_OBJ_TO_PTR(self_in); - self->has_ended = true; - if (self->callback) { - self->callback(); - } - return mp_const_none; -} -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_tools_wait_close_obj, pb_type_tools_wait_close); - -STATIC const mp_rom_map_elem_t pb_type_tools_wait_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_tools_wait_close_obj) }, -}; -MP_DEFINE_CONST_DICT(pb_type_tools_wait_locals_dict, pb_type_tools_wait_locals_dict_table); - -// This is a partial implementation of the Python generator type. It is missing -// send(value) and throw(type[, value[, traceback]]) -MP_DEFINE_CONST_OBJ_TYPE(pb_type_tools_wait, - MP_QSTR_wait, - MP_TYPE_FLAG_ITER_IS_ITERNEXT, - iter, pb_type_tools_wait_iternext, - locals_dict, &pb_type_tools_wait_locals_dict); - -// Statically allocated awaitable from which all others can be found. -STATIC pb_type_tools_wait_obj_t first_awaitable; - -// Reset first awaitable on initializing MicroPython. -void pb_type_tools_wait_reset(void) { - first_awaitable.base.type = &pb_type_tools_wait; - first_awaitable.has_ended = true; - first_awaitable.next_awaitable = MP_OBJ_NULL; -} - -mp_obj_t pb_type_tools_wait_new(mp_int_t duration, void (*callback)(void)) { - - // When to stop waiting. - uint32_t end_time = mp_hal_ticks_ms() + (uint32_t)duration; - - // Find next available awaitable. - pb_type_tools_wait_obj_t *awaitable = &first_awaitable; - while (!awaitable->has_ended && awaitable->next_awaitable != MP_OBJ_NULL) { - awaitable = awaitable->next_awaitable; - } - // If the last known awaitable is still in use, allocate another. - if (!awaitable->has_ended) { - // Attach to the previous one. - awaitable->next_awaitable = m_new_obj(pb_type_tools_wait_obj_t); - - // Initialize the new awaitable. - awaitable = awaitable->next_awaitable; - awaitable->next_awaitable = MP_OBJ_NULL; - awaitable->base.type = &pb_type_tools_wait; - } - - // Initialize awaitable with the end time and callback. - awaitable->callback = callback; - awaitable->has_ended = duration < 0 ? true: false; - awaitable->end_time = end_time; - - // Return the awaitable where the user can await it. - return MP_OBJ_FROM_PTR(awaitable); -} - -#endif // PYBRICKS_PY_TOOLS From 5689ed0a4429d37baa6fc14564d4eb120e7955a0 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 16:46:10 +0200 Subject: [PATCH 26/59] pybricks.tools.task: Add empty module and disable Python module. This adds a measuring point to compare build size. --- bricks/_common/modules/_task.py | 135 ----------------------------- bricks/_common/mpconfigport.h | 2 +- bricks/_common/sources.mk | 1 + pybricks/pybricks.c | 3 + pybricks/tools.h | 5 +- pybricks/tools/pb_module_task.c | 50 +++++++++++ pybricks/tools/pb_module_tools.c | 56 ++---------- pybricks/tools/pb_type_awaitable.c | 2 +- 8 files changed, 67 insertions(+), 187 deletions(-) delete mode 100644 bricks/_common/modules/_task.py create mode 100644 pybricks/tools/pb_module_task.c diff --git a/bricks/_common/modules/_task.py b/bricks/_common/modules/_task.py deleted file mode 100644 index cecd79ee3..000000000 --- a/bricks/_common/modules/_task.py +++ /dev/null @@ -1,135 +0,0 @@ -def all(*tasks): - """ - Runs each task in parallel and waits for all to complete. - - Args: - tasks: one or more tasks/generator functions to run in parallel. - - Returns: - A list containing the return value of each task in the same order - as *tasks*. - - Example:: - - # generator syntax - def example1(): - result1, result2 = yield from tasks.all(task1(), task2()) - ... - - # async/await syntax - async def example2(): - result1, result2 = await tasks.all(task1(), task2()) - ... - - # return value can be ignored - async def example3(): - await tasks.all(task1(), task2()) - ... - """ - r = [None] * len(tasks) - pending = {i: t for i, t in enumerate(tasks)} - done = [] - - try: - while pending: - for i, t in pending.items(): - try: - next(t) - except StopIteration as ex: - r[i] = ex.value - done.append(i) - - # can't modify pending while we are iterating it so defer to here - for i in done: - del pending[i] - - done.clear() - - yield - finally: - # In case of crash in one task, all other unfinished tasks are - # canceled. Calling close() on finished tasks is a nop. - for t in tasks: - t.close() - - return r - - -def race(*tasks): - """ - Runs each task in parallel and waits for the first one to complete. After - the first task completes, all other tasks will be cleaned up by calling the - generator ``close()`` function. - - Important: if you want to know which task won the race, all tasks must - return a value that is not ``None``. - - Args: - tasks: One or more tasks/generator functions to run in parallel. - - Returns: - A list of values corresponding to each task in the same order as - *tasks*. All values will be ``None`` except for the winner of the - race. - """ - r = [None] * len(tasks) - - try: - while True: - for i, t in enumerate(tasks): - next(t) - yield - except StopIteration as ex: - # winner of the race - save the return value - r[i] = ex.value - finally: - for t in tasks: - t.close() - - return r - - -def run(task, loop_time=10): - """ - Runs a task in the main event loop. - - This will run one iteration of the task every *loop_time*. - - Args: - loop_time: The target loop time in milliseconds. - - Basic usage:: - - async def main(): - await ... - ... - - tasks.run(main()) - - To run more than one task, use :meth:`tasks.all`:: - - tasks.run(tasks.all(task_1(), task_2())) - """ - from gc import collect - from pybricks.tools import _set_run_loop_active, _wait_block, StopWatch - - _set_run_loop_active(True) - - try: - timer = StopWatch() - start = timer.time() - - # TODO: keep track of loop time stats and provide API to user - - for _ in task: - # garbage collect to keep GC high watermark to a minimum - collect() - - # GC can take many milliseconds when lots of RAM is used so we need - # to only wait the time remaining after user code and GC. - val = max(0, loop_time - (timer.time() - start)) - _wait_block(val) - - start += loop_time - finally: - _set_run_loop_active(False) diff --git a/bricks/_common/mpconfigport.h b/bricks/_common/mpconfigport.h index 668f9fd0f..f1ef9940d 100644 --- a/bricks/_common/mpconfigport.h +++ b/bricks/_common/mpconfigport.h @@ -110,7 +110,7 @@ #define MICROPY_ENABLE_SCHEDULER (0) #define MICROPY_PY_INSTANCE_ATTRS (1) -#define MICROPY_MODULE_ATTR_DELEGATION (1) +#define MICROPY_MODULE_ATTR_DELEGATION (0) #define MICROPY_PERSISTENT_CODE_LOAD (1) #define MICROPY_ENABLE_EXTERNAL_IMPORT (0) #define MICROPY_HAS_FILE_READER (0) diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index 5d2b8eeb0..76fedcd7f 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -96,6 +96,7 @@ PYBRICKS_PYBRICKS_SRC_C = $(addprefix pybricks/,\ robotics/pb_module_robotics.c \ robotics/pb_type_drivebase.c \ robotics/pb_type_spikebase.c \ + tools/pb_module_task.c \ tools/pb_module_tools.c \ tools/pb_type_awaitable.c \ tools/pb_type_matrix.c \ diff --git a/pybricks/pybricks.c b/pybricks/pybricks.c index 51cc5ae22..2132a946e 100644 --- a/pybricks/pybricks.c +++ b/pybricks/pybricks.c @@ -16,6 +16,7 @@ #include #include #include +#include #include "genhdr/mpversion.h" @@ -103,6 +104,7 @@ void pb_package_pybricks_init(bool import_all) { if (nlr_push(&nlr) == 0) { // Initialize the package. pb_type_Color_reset(); + pb_module_task_init(); // Import all if requested. if (import_all) { pb_package_import_all(); @@ -119,6 +121,7 @@ void pb_package_pybricks_init(bool import_all) { // exceptions as it is only called before executing anything else. void pb_package_pybricks_init(bool import_all) { pb_type_Color_reset(); + pb_module_task_init(); } #endif // PYBRICKS_OPT_COMPILER diff --git a/pybricks/tools.h b/pybricks/tools.h index 51d44ab43..c2eed8bdd 100644 --- a/pybricks/tools.h +++ b/pybricks/tools.h @@ -10,6 +10,9 @@ #include "py/obj.h" +extern const mp_obj_module_t pb_module_task; +void pb_module_task_init(void); + /** * Tests if awaitable operation is complete. Returns MP_OBJ_STOP_ITERATION if * done, possibly with return argument, else mp_const_none. @@ -26,7 +29,7 @@ typedef mp_obj_t (*pb_awaitable_test_completion_t)(mp_obj_t obj); */ typedef void (*pb_awaitable_cancel_t)(mp_obj_t obj); -bool pb_module_tools_run_loop_is_active(); +bool pb_module_task_run_loop_is_active(); void pb_type_tools_awaitable_init(void); diff --git a/pybricks/tools/pb_module_task.c b/pybricks/tools/pb_module_task.c new file mode 100644 index 000000000..140b6889b --- /dev/null +++ b/pybricks/tools/pb_module_task.c @@ -0,0 +1,50 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2020 The Pybricks Authors + +#include "py/mpconfig.h" + +#if PYBRICKS_PY_TOOLS + +#include "py/builtin.h" +#include "py/mphal.h" +#include "py/objmodule.h" +#include "py/runtime.h" + +#include +#include +#include + +// Implementation of wait that always blocks. Needed for system runloop code +// to briefly wait inside runloop. +STATIC mp_obj_t pb_module_task_run(mp_obj_t time_in) { + mp_int_t time = pb_obj_get_int(time_in); + if (time > 0) { + mp_hal_delay_ms(time); + } + return mp_const_none; +} +MP_DEFINE_CONST_FUN_OBJ_1(pb_module_task_run_obj, pb_module_task_run); + +STATIC bool _pb_module_task_run_loop_is_active; + +bool pb_module_task_run_loop_is_active() { + return _pb_module_task_run_loop_is_active; +} + +void pb_module_task_init(void) { + _pb_module_task_run_loop_is_active = false; +} + +STATIC const mp_rom_map_elem_t task_globals_table[] = { + { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_task) }, + { MP_ROM_QSTR(MP_QSTR_run), MP_ROM_PTR(&pb_module_task_run_obj) }, + +}; +STATIC MP_DEFINE_CONST_DICT(pb_module_task_globals, task_globals_table); + +const mp_obj_module_t pb_module_task = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t *)&pb_module_task_globals, +}; + +#endif // PYBRICKS_PY_TOOLS diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index a0caef4d8..4420fb6ce 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -19,68 +19,29 @@ #include #include -// Implementation of wait that always blocks. Needed for system runloop code -// to briefly wait inside runloop. -STATIC mp_obj_t pb_module_tools__wait_block(mp_obj_t time_in) { - mp_int_t time = pb_obj_get_int(time_in); - if (time > 0) { - mp_hal_delay_ms(time); - } - return mp_const_none; -} -MP_DEFINE_CONST_FUN_OBJ_1(pb_module_tools__wait_block_obj, pb_module_tools__wait_block); - STATIC mp_obj_t tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, PB_ARG_REQUIRED(time)); // Inside run loop, return generator to await time. - if (pb_module_tools_run_loop_is_active()) { + if (pb_module_task_run_loop_is_active()) { return pb_type_tools_await_time(time_in); } // Outside of run loop, just block to wait. - return pb_module_tools__wait_block(time_in); -} -STATIC MP_DEFINE_CONST_FUN_OBJ_KW(tools_wait_obj, 0, tools_wait); - -STATIC bool _pb_module_tools_run_loop_is_active; - -bool pb_module_tools_run_loop_is_active() { - return _pb_module_tools_run_loop_is_active; -} - -STATIC mp_obj_t pb_module_tools___init__(void) { - _pb_module_tools_run_loop_is_active = false; - pb_type_tools_awaitable_init(); - return mp_const_none; -} -MP_DEFINE_CONST_FUN_OBJ_0(pb_module_tools___init___obj, pb_module_tools___init__); - -STATIC mp_obj_t pb_module_tools__set_run_loop_active(mp_obj_t self_in) { - _pb_module_tools_run_loop_is_active = mp_obj_is_true(self_in); - return mp_const_none; -} -MP_DEFINE_CONST_FUN_OBJ_1(pb_module_tools__set_run_loop_active_obj, pb_module_tools__set_run_loop_active); - -#if MICROPY_MODULE_ATTR_DELEGATION -// pybricks.tools.task is implemented as pure Python code in the frozen _task -// module. This handler makes it available through the pybricks package. -STATIC void pb_module_tools_attr(mp_obj_t self_in, qstr attr, mp_obj_t *dest) { - if (attr == MP_QSTR_task) { - const mp_obj_t args[] = { MP_OBJ_NEW_QSTR(MP_QSTR__task) }; - dest[0] = mp_builtin___import__(MP_ARRAY_SIZE(args), args); + mp_int_t time = pb_obj_get_int(time_in); + if (time > 0) { + mp_hal_delay_ms(time); } + return mp_const_none; } -#endif +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(tools_wait_obj, 0, tools_wait); STATIC const mp_rom_map_elem_t tools_globals_table[] = { { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_tools) }, - { MP_ROM_QSTR(MP_QSTR___init__), MP_ROM_PTR(&pb_module_tools___init___obj)}, - { MP_ROM_QSTR(MP_QSTR__set_run_loop_active), MP_ROM_PTR(&pb_module_tools__set_run_loop_active_obj)}, - { MP_ROM_QSTR(MP_QSTR__wait_block), MP_ROM_PTR(&pb_module_tools__wait_block_obj) }, { MP_ROM_QSTR(MP_QSTR_wait), MP_ROM_PTR(&tools_wait_obj) }, { MP_ROM_QSTR(MP_QSTR_StopWatch), MP_ROM_PTR(&pb_type_StopWatch) }, + { MP_ROM_QSTR(MP_QSTR_task), MP_ROM_PTR(&pb_module_task) }, #if MICROPY_PY_BUILTINS_FLOAT { MP_ROM_QSTR(MP_QSTR_Matrix), MP_ROM_PTR(&pb_type_Matrix) }, { MP_ROM_QSTR(MP_QSTR_vector), MP_ROM_PTR(&pb_geometry_vector_obj) }, @@ -88,9 +49,6 @@ STATIC const mp_rom_map_elem_t tools_globals_table[] = { // backwards compatibility for pybricks.geometry.Axis { MP_ROM_QSTR(MP_QSTR_Axis), MP_ROM_PTR(&pb_enum_type_Axis) }, #endif // MICROPY_PY_BUILTINS_FLOAT - #if MICROPY_MODULE_ATTR_DELEGATION - MP_MODULE_ATTR_DELEGATION_ENTRY(&pb_module_tools_attr), - #endif }; STATIC MP_DEFINE_CONST_DICT(pb_module_tools_globals, tools_globals_table); diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 1b90eb211..dfb69d0a6 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -143,7 +143,7 @@ mp_obj_t pb_type_tools_await_or_wait(mp_obj_t obj, pb_awaitable_test_completion_ mp_obj_t generator = pb_type_tools_awaitable_new(obj, test_completion, cancel); // Within run loop, just return the generator that user program will iterate. - if (pb_module_tools_run_loop_is_active()) { + if (pb_module_task_run_loop_is_active()) { return generator; } From a2f401ee178a574768db1a4e99c5265420b0e61d Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 4 May 2023 21:11:32 +0200 Subject: [PATCH 27/59] pybricks.tools.task: Implement in C. Also restore Move Hub firmware size limit after this optimization. --- lib/pbio/platform/move_hub/pbdrvconfig.h | 2 +- lib/pbio/platform/move_hub/platform.ld | 4 +- pybricks/tools/pb_module_task.c | 213 +++++++++++++++++++++-- 3 files changed, 203 insertions(+), 16 deletions(-) diff --git a/lib/pbio/platform/move_hub/pbdrvconfig.h b/lib/pbio/platform/move_hub/pbdrvconfig.h index 04b7f23a4..af1db6756 100644 --- a/lib/pbio/platform/move_hub/pbdrvconfig.h +++ b/lib/pbio/platform/move_hub/pbdrvconfig.h @@ -20,7 +20,7 @@ #define PBDRV_CONFIG_BLOCK_DEVICE (1) #define PBDRV_CONFIG_BLOCK_DEVICE_FLASH_STM32 (1) -#define PBDRV_CONFIG_BLOCK_DEVICE_FLASH_STM32_SIZE (3 * 1024) // Must match FLASH_USER_0 + FLASH_USER_1 in linker script +#define PBDRV_CONFIG_BLOCK_DEVICE_FLASH_STM32_SIZE (4 * 1024) // Must match FLASH_USER_0 + FLASH_USER_1 in linker script #define PBDRV_CONFIG_BLUETOOTH (1) #define PBDRV_CONFIG_BLUETOOTH_STM32_BLUENRG (1) diff --git a/lib/pbio/platform/move_hub/platform.ld b/lib/pbio/platform/move_hub/platform.ld index a9f32e775..f167bd2f6 100644 --- a/lib/pbio/platform/move_hub/platform.ld +++ b/lib/pbio/platform/move_hub/platform.ld @@ -14,9 +14,9 @@ MEMORY in FLASH_FIRMWARE and FLASH_USER_0 add up to 0*/ FLASH_BOOTLOADER (rx) : ORIGIN = 0x08000000, LENGTH = 20K /* The firmware. Installed via BLE bootloader. */ - FLASH_FIRMWARE (rx) : ORIGIN = 0x08005000, LENGTH = 105K + FLASH_FIRMWARE (rx) : ORIGIN = 0x08005000, LENGTH = 104K /* User data written by the firmware during shutdown. */ - FLASH_USER_0 (rx) : ORIGIN = 0x0801F400, LENGTH = 1K + FLASH_USER_0 (rx) : ORIGIN = 0x0801F000, LENGTH = 2K /* As above, but this part is not counted by bootloader checksum. */ FLASH_USER_1 (rx) : ORIGIN = 0x0801F800, LENGTH = 2K RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 16K diff --git a/pybricks/tools/pb_module_task.c b/pybricks/tools/pb_module_task.c index 140b6889b..a7dad7cdb 100644 --- a/pybricks/tools/pb_module_task.c +++ b/pybricks/tools/pb_module_task.c @@ -6,6 +6,7 @@ #if PYBRICKS_PY_TOOLS #include "py/builtin.h" +#include "py/gc.h" #include "py/mphal.h" #include "py/objmodule.h" #include "py/runtime.h" @@ -14,16 +15,8 @@ #include #include -// Implementation of wait that always blocks. Needed for system runloop code -// to briefly wait inside runloop. -STATIC mp_obj_t pb_module_task_run(mp_obj_t time_in) { - mp_int_t time = pb_obj_get_int(time_in); - if (time > 0) { - mp_hal_delay_ms(time); - } - return mp_const_none; -} -MP_DEFINE_CONST_FUN_OBJ_1(pb_module_task_run_obj, pb_module_task_run); +#include +#include STATIC bool _pb_module_task_run_loop_is_active; @@ -33,12 +26,206 @@ bool pb_module_task_run_loop_is_active() { void pb_module_task_init(void) { _pb_module_task_run_loop_is_active = false; + pb_type_tools_awaitable_init(); } -STATIC const mp_rom_map_elem_t task_globals_table[] = { - { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_task) }, - { MP_ROM_QSTR(MP_QSTR_run), MP_ROM_PTR(&pb_module_task_run_obj) }, +STATIC mp_obj_t pb_module_task_run(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, + PB_ARG_REQUIRED(task), + PB_ARG_DEFAULT_INT(loop_time, 10)); + + _pb_module_task_run_loop_is_active = true; + + uint32_t start_time = mp_hal_ticks_ms(); + uint32_t loop_time = pb_obj_get_positive_int(loop_time_in); + + mp_obj_iter_buf_t iter_buf; + mp_obj_t iterable = mp_getiter(task_in, &iter_buf); + + nlr_buf_t nlr; + if (nlr_push(&nlr) == 0) { + + while (mp_iternext(iterable) != MP_OBJ_STOP_ITERATION) { + + gc_collect(); + + if (loop_time == 0) { + continue; + } + + uint32_t elapsed = mp_hal_ticks_ms() - start_time; + if (elapsed < loop_time) { + mp_hal_delay_ms(loop_time - elapsed); + } + start_time += loop_time; + } + + nlr_pop(); + _pb_module_task_run_loop_is_active = false; + } else { + _pb_module_task_run_loop_is_active = false; + nlr_jump(nlr.ret_val); + } + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_module_task_run_obj, 1, pb_module_task_run); + +/** + * State of one coroutine task handled by all/race. + */ +typedef struct { + mp_obj_t arg; + mp_obj_t return_val; + mp_obj_iter_buf_t iter_buf; + mp_obj_t iterable; + bool done; +} pb_module_task_progress_t; + +typedef struct { + mp_obj_base_t base; + /** + * The number of tasks managed by this all/race awaitable. + */ + size_t num_tasks; + /** + * The number of tasks that must finish before the collection is done. + */ + size_t num_tasks_required; + /** + * The tasks managed by this all or race awaitable. + */ + pb_module_task_progress_t *tasks; +} pb_module_task_collection_obj_t; + +// Cancel all tasks by calling their close methods. +STATIC mp_obj_t pb_module_task_collection_close(mp_obj_t self_in) { + pb_module_task_collection_obj_t *self = MP_OBJ_TO_PTR(self_in); + for (size_t i = 0; i < self->num_tasks; i++) { + pb_module_task_progress_t *task = &self->tasks[i]; + + // Task already complete, no need to cancel. + if (task->done) { + continue; + } + + // Find close() on coroutine object, then call it. + mp_obj_t dest[2]; + mp_load_method_maybe(task->arg, MP_QSTR_close, dest); + if (dest[0] != MP_OBJ_NULL) { + mp_call_method_n_kw(0, 0, dest); + } + } + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_module_task_collection_close_obj, pb_module_task_collection_close); +STATIC mp_obj_t pb_module_task_collection_iternext(mp_obj_t self_in) { + pb_module_task_collection_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // Do one iteration of each task. + nlr_buf_t nlr; + if (nlr_push(&nlr) == 0) { + + size_t done_total = 0; + + for (size_t i = 0; i < self->num_tasks; i++) { + + pb_module_task_progress_t *task = &self->tasks[i]; + + // This task already complete, skip. + if (task->done) { + done_total++; + continue; + } + + // Do one task iteration. + mp_obj_t result = mp_iternext(task->iterable); + + // Not done yet, try next time. + if (result == mp_const_none) { + continue; + } + + // Task is done, save return value. + if (result == MP_OBJ_STOP_ITERATION) { + if (MP_STATE_THREAD(stop_iteration_arg) != MP_OBJ_NULL) { + task->return_val = MP_STATE_THREAD(stop_iteration_arg); + } + task->done = true; + done_total++; + + // If enough tasks are done, don't finish this round. This way, + // in race(), there is only one winner. + if (done_total >= self->num_tasks_required) { + // Cancel everything else. + pb_module_task_collection_close(self_in); + break; + } + } + } + // Successfully did one iteration of all tasks. + nlr_pop(); + + // If collection not done yet, indicate that it should run again. + if (done_total < self->num_tasks_required) { + return mp_const_none; + } + + // Otherwise raise StopIteration with return values. + mp_obj_t *ret = m_new(mp_obj_t, self->num_tasks); + for (size_t i = 0; i < self->num_tasks; i++) { + ret[i] = self->tasks[i].return_val; + } + return mp_make_stop_iteration(mp_obj_new_list(self->num_tasks, ret)); + } else { + // On failure of one task, cancel others, then stop iterating collection. + pb_module_task_collection_close(self_in); + nlr_jump(nlr.ret_val); + } +} + +STATIC const mp_rom_map_elem_t pb_module_task_collection_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_module_task_collection_close_obj) }, +}; +MP_DEFINE_CONST_DICT(pb_module_task_collection_locals_dict, pb_module_task_collection_locals_dict_table); + +extern const mp_obj_type_t pb_module_task_all; + +STATIC mp_obj_t pb_module_task_collection_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { + + pb_module_task_collection_obj_t *self = mp_obj_malloc(pb_module_task_collection_obj_t, type); + self->num_tasks = n_args; + self->num_tasks_required = type == &pb_module_task_all ? n_args : 1; + self->tasks = m_new(pb_module_task_progress_t, n_args); + for (size_t i = 0; i < n_args; i++) { + pb_module_task_progress_t *task = &self->tasks[i]; + task->arg = args[i]; + task->return_val = mp_const_none; + task->iterable = mp_getiter(args[i], &task->iter_buf); + task->done = false; + } + return MP_OBJ_FROM_PTR(self); +} + +MP_DEFINE_CONST_OBJ_TYPE(pb_module_task_all, + MP_QSTR_all, + MP_TYPE_FLAG_ITER_IS_ITERNEXT, + iter, pb_module_task_collection_iternext, + make_new, pb_module_task_collection_new, + locals_dict, &pb_module_task_collection_locals_dict); + +MP_DEFINE_CONST_OBJ_TYPE(pb_module_task_race, + MP_QSTR_race, + MP_TYPE_FLAG_ITER_IS_ITERNEXT, + iter, pb_module_task_collection_iternext, + make_new, pb_module_task_collection_new, + locals_dict, &pb_module_task_collection_locals_dict); + +STATIC const mp_rom_map_elem_t task_globals_table[] = { + { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_task) }, + { MP_ROM_QSTR(MP_QSTR_run), MP_ROM_PTR(&pb_module_task_run_obj) }, + { MP_ROM_QSTR(MP_QSTR_all), MP_ROM_PTR(&pb_module_task_all) }, + { MP_ROM_QSTR(MP_QSTR_race), MP_ROM_PTR(&pb_module_task_race) }, }; STATIC MP_DEFINE_CONST_DICT(pb_module_task_globals, task_globals_table); From 7ae0dcf103f80fef6648d284c4e1b997c7cb3b25 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 5 May 2023 15:08:54 +0200 Subject: [PATCH 28/59] pybricks/tools/pb_type_awaitable: Refactor. This restores the ability to correctly cancel parallel awaitables that belong to the same resource. This was removed while unifying the awaitable types in previous commits. In practice, this means that if a motor operation is awaited while another motor operation is already running, the older operation will be cancelled and the new one will start. Flags are available to control the cancel behavior so the same code can be shared with awaitables that don't need linking like wait. --- pybricks/common.h | 4 +- pybricks/common/pb_type_motor.c | 70 ++++++----- pybricks/common/pb_type_speaker.c | 25 +++- pybricks/pybricks.c | 2 + pybricks/robotics/pb_type_drivebase.c | 20 +++- pybricks/tools.h | 23 +--- pybricks/tools/pb_module_task.c | 3 +- pybricks/tools/pb_module_tools.c | 42 +++++-- pybricks/tools/pb_type_awaitable.c | 161 +++++++++++++++----------- pybricks/tools/pb_type_awaitable.h | 79 +++++++++++++ 10 files changed, 294 insertions(+), 135 deletions(-) create mode 100644 pybricks/tools/pb_type_awaitable.h diff --git a/pybricks/common.h b/pybricks/common.h index dc07a2271..4c517295f 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -20,6 +20,8 @@ #include #include +#include +#include void pb_package_pybricks_init(bool import_all); void pb_package_pybricks_deinit(void); @@ -113,6 +115,7 @@ struct _common_Motor_obj_t { mp_obj_t logger; #endif pbio_port_id_t port; + pb_type_awaitable_obj_t *first_awaitable; }; extern const mp_obj_type_t pb_type_Motor; @@ -128,7 +131,6 @@ 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); diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 8fa6652c9..3f7213c41 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -123,9 +124,35 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, self->logger = common_Logger_obj_make_new(&self->srv->log, PBIO_SERVO_LOGGER_NUM_COLS); #endif + self->first_awaitable = NULL; + return MP_OBJ_FROM_PTR(self); } +STATIC mp_obj_t common_Motor_test_completion(mp_obj_t self_in, uint32_t start_time) { + common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); + // Handle I/O exceptions like port unplugged. + if (!pbio_servo_update_loop_is_running(self->srv)) { + pb_assert(PBIO_ERROR_NO_DEV); + } + + // Get completion state. + return pbio_control_is_done(&self->srv->control) ? MP_OBJ_STOP_ITERATION : mp_const_none; +} + +STATIC void common_Motor_cancel(mp_obj_t self_in) { + common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); + // TODO: Can drop next line if moved to pbio + pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, self->max_voltage_last)); + pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_COAST)); +} + +STATIC const pb_type_awaitable_config_t motor_awaitable_config = { + .test_completion_func = common_Motor_test_completion, + .cancel_func = common_Motor_cancel, + .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE, +}; + // pybricks._common.Motor.angle STATIC mp_obj_t common_Motor_angle(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); @@ -150,7 +177,7 @@ STATIC mp_obj_t common_Motor_reset_angle(size_t n_args, const mp_obj_t *pos_args // Set the new angle pb_assert(pbio_servo_reset_angle(self->srv, reset_angle, reset_to_abs)); - + pb_type_awaitable_cancel_all(pos_args[0], motor_awaitable_config.cancel_opt, self->first_awaitable); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_reset_angle_obj, 1, common_Motor_reset_angle); @@ -175,7 +202,7 @@ STATIC mp_obj_t common_Motor_run(size_t n_args, const mp_obj_t *pos_args, mp_map mp_int_t speed = pb_obj_get_int(speed_in); pb_assert(pbio_servo_run_forever(self->srv, speed)); - + pb_type_awaitable_cancel_all(pos_args[0], motor_awaitable_config.cancel_opt, self->first_awaitable); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); @@ -184,30 +211,11 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); STATIC mp_obj_t common_Motor_hold(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_HOLD)); + pb_type_awaitable_cancel_all(self_in, motor_awaitable_config.cancel_opt, self->first_awaitable); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_hold_obj, common_Motor_hold); -STATIC mp_obj_t common_Motor_test_completion(mp_obj_t self_in) { - - common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); - - // Handle I/O exceptions like port unplugged. - if (!pbio_servo_update_loop_is_running(self->srv)) { - pb_assert(PBIO_ERROR_NO_DEV); - } - - // Get completion state. - return pbio_control_is_done(&self->srv->control) ? MP_OBJ_STOP_ITERATION : mp_const_none; -} - -STATIC void common_Motor_cancel(mp_obj_t self_in) { - common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); - // TODO: Can drop next line if moved to pbio - pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, self->max_voltage_last)); - pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_COAST)); -} - // pybricks._common.Motor.run_time STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, @@ -230,11 +238,11 @@ STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, m return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_tools_await_or_wait(pos_args[0], common_Motor_test_completion, common_Motor_cancel); + return pb_type_awaitable_await_or_block(pos_args[0], &motor_awaitable_config, self->first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_time_obj, 1, common_Motor_run_time); -STATIC mp_obj_t common_Motor_stall_test_completion(mp_obj_t self_in) { +STATIC mp_obj_t common_Motor_stall_test_completion(mp_obj_t self_in, uint32_t start_time) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); @@ -266,6 +274,12 @@ STATIC mp_obj_t common_Motor_stall_test_completion(mp_obj_t self_in) { return mp_make_stop_iteration(mp_obj_new_int(stall_angle)); } +STATIC const pb_type_awaitable_config_t motor_stalled_awaitable_config = { + .test_completion_func = common_Motor_stall_test_completion, + .cancel_func = common_Motor_cancel, + .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE, +}; + // pybricks._common.Motor.run_until_stalled STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, @@ -296,7 +310,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po pb_assert(pbio_servo_run_forever(self->srv, speed)); // Handle completion by awaiting or blocking. - return pb_type_tools_await_or_wait(pos_args[0], common_Motor_stall_test_completion, common_Motor_cancel); + return pb_type_awaitable_await_or_block(pos_args[0], &motor_stalled_awaitable_config, self->first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_until_stalled_obj, 1, common_Motor_run_until_stalled); @@ -321,7 +335,7 @@ STATIC mp_obj_t common_Motor_run_angle(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_tools_await_or_wait(pos_args[0], common_Motor_test_completion, common_Motor_cancel); + return pb_type_awaitable_await_or_block(pos_args[0], &motor_awaitable_config, self->first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_angle_obj, 1, common_Motor_run_angle); @@ -346,7 +360,7 @@ STATIC mp_obj_t common_Motor_run_target(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_tools_await_or_wait(pos_args[0], common_Motor_test_completion, common_Motor_cancel); + return pb_type_awaitable_await_or_block(pos_args[0], &motor_awaitable_config, self->first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_target_obj, 1, common_Motor_run_target); @@ -358,6 +372,7 @@ STATIC mp_obj_t common_Motor_track_target(size_t n_args, const mp_obj_t *pos_arg mp_int_t target_angle = pb_obj_get_int(target_angle_in); pb_assert(pbio_servo_track_target(self->srv, target_angle)); + pb_type_awaitable_cancel_all(pos_args[0], motor_awaitable_config.cancel_opt, self->first_awaitable); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_track_target_obj, 1, common_Motor_track_target); @@ -408,7 +423,6 @@ 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) }, diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 24b144b80..117d6c8fd 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -33,6 +34,7 @@ typedef struct { uint32_t note_duration; uint32_t beep_end_time; uint32_t release_end_time; + pb_type_awaitable_obj_t *first_awaitable; // volume in 0..100 range uint8_t volume; @@ -113,6 +115,7 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg if (!self->initialized) { self->base.type = &pb_type_Speaker; self->initialized = true; + self->first_awaitable = NULL; } // REVISIT: If a user creates two Speaker instances, this will reset the volume settings for both. @@ -120,10 +123,12 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg self->volume = 100; self->sample_attenuator = INT16_MAX; + + return MP_OBJ_FROM_PTR(self); } -STATIC mp_obj_t pb_type_Speaker_beep_test_completion(mp_obj_t self_in) { +STATIC mp_obj_t pb_type_Speaker_beep_test_completion(mp_obj_t self_in, uint32_t start_time) { pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); if (mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX) { pb_type_Speaker_stop_beep(); @@ -140,6 +145,12 @@ STATIC void pb_type_Speaker_cancel(mp_obj_t self_in) { self->notes_generator = MP_OBJ_NULL; } +STATIC const pb_type_awaitable_config_t speaker_beep_awaitable_config = { + .test_completion_func = pb_type_Speaker_beep_test_completion, + .cancel_func = pb_type_Speaker_cancel, + .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK, +}; + STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pb_type_Speaker_obj_t, self, @@ -158,7 +169,7 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp self->beep_end_time = mp_hal_ticks_ms() + (uint32_t)duration; self->release_end_time = self->beep_end_time; self->notes_generator = MP_OBJ_NULL; - return pb_type_tools_await_or_wait(pos_args[0], pb_type_Speaker_beep_test_completion, pb_type_Speaker_cancel); + return pb_type_awaitable_await_or_block(pos_args[0], &speaker_beep_awaitable_config, self->first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_beep_obj, 1, pb_type_Speaker_beep); @@ -329,7 +340,7 @@ STATIC void pb_type_Speaker_play_note(pb_type_Speaker_obj_t *self, mp_obj_t obj, self->beep_end_time = release ? time_now + 7 * duration / 8 : time_now + duration; } -STATIC mp_obj_t pb_type_Speaker_notes_test_completion(mp_obj_t self_in) { +STATIC mp_obj_t pb_type_Speaker_notes_test_completion(mp_obj_t self_in, uint32_t start_time) { pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); bool release_done = mp_hal_ticks_ms() - self->release_end_time < (uint32_t)INT32_MAX; @@ -357,6 +368,12 @@ STATIC mp_obj_t pb_type_Speaker_notes_test_completion(mp_obj_t self_in) { return mp_const_none; } +STATIC const pb_type_awaitable_config_t speaker_notes_awaitable_config = { + .test_completion_func = pb_type_Speaker_notes_test_completion, + .cancel_func = pb_type_Speaker_cancel, + .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK, +}; + STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pb_type_Speaker_obj_t, self, @@ -367,7 +384,7 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar self->note_duration = 4 * 60 * 1000 / pb_obj_get_int(tempo_in); self->beep_end_time = mp_hal_ticks_ms(); self->release_end_time = self->beep_end_time; - return pb_type_tools_await_or_wait(pos_args[0], pb_type_Speaker_notes_test_completion, pb_type_Speaker_cancel); + return pb_type_awaitable_await_or_block(pos_args[0], &speaker_notes_awaitable_config, self->first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_play_notes_obj, 1, pb_type_Speaker_play_notes); diff --git a/pybricks/pybricks.c b/pybricks/pybricks.c index 2132a946e..61a84cb08 100644 --- a/pybricks/pybricks.c +++ b/pybricks/pybricks.c @@ -105,6 +105,7 @@ void pb_package_pybricks_init(bool import_all) { // Initialize the package. pb_type_Color_reset(); pb_module_task_init(); + pb_module_tools_init(); // Import all if requested. if (import_all) { pb_package_import_all(); @@ -122,6 +123,7 @@ void pb_package_pybricks_init(bool import_all) { void pb_package_pybricks_init(bool import_all) { pb_type_Color_reset(); pb_module_task_init(); + pb_module_tools_init(); } #endif // PYBRICKS_OPT_COMPILER diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 8a2595ece..fe8212970 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -34,6 +35,7 @@ struct _pb_type_DriveBase_obj_t { mp_obj_t heading_control; mp_obj_t distance_control; #endif + pb_type_awaitable_obj_t *first_awaitable; }; // pybricks.robotics.DriveBase.reset @@ -83,10 +85,12 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a // Reset drivebase state pb_type_DriveBase_reset(MP_OBJ_FROM_PTR(self)); + self->first_awaitable = NULL; + return MP_OBJ_FROM_PTR(self); } -STATIC mp_obj_t pb_type_DriveBase_test_completion(mp_obj_t self_in) { +STATIC mp_obj_t pb_type_DriveBase_test_completion(mp_obj_t self_in, uint32_t start_time) { pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); @@ -104,6 +108,12 @@ STATIC void pb_type_DriveBase_cancel(mp_obj_t self_in) { pb_assert(pbio_drivebase_stop(self->db, PBIO_CONTROL_ON_COMPLETION_COAST)); } +STATIC const pb_type_awaitable_config_t drivebase_awaitable_config = { + .test_completion_func = pb_type_DriveBase_test_completion, + .cancel_func = pb_type_DriveBase_cancel, + .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE, +}; + // pybricks.robotics.DriveBase.straight STATIC mp_obj_t pb_type_DriveBase_straight(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, @@ -122,7 +132,7 @@ STATIC mp_obj_t pb_type_DriveBase_straight(size_t n_args, const mp_obj_t *pos_ar return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_tools_await_or_wait(pos_args[0], pb_type_DriveBase_test_completion, pb_type_DriveBase_cancel); + return pb_type_awaitable_await_or_block(pos_args[0], &drivebase_awaitable_config, self->first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_straight_obj, 1, pb_type_DriveBase_straight); @@ -145,7 +155,7 @@ STATIC mp_obj_t pb_type_DriveBase_turn(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_tools_await_or_wait(pos_args[0], pb_type_DriveBase_test_completion, pb_type_DriveBase_cancel); + return pb_type_awaitable_await_or_block(pos_args[0], &drivebase_awaitable_config, self->first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_turn_obj, 1, pb_type_DriveBase_turn); @@ -169,7 +179,7 @@ STATIC mp_obj_t pb_type_DriveBase_curve(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_tools_await_or_wait(pos_args[0], pb_type_DriveBase_test_completion, pb_type_DriveBase_cancel); + return pb_type_awaitable_await_or_block(pos_args[0], &drivebase_awaitable_config, self->first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_curve_obj, 1, pb_type_DriveBase_curve); @@ -185,7 +195,7 @@ STATIC mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, mp_int_t turn_rate = pb_obj_get_int(turn_rate_in); pb_assert(pbio_drivebase_drive_forever(self->db, speed, turn_rate)); - + pb_type_awaitable_cancel_all(pos_args[0], drivebase_awaitable_config.cancel_opt, self->first_awaitable); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_drive_obj, 1, pb_type_DriveBase_drive); diff --git a/pybricks/tools.h b/pybricks/tools.h index c2eed8bdd..abb4d7b95 100644 --- a/pybricks/tools.h +++ b/pybricks/tools.h @@ -11,32 +11,13 @@ #include "py/obj.h" extern const mp_obj_module_t pb_module_task; + void pb_module_task_init(void); -/** - * Tests if awaitable operation is complete. Returns MP_OBJ_STOP_ITERATION if - * done, possibly with return argument, else mp_const_none. - * - * On completion, this function is expected to close/stop hardware - * operations as needed (hold a motor, etc.). This is not the same as cancel - * below, which always stops the relevant hardware (i.e. always coast). - */ -typedef mp_obj_t (*pb_awaitable_test_completion_t)(mp_obj_t obj); - -/** - * Called on cancel/close. Used to stop hardware operation in unhandled - * conditions. - */ -typedef void (*pb_awaitable_cancel_t)(mp_obj_t obj); +void pb_module_tools_init(void); bool pb_module_task_run_loop_is_active(); -void pb_type_tools_awaitable_init(void); - -mp_obj_t pb_type_tools_await_or_wait(mp_obj_t obj, pb_awaitable_test_completion_t test_completion, pb_awaitable_cancel_t cancel); - -mp_obj_t pb_type_tools_await_time(mp_obj_t duration_in); - extern const mp_obj_type_t pb_type_StopWatch; #endif // PYBRICKS_PY_TOOLS diff --git a/pybricks/tools/pb_module_task.c b/pybricks/tools/pb_module_task.c index a7dad7cdb..633f215b9 100644 --- a/pybricks/tools/pb_module_task.c +++ b/pybricks/tools/pb_module_task.c @@ -26,7 +26,6 @@ bool pb_module_task_run_loop_is_active() { void pb_module_task_init(void) { _pb_module_task_run_loop_is_active = false; - pb_type_tools_awaitable_init(); } STATIC mp_obj_t pb_module_task_run(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -178,7 +177,7 @@ STATIC mp_obj_t pb_module_task_collection_iternext(mp_obj_t self_in) { } return mp_make_stop_iteration(mp_obj_new_list(self->num_tasks, ret)); } else { - // On failure of one task, cancel others, then stop iterating collection. + // On failure of one task, cancel others, then stop iterating collection by re-raising. pb_module_task_collection_close(self_in); nlr_jump(nlr.ret_val); } diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 4420fb6ce..fd9ad2d60 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -10,6 +10,8 @@ #include "py/objmodule.h" #include "py/runtime.h" +#include + #include #include #include @@ -19,21 +21,45 @@ #include #include +// The awaitable for the wait() function has no object associated with +// it (unlike e.g. a motor), so we make a starting point here. +STATIC pb_type_awaitable_obj_t *first_awaitable; + +void pb_module_tools_init(void) { + first_awaitable = NULL; +} + +STATIC mp_obj_t pb_tools_wait_test_completion(mp_obj_t obj, uint32_t start_time) { + // obj was validated to be small int, so we can do a cheap comparison here. + return mp_hal_ticks_ms() - start_time >= (uint32_t)MP_OBJ_SMALL_INT_VALUE(obj) ? MP_OBJ_STOP_ITERATION : mp_const_none; +} + +STATIC const pb_type_awaitable_config_t wait_awaitable_config = { + .test_completion_func = pb_tools_wait_test_completion, + .cancel_func = NULL, + .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_NONE, +}; + STATIC mp_obj_t tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, PB_ARG_REQUIRED(time)); - // Inside run loop, return generator to await time. - if (pb_module_task_run_loop_is_active()) { - return pb_type_tools_await_time(time_in); - } - // Outside of run loop, just block to wait. mp_int_t time = pb_obj_get_int(time_in); - if (time > 0) { - mp_hal_delay_ms(time); + + // Do blocking wait outside run loop. + if (!pb_module_task_run_loop_is_active()) { + if (time > 0) { + mp_hal_delay_ms(time); + } + return mp_const_none; } - return mp_const_none; + + // Require that duration is nonnegative small int. This makes it cheaper to + // completion state in iteration loop. + time = pbio_int_math_bind(time, 0, INT32_MAX >> 2); + + return pb_type_awaitable_await_or_block(MP_OBJ_NEW_SMALL_INT(time), &wait_awaitable_config, first_awaitable); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(tools_wait_obj, 0, tools_wait); diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index dfb69d0a6..f2c2e1624 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -1,5 +1,5 @@ // SPDX-License-Identifier: MIT -// Copyright (c) 2023 The Pybricks Authors +// Copyright (c) 2022-2023 The Pybricks Authors #include "py/mpconfig.h" @@ -9,39 +9,36 @@ #include "py/mpstate.h" #include +#include -/** - * A generator-like type for waiting on some operation to complete. - */ -typedef struct _pb_type_tools_awaitable_obj_t pb_type_tools_awaitable_obj_t; - -struct _pb_type_tools_awaitable_obj_t { +struct _pb_type_awaitable_obj_t { mp_obj_base_t base; /** * Object associated with this awaitable, such as the motor we wait on. */ mp_obj_t obj; /** - * End time. Only used for simple waits with no associated object. + * Start time. Gets passed to completion test to allow for graceful timeout. */ - uint32_t end_time; + uint32_t start_time; /** - * Tests if operation is complete. + * Tests if operation is complete. Gets reset to NULL on completion, + * which means that it can be used again. */ - pb_awaitable_test_completion_t test_completion; + pb_type_awaitable_test_completion_t test_completion; /** * Called on cancellation. */ - pb_awaitable_cancel_t cancel; + pb_type_awaitable_cancel_t cancel; /** * Linked list of awaitables. */ - pb_type_tools_awaitable_obj_t *next_awaitable; + pb_type_awaitable_obj_t *next_awaitable; }; // close() cancels the awaitable. -STATIC mp_obj_t pb_type_tools_awaitable_close(mp_obj_t self_in) { - pb_type_tools_awaitable_obj_t *self = MP_OBJ_TO_PTR(self_in); +STATIC mp_obj_t pb_type_awaitable_close(mp_obj_t self_in) { + pb_type_awaitable_obj_t *self = MP_OBJ_TO_PTR(self_in); self->test_completion = NULL; // Handle optional clean up/cancelling of hardware operation. if (self->cancel) { @@ -49,10 +46,10 @@ STATIC mp_obj_t pb_type_tools_awaitable_close(mp_obj_t self_in) { } return mp_const_none; } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_tools_awaitable_close_obj, pb_type_tools_awaitable_close); +STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_awaitable_close_obj, pb_type_awaitable_close); -STATIC mp_obj_t pb_type_tools_awaitable_iternext(mp_obj_t self_in) { - pb_type_tools_awaitable_obj_t *self = MP_OBJ_TO_PTR(self_in); +STATIC mp_obj_t pb_type_awaitable_iternext(mp_obj_t self_in) { + pb_type_awaitable_obj_t *self = MP_OBJ_TO_PTR(self_in); // If completed callback was unset, then we are done. if (self->test_completion == NULL) { @@ -60,7 +57,7 @@ STATIC mp_obj_t pb_type_tools_awaitable_iternext(mp_obj_t self_in) { } // Test completion status. - mp_obj_t completion = self->test_completion(self->obj); + mp_obj_t completion = self->test_completion(self->obj, self->start_time); // none means keep going, everything else means done so finalize. if (completion != mp_const_none) { @@ -69,78 +66,110 @@ STATIC mp_obj_t pb_type_tools_awaitable_iternext(mp_obj_t self_in) { return completion; } -STATIC const mp_rom_map_elem_t pb_type_tools_awaitable_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_tools_awaitable_close_obj) }, +STATIC const mp_rom_map_elem_t pb_type_awaitable_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_awaitable_close_obj) }, }; -MP_DEFINE_CONST_DICT(pb_type_tools_awaitable_locals_dict, pb_type_tools_awaitable_locals_dict_table); +MP_DEFINE_CONST_DICT(pb_type_awaitable_locals_dict, pb_type_awaitable_locals_dict_table); // This is a partial implementation of the Python generator type. It is missing // send(value) and throw(type[, value[, traceback]]) -MP_DEFINE_CONST_OBJ_TYPE(pb_type_tools_awaitable, +MP_DEFINE_CONST_OBJ_TYPE(pb_type_awaitable, MP_QSTR_wait, MP_TYPE_FLAG_ITER_IS_ITERNEXT, - iter, pb_type_tools_awaitable_iternext, - locals_dict, &pb_type_tools_awaitable_locals_dict); + iter, pb_type_awaitable_iternext, + locals_dict, &pb_type_awaitable_locals_dict); -// Statically allocated awaitable from which all others can be found. -STATIC pb_type_tools_awaitable_obj_t first_awaitable; +STATIC pb_type_awaitable_obj_t *pb_type_awaitable_get(pb_type_awaitable_obj_t *first_awaitable) { -// Reset first awaitable on initializing MicroPython. -void pb_type_tools_awaitable_init(void) { - first_awaitable.base.type = &pb_type_tools_awaitable; - first_awaitable.test_completion = NULL; - first_awaitable.next_awaitable = MP_OBJ_NULL; -} - -STATIC pb_type_tools_awaitable_obj_t *pb_type_tools_awaitable_get(void) { + // If the first awaitable was not yet created, do so now. + if (!first_awaitable) { + first_awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); + first_awaitable->test_completion = NULL; + first_awaitable->next_awaitable = NULL; + } - // Find next available awaitable. - pb_type_tools_awaitable_obj_t *awaitable = &first_awaitable; - while (awaitable->test_completion != NULL && awaitable->next_awaitable != MP_OBJ_NULL) { + // Find next available awaitable that exists and is not used. + pb_type_awaitable_obj_t *awaitable = first_awaitable; + while (awaitable->next_awaitable && awaitable->test_completion) { awaitable = awaitable->next_awaitable; } - // If the last known awaitable is still in use, allocate another. - if (awaitable->test_completion != NULL) { + // Above loop stops if a) there is no next awaitable or b) the current + // awaitable is not in use. Only case a) requires allocating another. + if (!awaitable->next_awaitable) { // Attach to the previous one. - awaitable->next_awaitable = mp_obj_malloc(pb_type_tools_awaitable_obj_t, &pb_type_tools_awaitable); + awaitable->next_awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); // Initialize the new awaitable. awaitable = awaitable->next_awaitable; - awaitable->next_awaitable = MP_OBJ_NULL; + awaitable->test_completion = NULL; + awaitable->next_awaitable = NULL; } return awaitable; } -// Get an available awaitable, add callbacks, and return as object so user can await it. -STATIC mp_obj_t pb_type_tools_awaitable_new(mp_obj_t obj, pb_awaitable_test_completion_t test_completion, pb_awaitable_cancel_t cancel) { - pb_type_tools_awaitable_obj_t *awaitable = pb_type_tools_awaitable_get(); - awaitable->obj = obj; - awaitable->test_completion = test_completion; - awaitable->cancel = cancel; - return MP_OBJ_FROM_PTR(awaitable); -} +/** + * Cancels all awaitables associated with an object. + * + * This is normally used by the function that makes a new awaitable, but it can + * also be called independently to cancel without starting a new awaitable. + * + * @param [in] obj The object. + * @param [in] cancel_opt Cancellation type. + * @param [in] first_awaitable The first awaitable in the linked list of awaitables from @p obj. + */ +void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_cancel_opt_t cancel_opt, pb_type_awaitable_obj_t *first_awaitable) { -STATIC mp_obj_t pb_tools_wait_test_completion(mp_obj_t obj) { - pb_type_tools_awaitable_obj_t *awaitable = MP_OBJ_TO_PTR(obj); - return (mp_hal_ticks_ms() - awaitable->end_time) < (uint32_t)INT32_MAX ? MP_OBJ_STOP_ITERATION : mp_const_none; + // Exit if nothing to do. + if (cancel_opt == PB_TYPE_AWAITABLE_CANCEL_NONE || !first_awaitable) { + return; + } + + pb_type_awaitable_obj_t *awaitable = first_awaitable; + while (awaitable->next_awaitable) { + // Don't cancel if already done. + if (!awaitable->test_completion) { + continue; + } + // Cancel hardware operation if requested and available. + if (cancel_opt & PB_TYPE_AWAITABLE_CANCEL_CALLBACK && awaitable->cancel) { + awaitable->cancel(awaitable->obj); + } + // Set awaitable to done in order to cancel it gracefully. + if (cancel_opt & PB_TYPE_AWAITABLE_CANCEL_AWAITABLE) { + awaitable->test_completion = NULL; + } + awaitable = awaitable->next_awaitable; + } } -mp_obj_t pb_type_tools_await_time(mp_obj_t duration_in) { - mp_int_t duration = mp_obj_get_int(duration_in); - pb_type_tools_awaitable_obj_t *awaitable = pb_type_tools_awaitable_get(); - awaitable->obj = MP_OBJ_FROM_PTR(awaitable); - awaitable->test_completion = duration > 0 ? pb_tools_wait_test_completion : NULL; - awaitable->end_time = mp_hal_ticks_ms() + (uint32_t)duration; - awaitable->cancel = NULL; +// Get an available awaitable, add callbacks, and return as object so user can await it. +STATIC mp_obj_t pb_type_awaitable_new(mp_obj_t obj, const pb_type_awaitable_config_t *config, pb_type_awaitable_obj_t *first_awaitable) { + + // First cancel linked awaitables if requested. + pb_type_awaitable_cancel_all(obj, config->cancel_opt, first_awaitable); + + // Get and initialize awaitable. + pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(first_awaitable); + awaitable->obj = obj; + awaitable->test_completion = config->test_completion_func; + awaitable->cancel = config->cancel_func; + awaitable->start_time = mp_hal_ticks_ms(); return MP_OBJ_FROM_PTR(awaitable); } -// Helper functions to simplify the final part of most methods that can be -// either blocking or not, like motor methods. -mp_obj_t pb_type_tools_await_or_wait(mp_obj_t obj, pb_awaitable_test_completion_t test_completion, pb_awaitable_cancel_t cancel) { +/** + * Get a new awaitable in async mode or block and wait for it to complete in sync mode. + * + * Automatically cancels any previous awaitables associated with the object if requested. + * + * @param [in] obj The object whose method we want to wait for completion. + * @param [in] config Configuration for the awaitable. + * @param [in] first_awaitable The first awaitable in the linked list of awaitables from @p obj. + */ +mp_obj_t pb_type_awaitable_await_or_block(mp_obj_t obj, const pb_type_awaitable_config_t *config, pb_type_awaitable_obj_t *first_awaitable) { // Make an awaitable object for the given operation. - mp_obj_t generator = pb_type_tools_awaitable_new(obj, test_completion, cancel); + mp_obj_t generator = pb_type_awaitable_new(obj, config, first_awaitable); // Within run loop, just return the generator that user program will iterate. if (pb_module_task_run_loop_is_active()) { @@ -151,14 +180,14 @@ mp_obj_t pb_type_tools_await_or_wait(mp_obj_t obj, pb_awaitable_test_completion_ nlr_buf_t nlr; mp_obj_t ret = MP_OBJ_NULL; if (nlr_push(&nlr) == 0) { - while (pb_type_tools_awaitable_iternext(generator) == mp_const_none) { + while (pb_type_awaitable_iternext(generator) == mp_const_none) { mp_hal_delay_ms(5); } ret = MP_STATE_THREAD(stop_iteration_arg); nlr_pop(); } else { // Cancel the operation if an exception was raised. - pb_type_tools_awaitable_obj_t *self = MP_OBJ_TO_PTR(generator); + pb_type_awaitable_obj_t *self = MP_OBJ_TO_PTR(generator); if (self->cancel) { self->cancel(self->obj); } diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h new file mode 100644 index 000000000..281db6ad8 --- /dev/null +++ b/pybricks/tools/pb_type_awaitable.h @@ -0,0 +1,79 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2023 The Pybricks Authors + +#ifndef PYBRICKS_INCLUDED_PYBRICKS_TOOLS_AWAITABLE_H +#define PYBRICKS_INCLUDED_PYBRICKS_TOOLS_AWAITABLE_H + +#include "py/mpconfig.h" + +#if PYBRICKS_PY_TOOLS + +#include "py/obj.h" + +/** + * Options for canceling an awaitable. + */ +typedef enum _pb_type_awaitable_cancel_opt_t { + /** Do nothing to linked awaitables. */ + PB_TYPE_AWAITABLE_CANCEL_NONE = 0, + /** + * Makes all linked awaitables end gracefully. Can be used if awaitables + * running in parallel are using the same resources. This way, the newly + * started operation "wins" and everything else is cancelled. + */ + PB_TYPE_AWAITABLE_CANCEL_AWAITABLE = 1 << 1, + /** + * Calls the cancel function for each linked awaitable that is not already + * done. Only used to close hardware resources that aren't already cleaned + * up by lower level drivers. + */ + PB_TYPE_AWAITABLE_CANCEL_CALLBACK = 1 << 2, +} pb_type_awaitable_cancel_opt_t; + +/** + * A generator-like type for waiting on some operation to complete. + */ +typedef struct _pb_type_awaitable_obj_t pb_type_awaitable_obj_t; + +/** + * Tests if awaitable operation is complete. Returns MP_OBJ_STOP_ITERATION if + * done, possibly with return argument, else mp_const_none. + * + * On completion, this function is expected to close/stop hardware + * operations as needed (hold a motor, etc.). This is not the same as cancel + * below, which always stops the relevant hardware (i.e. always coast). + */ +typedef mp_obj_t (*pb_type_awaitable_test_completion_t)(mp_obj_t obj, uint32_t start_time); + +/** + * Called on cancel/close. Used to stop hardware operation in unhandled + * conditions. + */ +typedef void (*pb_type_awaitable_cancel_t)(mp_obj_t obj); + +/** + * Constant configuration of functions and settings that govern the behavior + * of an awaitable associated with a particular method or function. + */ +typedef struct _pb_type_awaitable_config_t { + /** + * Tests if operation is complete. + */ + pb_type_awaitable_test_completion_t test_completion_func; + /** + * Called on cancellation. + */ + pb_type_awaitable_cancel_t cancel_func; + /** + * Cancellation options. + */ + pb_type_awaitable_cancel_opt_t cancel_opt; +} pb_type_awaitable_config_t; + +void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_cancel_opt_t cancel_opt, pb_type_awaitable_obj_t *first_awaitable); + +mp_obj_t pb_type_awaitable_await_or_block(mp_obj_t obj, const pb_type_awaitable_config_t *config, pb_type_awaitable_obj_t *first_awaitable); + +#endif // PYBRICKS_PY_TOOLS + +#endif // PYBRICKS_INCLUDED_PYBRICKS_TOOLS_AWAITABLE_H From e11f4fef1cdd0870fdaab34fa592ae3e1afdbd8a Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Sat, 6 May 2023 10:08:01 +0200 Subject: [PATCH 29/59] pybricks.tools: Task is a type. all() and race() _create_ objects, so in this exploration we treat them as such. --- bricks/_common/sources.mk | 2 +- pybricks/pybricks.c | 2 - pybricks/tools.h | 8 +- pybricks/tools/pb_module_task.c | 236 ------------------------- pybricks/tools/pb_module_tools.c | 93 +++++++--- pybricks/tools/pb_type_awaitable.c | 2 +- pybricks/tools/pb_type_task.c | 164 +++++++++++++++++ tests/virtualhub/multitasking/basic.py | 12 +- tests/virtualhub/multitasking/motor.py | 6 +- 9 files changed, 251 insertions(+), 274 deletions(-) delete mode 100644 pybricks/tools/pb_module_task.c create mode 100644 pybricks/tools/pb_type_task.c diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index 76fedcd7f..5cc2a4a8c 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -96,11 +96,11 @@ PYBRICKS_PYBRICKS_SRC_C = $(addprefix pybricks/,\ robotics/pb_module_robotics.c \ robotics/pb_type_drivebase.c \ robotics/pb_type_spikebase.c \ - tools/pb_module_task.c \ tools/pb_module_tools.c \ tools/pb_type_awaitable.c \ tools/pb_type_matrix.c \ tools/pb_type_stopwatch.c \ + tools/pb_type_task.c \ util_mp/pb_obj_helper.c \ util_mp/pb_type_enum.c \ util_pb/pb_color_map.c \ diff --git a/pybricks/pybricks.c b/pybricks/pybricks.c index 61a84cb08..3ab87eab3 100644 --- a/pybricks/pybricks.c +++ b/pybricks/pybricks.c @@ -104,7 +104,6 @@ void pb_package_pybricks_init(bool import_all) { if (nlr_push(&nlr) == 0) { // Initialize the package. pb_type_Color_reset(); - pb_module_task_init(); pb_module_tools_init(); // Import all if requested. if (import_all) { @@ -122,7 +121,6 @@ void pb_package_pybricks_init(bool import_all) { // exceptions as it is only called before executing anything else. void pb_package_pybricks_init(bool import_all) { pb_type_Color_reset(); - pb_module_task_init(); pb_module_tools_init(); } #endif // PYBRICKS_OPT_COMPILER diff --git a/pybricks/tools.h b/pybricks/tools.h index abb4d7b95..5ecc2ef58 100644 --- a/pybricks/tools.h +++ b/pybricks/tools.h @@ -10,16 +10,14 @@ #include "py/obj.h" -extern const mp_obj_module_t pb_module_task; - -void pb_module_task_init(void); - void pb_module_tools_init(void); -bool pb_module_task_run_loop_is_active(); +bool pb_module_tools_run_loop_is_active(void); extern const mp_obj_type_t pb_type_StopWatch; +extern const mp_obj_type_t pb_type_Task; + #endif // PYBRICKS_PY_TOOLS #endif // PYBRICKS_INCLUDED_PYBRICKS_TOOLS_H diff --git a/pybricks/tools/pb_module_task.c b/pybricks/tools/pb_module_task.c deleted file mode 100644 index 633f215b9..000000000 --- a/pybricks/tools/pb_module_task.c +++ /dev/null @@ -1,236 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2018-2020 The Pybricks Authors - -#include "py/mpconfig.h" - -#if PYBRICKS_PY_TOOLS - -#include "py/builtin.h" -#include "py/gc.h" -#include "py/mphal.h" -#include "py/objmodule.h" -#include "py/runtime.h" - -#include -#include -#include - -#include -#include - -STATIC bool _pb_module_task_run_loop_is_active; - -bool pb_module_task_run_loop_is_active() { - return _pb_module_task_run_loop_is_active; -} - -void pb_module_task_init(void) { - _pb_module_task_run_loop_is_active = false; -} - -STATIC mp_obj_t pb_module_task_run(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, - PB_ARG_REQUIRED(task), - PB_ARG_DEFAULT_INT(loop_time, 10)); - - _pb_module_task_run_loop_is_active = true; - - uint32_t start_time = mp_hal_ticks_ms(); - uint32_t loop_time = pb_obj_get_positive_int(loop_time_in); - - mp_obj_iter_buf_t iter_buf; - mp_obj_t iterable = mp_getiter(task_in, &iter_buf); - - nlr_buf_t nlr; - if (nlr_push(&nlr) == 0) { - - while (mp_iternext(iterable) != MP_OBJ_STOP_ITERATION) { - - gc_collect(); - - if (loop_time == 0) { - continue; - } - - uint32_t elapsed = mp_hal_ticks_ms() - start_time; - if (elapsed < loop_time) { - mp_hal_delay_ms(loop_time - elapsed); - } - start_time += loop_time; - } - - nlr_pop(); - _pb_module_task_run_loop_is_active = false; - } else { - _pb_module_task_run_loop_is_active = false; - nlr_jump(nlr.ret_val); - } - return mp_const_none; -} -STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_module_task_run_obj, 1, pb_module_task_run); - -/** - * State of one coroutine task handled by all/race. - */ -typedef struct { - mp_obj_t arg; - mp_obj_t return_val; - mp_obj_iter_buf_t iter_buf; - mp_obj_t iterable; - bool done; -} pb_module_task_progress_t; - -typedef struct { - mp_obj_base_t base; - /** - * The number of tasks managed by this all/race awaitable. - */ - size_t num_tasks; - /** - * The number of tasks that must finish before the collection is done. - */ - size_t num_tasks_required; - /** - * The tasks managed by this all or race awaitable. - */ - pb_module_task_progress_t *tasks; -} pb_module_task_collection_obj_t; - -// Cancel all tasks by calling their close methods. -STATIC mp_obj_t pb_module_task_collection_close(mp_obj_t self_in) { - pb_module_task_collection_obj_t *self = MP_OBJ_TO_PTR(self_in); - for (size_t i = 0; i < self->num_tasks; i++) { - pb_module_task_progress_t *task = &self->tasks[i]; - - // Task already complete, no need to cancel. - if (task->done) { - continue; - } - - // Find close() on coroutine object, then call it. - mp_obj_t dest[2]; - mp_load_method_maybe(task->arg, MP_QSTR_close, dest); - if (dest[0] != MP_OBJ_NULL) { - mp_call_method_n_kw(0, 0, dest); - } - } - return mp_const_none; -} -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_module_task_collection_close_obj, pb_module_task_collection_close); - -STATIC mp_obj_t pb_module_task_collection_iternext(mp_obj_t self_in) { - pb_module_task_collection_obj_t *self = MP_OBJ_TO_PTR(self_in); - - // Do one iteration of each task. - nlr_buf_t nlr; - if (nlr_push(&nlr) == 0) { - - size_t done_total = 0; - - for (size_t i = 0; i < self->num_tasks; i++) { - - pb_module_task_progress_t *task = &self->tasks[i]; - - // This task already complete, skip. - if (task->done) { - done_total++; - continue; - } - - // Do one task iteration. - mp_obj_t result = mp_iternext(task->iterable); - - // Not done yet, try next time. - if (result == mp_const_none) { - continue; - } - - // Task is done, save return value. - if (result == MP_OBJ_STOP_ITERATION) { - if (MP_STATE_THREAD(stop_iteration_arg) != MP_OBJ_NULL) { - task->return_val = MP_STATE_THREAD(stop_iteration_arg); - } - task->done = true; - done_total++; - - // If enough tasks are done, don't finish this round. This way, - // in race(), there is only one winner. - if (done_total >= self->num_tasks_required) { - // Cancel everything else. - pb_module_task_collection_close(self_in); - break; - } - } - } - // Successfully did one iteration of all tasks. - nlr_pop(); - - // If collection not done yet, indicate that it should run again. - if (done_total < self->num_tasks_required) { - return mp_const_none; - } - - // Otherwise raise StopIteration with return values. - mp_obj_t *ret = m_new(mp_obj_t, self->num_tasks); - for (size_t i = 0; i < self->num_tasks; i++) { - ret[i] = self->tasks[i].return_val; - } - return mp_make_stop_iteration(mp_obj_new_list(self->num_tasks, ret)); - } else { - // On failure of one task, cancel others, then stop iterating collection by re-raising. - pb_module_task_collection_close(self_in); - nlr_jump(nlr.ret_val); - } -} - -STATIC const mp_rom_map_elem_t pb_module_task_collection_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_module_task_collection_close_obj) }, -}; -MP_DEFINE_CONST_DICT(pb_module_task_collection_locals_dict, pb_module_task_collection_locals_dict_table); - -extern const mp_obj_type_t pb_module_task_all; - -STATIC mp_obj_t pb_module_task_collection_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { - - pb_module_task_collection_obj_t *self = mp_obj_malloc(pb_module_task_collection_obj_t, type); - self->num_tasks = n_args; - self->num_tasks_required = type == &pb_module_task_all ? n_args : 1; - self->tasks = m_new(pb_module_task_progress_t, n_args); - for (size_t i = 0; i < n_args; i++) { - pb_module_task_progress_t *task = &self->tasks[i]; - task->arg = args[i]; - task->return_val = mp_const_none; - task->iterable = mp_getiter(args[i], &task->iter_buf); - task->done = false; - } - return MP_OBJ_FROM_PTR(self); -} - -MP_DEFINE_CONST_OBJ_TYPE(pb_module_task_all, - MP_QSTR_all, - MP_TYPE_FLAG_ITER_IS_ITERNEXT, - iter, pb_module_task_collection_iternext, - make_new, pb_module_task_collection_new, - locals_dict, &pb_module_task_collection_locals_dict); - -MP_DEFINE_CONST_OBJ_TYPE(pb_module_task_race, - MP_QSTR_race, - MP_TYPE_FLAG_ITER_IS_ITERNEXT, - iter, pb_module_task_collection_iternext, - make_new, pb_module_task_collection_new, - locals_dict, &pb_module_task_collection_locals_dict); - -STATIC const mp_rom_map_elem_t task_globals_table[] = { - { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_task) }, - { MP_ROM_QSTR(MP_QSTR_run), MP_ROM_PTR(&pb_module_task_run_obj) }, - { MP_ROM_QSTR(MP_QSTR_all), MP_ROM_PTR(&pb_module_task_all) }, - { MP_ROM_QSTR(MP_QSTR_race), MP_ROM_PTR(&pb_module_task_race) }, -}; -STATIC MP_DEFINE_CONST_DICT(pb_module_task_globals, task_globals_table); - -const mp_obj_module_t pb_module_task = { - .base = { &mp_type_module }, - .globals = (mp_obj_dict_t *)&pb_module_task_globals, -}; - -#endif // PYBRICKS_PY_TOOLS diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index fd9ad2d60..dbff1851c 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -6,6 +6,7 @@ #if PYBRICKS_PY_TOOLS #include "py/builtin.h" +#include "py/gc.h" #include "py/mphal.h" #include "py/objmodule.h" #include "py/runtime.h" @@ -22,33 +23,29 @@ #include // The awaitable for the wait() function has no object associated with -// it (unlike e.g. a motor), so we make a starting point here. -STATIC pb_type_awaitable_obj_t *first_awaitable; +// it (unlike e.g. a motor), so we make a starting point here. This gets +// cleared when the module initializes. +STATIC pb_type_awaitable_obj_t *first_wait_awaitable; -void pb_module_tools_init(void) { - first_awaitable = NULL; -} - -STATIC mp_obj_t pb_tools_wait_test_completion(mp_obj_t obj, uint32_t start_time) { +STATIC mp_obj_t pb_module_tools_wait_test_completion(mp_obj_t obj, uint32_t start_time) { // obj was validated to be small int, so we can do a cheap comparison here. return mp_hal_ticks_ms() - start_time >= (uint32_t)MP_OBJ_SMALL_INT_VALUE(obj) ? MP_OBJ_STOP_ITERATION : mp_const_none; } -STATIC const pb_type_awaitable_config_t wait_awaitable_config = { - .test_completion_func = pb_tools_wait_test_completion, +STATIC const pb_type_awaitable_config_t pb_module_tools_wait_config = { + .test_completion_func = pb_module_tools_wait_test_completion, .cancel_func = NULL, .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_NONE, }; -STATIC mp_obj_t tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, PB_ARG_REQUIRED(time)); - mp_int_t time = pb_obj_get_int(time_in); - // Do blocking wait outside run loop. - if (!pb_module_task_run_loop_is_active()) { + // outside run loop, do blocking wait. + if (!pb_module_tools_run_loop_is_active()) { if (time > 0) { mp_hal_delay_ms(time); } @@ -56,18 +53,74 @@ STATIC mp_obj_t tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw } // Require that duration is nonnegative small int. This makes it cheaper to - // completion state in iteration loop. + // test completion state in iteration loop. time = pbio_int_math_bind(time, 0, INT32_MAX >> 2); - return pb_type_awaitable_await_or_block(MP_OBJ_NEW_SMALL_INT(time), &wait_awaitable_config, first_awaitable); + return pb_type_awaitable_await_or_block(MP_OBJ_NEW_SMALL_INT(time), &pb_module_tools_wait_config, first_wait_awaitable); +} +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_module_tools_wait_obj, 0, pb_module_tools_wait); + +// Global state of the run loop for async user programs. Gets set when run_task +// is called and cleared when it completes +STATIC bool run_loop_is_active; + +bool pb_module_tools_run_loop_is_active() { + return run_loop_is_active; +} + +// Reset global state when user program starts. +void pb_module_tools_init(void) { + first_wait_awaitable = NULL; + run_loop_is_active = false; +} + +STATIC mp_obj_t pb_module_tools_run_task(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, + PB_ARG_REQUIRED(task), + PB_ARG_DEFAULT_INT(loop_time, 10)); + + run_loop_is_active = true; + + uint32_t start_time = mp_hal_ticks_ms(); + uint32_t loop_time = pb_obj_get_positive_int(loop_time_in); + + mp_obj_iter_buf_t iter_buf; + mp_obj_t iterable = mp_getiter(task_in, &iter_buf); + + nlr_buf_t nlr; + if (nlr_push(&nlr) == 0) { + + while (mp_iternext(iterable) != MP_OBJ_STOP_ITERATION) { + + gc_collect(); + + if (loop_time == 0) { + continue; + } + + uint32_t elapsed = mp_hal_ticks_ms() - start_time; + if (elapsed < loop_time) { + mp_hal_delay_ms(loop_time - elapsed); + } + start_time += loop_time; + } + + nlr_pop(); + run_loop_is_active = false; + } else { + run_loop_is_active = false; + nlr_jump(nlr.ret_val); + } + return mp_const_none; } -STATIC MP_DEFINE_CONST_FUN_OBJ_KW(tools_wait_obj, 0, tools_wait); +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_module_tools_run_task_obj, 1, pb_module_tools_run_task); STATIC const mp_rom_map_elem_t tools_globals_table[] = { - { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_tools) }, - { MP_ROM_QSTR(MP_QSTR_wait), MP_ROM_PTR(&tools_wait_obj) }, - { MP_ROM_QSTR(MP_QSTR_StopWatch), MP_ROM_PTR(&pb_type_StopWatch) }, - { MP_ROM_QSTR(MP_QSTR_task), MP_ROM_PTR(&pb_module_task) }, + { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_tools) }, + { MP_ROM_QSTR(MP_QSTR_wait), MP_ROM_PTR(&pb_module_tools_wait_obj) }, + { MP_ROM_QSTR(MP_QSTR_run_task), MP_ROM_PTR(&pb_module_tools_run_task_obj) }, + { MP_ROM_QSTR(MP_QSTR_StopWatch), MP_ROM_PTR(&pb_type_StopWatch) }, + { MP_ROM_QSTR(MP_QSTR_Task), MP_ROM_PTR(&pb_type_Task) }, #if MICROPY_PY_BUILTINS_FLOAT { MP_ROM_QSTR(MP_QSTR_Matrix), MP_ROM_PTR(&pb_type_Matrix) }, { MP_ROM_QSTR(MP_QSTR_vector), MP_ROM_PTR(&pb_geometry_vector_obj) }, diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index f2c2e1624..9450b7411 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -172,7 +172,7 @@ mp_obj_t pb_type_awaitable_await_or_block(mp_obj_t obj, const pb_type_awaitable_ mp_obj_t generator = pb_type_awaitable_new(obj, config, first_awaitable); // Within run loop, just return the generator that user program will iterate. - if (pb_module_task_run_loop_is_active()) { + if (pb_module_tools_run_loop_is_active()) { return generator; } diff --git a/pybricks/tools/pb_type_task.c b/pybricks/tools/pb_type_task.c new file mode 100644 index 000000000..336ab8d82 --- /dev/null +++ b/pybricks/tools/pb_type_task.c @@ -0,0 +1,164 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2018-2020 The Pybricks Authors + +#include "py/mpconfig.h" + +#if PYBRICKS_PY_TOOLS + +#include "py/builtin.h" +#include "py/objmodule.h" +#include "py/runtime.h" + +#include +#include +#include + +#include +#include + +/** + * State of one coroutine task handled by Task. + */ +typedef struct { + mp_obj_t arg; + mp_obj_t return_val; + mp_obj_iter_buf_t iter_buf; + mp_obj_t iterable; + bool done; +} pb_type_Task_progress_t; + +typedef struct { + mp_obj_base_t base; + /** + * The number of tasks managed by this all/race awaitable. + */ + size_t num_tasks; + /** + * The number of tasks that must finish before the collection is done. + */ + size_t num_tasks_required; + /** + * The tasks managed by this all or race awaitable. + */ + pb_type_Task_progress_t *tasks; +} pb_type_Task_obj_t; + +// Cancel all tasks by calling their close methods. +STATIC mp_obj_t pb_type_Task_close(mp_obj_t self_in) { + pb_type_Task_obj_t *self = MP_OBJ_TO_PTR(self_in); + for (size_t i = 0; i < self->num_tasks; i++) { + pb_type_Task_progress_t *task = &self->tasks[i]; + + // Task already complete, no need to cancel. + if (task->done) { + continue; + } + + // Find close() on coroutine object, then call it. + mp_obj_t dest[2]; + mp_load_method_maybe(task->arg, MP_QSTR_close, dest); + if (dest[0] != MP_OBJ_NULL) { + mp_call_method_n_kw(0, 0, dest); + } + } + return mp_const_none; +} +STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_Task_close_obj, pb_type_Task_close); + +STATIC mp_obj_t pb_type_Task_iternext(mp_obj_t self_in) { + pb_type_Task_obj_t *self = MP_OBJ_TO_PTR(self_in); + + // Do one iteration of each task. + nlr_buf_t nlr; + if (nlr_push(&nlr) == 0) { + + size_t done_total = 0; + + for (size_t i = 0; i < self->num_tasks; i++) { + + pb_type_Task_progress_t *task = &self->tasks[i]; + + // This task already complete, skip. + if (task->done) { + done_total++; + continue; + } + + // Do one task iteration. + mp_obj_t result = mp_iternext(task->iterable); + + // Not done yet, try next time. + if (result == mp_const_none) { + continue; + } + + // Task is done, save return value. + if (result == MP_OBJ_STOP_ITERATION) { + if (MP_STATE_THREAD(stop_iteration_arg) != MP_OBJ_NULL) { + task->return_val = MP_STATE_THREAD(stop_iteration_arg); + } + task->done = true; + done_total++; + + // If enough tasks are done, don't finish this round. This way, + // in race(), there is only one winner. + if (done_total >= self->num_tasks_required) { + // Cancel everything else. + pb_type_Task_close(self_in); + break; + } + } + } + // Successfully did one iteration of all tasks. + nlr_pop(); + + // If collection not done yet, indicate that it should run again. + if (done_total < self->num_tasks_required) { + return mp_const_none; + } + + // Otherwise raise StopIteration with return values. + mp_obj_t *ret = m_new(mp_obj_t, self->num_tasks); + for (size_t i = 0; i < self->num_tasks; i++) { + ret[i] = self->tasks[i].return_val; + } + return mp_make_stop_iteration(mp_obj_new_list(self->num_tasks, ret)); + } else { + // On failure of one task, cancel others, then stop iterating collection by re-raising. + pb_type_Task_close(self_in); + nlr_jump(nlr.ret_val); + } +} + +STATIC const mp_rom_map_elem_t pb_type_Task_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_close), MP_ROM_PTR(&pb_type_Task_close_obj) }, +}; +MP_DEFINE_CONST_DICT(pb_type_Task_locals_dict, pb_type_Task_locals_dict_table); + +STATIC mp_obj_t pb_type_Task_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { + + // Whether to race until one task is done (True) or wait for all tasks (False). + bool race = n_kw == 1 && mp_obj_is_true(args[n_args - 1]); + + pb_type_Task_obj_t *self = mp_obj_malloc(pb_type_Task_obj_t, type); + self->num_tasks = n_args; + self->num_tasks_required = race ? 1 : n_args; + self->tasks = m_new(pb_type_Task_progress_t, n_args); + for (size_t i = 0; i < n_args; i++) { + pb_type_Task_progress_t *task = &self->tasks[i]; + task->arg = args[i]; + task->return_val = mp_const_none; + task->iterable = mp_getiter(args[i], &task->iter_buf); + task->done = false; + } + return MP_OBJ_FROM_PTR(self); +} + +MP_DEFINE_CONST_OBJ_TYPE(pb_type_Task, + MP_QSTR_Task, + MP_TYPE_FLAG_ITER_IS_ITERNEXT, + iter, pb_type_Task_iternext, + make_new, pb_type_Task_new, + locals_dict, &pb_type_Task_locals_dict); + +#endif // PYBRICKS_PY_TOOLS diff --git a/tests/virtualhub/multitasking/basic.py b/tests/virtualhub/multitasking/basic.py index 629b86fa0..f54d26ece 100644 --- a/tests/virtualhub/multitasking/basic.py +++ b/tests/virtualhub/multitasking/basic.py @@ -1,4 +1,4 @@ -from pybricks.tools import task +from pybricks.tools import Task, run_task class CallCounter: @@ -55,7 +55,7 @@ async def test_all_1(): task2 = return_after(2, task2_cancel) # should return [0, 1] - both allowed to complete - print(await task.all(task1, task2)) + print(await Task(task1, task2)) # neither should be canceled print(task1_cancel.count, task2_cancel.count) @@ -84,7 +84,7 @@ async def test_all_2(): # should throw RuntimeError("task2") try: - await task.all(task1, task2) + await Task(task1, task2) except Exception as e: print(type(e), e.args) @@ -114,7 +114,7 @@ async def test_race_1(): task2 = return_after(2, task2_cancel) # returns [0, None] - only first completes - print(await task.race(task1, task2)) + print(await Task(task1, task2, race=True)) # task2 should be canceled print(task1_cancel.count, task2_cancel.count) @@ -143,7 +143,7 @@ async def test_race_2(): # should throw RuntimeError("task2") try: - await task.race(task1, task2) + await Task(task1, task2, race=True) except Exception as e: print(type(e), e.args) @@ -171,4 +171,4 @@ async def main(): # run as fast as possible for CI -task.run(main(), loop_time=0) +run_task(main(), loop_time=0) diff --git a/tests/virtualhub/multitasking/motor.py b/tests/virtualhub/multitasking/motor.py index 6568382e2..c866ac959 100644 --- a/tests/virtualhub/multitasking/motor.py +++ b/tests/virtualhub/multitasking/motor.py @@ -1,7 +1,7 @@ from pybricks.pupdevices import Motor from pybricks.parameters import Port, Direction from pybricks.robotics import DriveBase -from pybricks.tools import wait, task +from pybricks.tools import wait, Task, run_task # Initialize devices as usual. left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) @@ -37,10 +37,10 @@ async def hello(name): # This is the main program async def main(): print("Running multiple tasks at once!") - await task.all(square(), center(), hello("Pybricks")) + await Task(square(), center(), hello("Pybricks")) print("You can also just run one task.") await hello("World!") # Run the main program. -task.run(main()) +run_task(main()) From c9055bbbd5f010ebff3f62c349e05e674dc6785e Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Sun, 7 May 2023 15:53:31 +0200 Subject: [PATCH 30/59] pybricks.tools.Task: Rename to multitask. This makes it look like an async function, which is closer to the would-be Python implementation. --- pybricks/tools/pb_module_tools.c | 2 +- tests/virtualhub/multitasking/basic.py | 10 +++++----- tests/virtualhub/multitasking/motor.py | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index dbff1851c..04e5fcebd 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -120,7 +120,7 @@ STATIC const mp_rom_map_elem_t tools_globals_table[] = { { MP_ROM_QSTR(MP_QSTR_wait), MP_ROM_PTR(&pb_module_tools_wait_obj) }, { MP_ROM_QSTR(MP_QSTR_run_task), MP_ROM_PTR(&pb_module_tools_run_task_obj) }, { MP_ROM_QSTR(MP_QSTR_StopWatch), MP_ROM_PTR(&pb_type_StopWatch) }, - { MP_ROM_QSTR(MP_QSTR_Task), MP_ROM_PTR(&pb_type_Task) }, + { MP_ROM_QSTR(MP_QSTR_multitask), MP_ROM_PTR(&pb_type_Task) }, #if MICROPY_PY_BUILTINS_FLOAT { MP_ROM_QSTR(MP_QSTR_Matrix), MP_ROM_PTR(&pb_type_Matrix) }, { MP_ROM_QSTR(MP_QSTR_vector), MP_ROM_PTR(&pb_geometry_vector_obj) }, diff --git a/tests/virtualhub/multitasking/basic.py b/tests/virtualhub/multitasking/basic.py index f54d26ece..dcc53258d 100644 --- a/tests/virtualhub/multitasking/basic.py +++ b/tests/virtualhub/multitasking/basic.py @@ -1,4 +1,4 @@ -from pybricks.tools import Task, run_task +from pybricks.tools import multitask, run_task class CallCounter: @@ -55,7 +55,7 @@ async def test_all_1(): task2 = return_after(2, task2_cancel) # should return [0, 1] - both allowed to complete - print(await Task(task1, task2)) + print(await multitask(task1, task2)) # neither should be canceled print(task1_cancel.count, task2_cancel.count) @@ -84,7 +84,7 @@ async def test_all_2(): # should throw RuntimeError("task2") try: - await Task(task1, task2) + await multitask(task1, task2) except Exception as e: print(type(e), e.args) @@ -114,7 +114,7 @@ async def test_race_1(): task2 = return_after(2, task2_cancel) # returns [0, None] - only first completes - print(await Task(task1, task2, race=True)) + print(await multitask(task1, task2, race=True)) # task2 should be canceled print(task1_cancel.count, task2_cancel.count) @@ -143,7 +143,7 @@ async def test_race_2(): # should throw RuntimeError("task2") try: - await Task(task1, task2, race=True) + await multitask(task1, task2, race=True) except Exception as e: print(type(e), e.args) diff --git a/tests/virtualhub/multitasking/motor.py b/tests/virtualhub/multitasking/motor.py index c866ac959..caa6364a9 100644 --- a/tests/virtualhub/multitasking/motor.py +++ b/tests/virtualhub/multitasking/motor.py @@ -1,7 +1,7 @@ from pybricks.pupdevices import Motor from pybricks.parameters import Port, Direction from pybricks.robotics import DriveBase -from pybricks.tools import wait, Task, run_task +from pybricks.tools import wait, multitask, run_task # Initialize devices as usual. left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) @@ -37,7 +37,7 @@ async def hello(name): # This is the main program async def main(): print("Running multiple tasks at once!") - await Task(square(), center(), hello("Pybricks")) + await multitask(square(), center(), hello("Pybricks")) print("You can also just run one task.") await hello("World!") From e4d04c76cf2c7bc079e5ff9c012126e4c87fb00f Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 9 May 2023 11:52:47 +0200 Subject: [PATCH 31/59] pbio/control: Set temporary limits via pbio. This will simplify the module-level implementation of run_until_stalled since no values need to be restored there via exception handlers. --- lib/pbio/include/pbio/control_settings.h | 4 ++++ lib/pbio/src/control.c | 7 +++++-- lib/pbio/src/servo.c | 1 + 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/lib/pbio/include/pbio/control_settings.h b/lib/pbio/include/pbio/control_settings.h index 1bbe1fcd1..63e6dc817 100644 --- a/lib/pbio/include/pbio/control_settings.h +++ b/lib/pbio/include/pbio/control_settings.h @@ -81,6 +81,10 @@ typedef struct _pbio_control_settings_t { * Maximum feedback actuation value. On a motor this is the maximum torque. */ int32_t actuation_max; + /** + * As above, but a temporary value used for only one maneuver. + */ + int32_t actuation_max_temporary; /** * Position error feedback constant. */ diff --git a/lib/pbio/src/control.c b/lib/pbio/src/control.c index 603dcc900..2e4f74b50 100644 --- a/lib/pbio/src/control.c +++ b/lib/pbio/src/control.c @@ -273,7 +273,7 @@ void pbio_control_update( int32_t torque_integral = pbio_control_settings_mul_by_gain(integral_error, ctl->settings.pid_ki); // Total torque signal, capped by the actuation limit - int32_t torque = pbio_int_math_clamp(torque_proportional + torque_integral + torque_derivative, ctl->settings.actuation_max); + int32_t torque = pbio_int_math_clamp(torque_proportional + torque_integral + torque_derivative, ctl->settings.actuation_max_temporary); // This completes the computation of the control signal. // The next steps take care of handling windup, or triggering a stop if we are on target. @@ -282,7 +282,7 @@ void pbio_control_update( // if we get at this limit. We wait a little longer though, to make sure it does not fall back to below the limit // within one sample, which we can predict using the current rate times the loop time, with a factor two tolerance. int32_t windup_margin = pbio_control_settings_mul_by_loop_time(pbio_int_math_abs(state->speed)) * 2; - int32_t max_windup_torque = ctl->settings.actuation_max + pbio_control_settings_mul_by_gain(windup_margin, ctl->settings.pid_kp); + int32_t max_windup_torque = ctl->settings.actuation_max_temporary + pbio_control_settings_mul_by_gain(windup_margin, ctl->settings.pid_kp); // Speed value that is rounded to zero if small. This is used for a // direction error check below to avoid false reverses near zero. @@ -437,6 +437,9 @@ static void pbio_control_set_control_type(pbio_control_t *ctl, uint32_t time_now // Set on completion action for this maneuver. ctl->on_completion = on_completion; + // Reset maximum actuation value used for this run. + ctl->settings.actuation_max_temporary = ctl->settings.actuation_max; + // Reset done state. It will get the correct value during the next control // update. REVISIT: Evaluate it here. pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_ON_TARGET, false); diff --git a/lib/pbio/src/servo.c b/lib/pbio/src/servo.c index 413658d53..0f3db3e5f 100644 --- a/lib/pbio/src/servo.c +++ b/lib/pbio/src/servo.c @@ -276,6 +276,7 @@ static pbio_error_t pbio_servo_initialize_settings(pbio_servo_t *srv, int32_t ge .acceleration = DEG_TO_MDEG(2000), .deceleration = DEG_TO_MDEG(2000), .actuation_max = pbio_observer_voltage_to_torque(srv->observer.model, max_voltage), + .actuation_max_temporary = pbio_observer_voltage_to_torque(srv->observer.model, max_voltage), // The nominal voltage is an indication for the nominal torque limit. To // ensure proportional control can always get the motor to within the // configured tolerance, we select pid_kp such that proportional feedback From 126c172577634ba47f2152966579191ec3dbf41c Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 9 May 2023 19:40:51 +0200 Subject: [PATCH 32/59] pbio/drivebase: Use function call to check for active. --- lib/pbio/src/drivebase.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index 92cea56bf..e348f83de 100644 --- a/lib/pbio/src/drivebase.c +++ b/lib/pbio/src/drivebase.c @@ -392,8 +392,8 @@ bool pbio_drivebase_is_done(const pbio_drivebase_t *db) { */ static pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) { - // If passive, then exit - if (db->control_heading.type == PBIO_CONTROL_NONE || db->control_distance.type == PBIO_CONTROL_NONE) { + // If passive, no need to update. + if (!pbio_drivebase_control_is_active(db)) { return PBIO_SUCCESS; } From eacf883c6e7f4b0e7505db047f8ff9df6e2233a8 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 10 May 2023 20:32:15 +0200 Subject: [PATCH 33/59] pbio/control: Fix control type enum names. Also use the appropriate function to check it instead of comparing directly. --- lib/pbio/include/pbio/control.h | 6 +++--- lib/pbio/src/control.c | 22 +++++++++++----------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/lib/pbio/include/pbio/control.h b/lib/pbio/include/pbio/control.h index e12ff0d57..b475ebb5b 100644 --- a/lib/pbio/include/pbio/control.h +++ b/lib/pbio/include/pbio/control.h @@ -85,20 +85,20 @@ typedef enum { /** * No control is active. */ - PBIO_CONTROL_NONE, + PBIO_CONTROL_TYPE_NONE, /** * Run at a given speed for a given amount of time. The exact position * during and after the maneuver is not important. This uses PI control * on the speed error, which is implemented as PD control on a position * signal whose reference pauses when it is blocked. */ - PBIO_CONTROL_TIMED, + PBIO_CONTROL_TYPE_TIMED, /** * Run run at a given speed to a given position, however long it takes. This * uses classical PID control, except that D uses the estimated speed. It * uses anti-windup schemes to prevent P and I from growing when blocked. */ - PBIO_CONTROL_POSITION, + PBIO_CONTROL_TYPE_POSITION, } pbio_control_type_t; /** diff --git a/lib/pbio/src/control.c b/lib/pbio/src/control.c index 2e4f74b50..36afab651 100644 --- a/lib/pbio/src/control.c +++ b/lib/pbio/src/control.c @@ -411,7 +411,7 @@ void pbio_control_update( * @param [in] ctl Control status structure. */ void pbio_control_stop(pbio_control_t *ctl) { - ctl->type = PBIO_CONTROL_NONE; + ctl->type = PBIO_CONTROL_TYPE_NONE; pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_ON_TARGET, true); pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_STALLED, false); ctl->pid_average = 0; @@ -429,7 +429,7 @@ void pbio_control_stop(pbio_control_t *ctl) { static void pbio_control_set_control_type(pbio_control_t *ctl, uint32_t time_now, pbio_control_type_t type, pbio_control_on_completion_t on_completion) { // Setting none control type is the same as stopping. - if (type == PBIO_CONTROL_NONE) { + if (type == PBIO_CONTROL_TYPE_NONE) { pbio_control_stop(ctl); return; } @@ -454,7 +454,7 @@ static void pbio_control_set_control_type(pbio_control_t *ctl, uint32_t time_now pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_STALLED, false); // Reset integrator for new control type. - if (type == PBIO_CONTROL_POSITION) { + if (type == PBIO_CONTROL_TYPE_POSITION) { // If the new type is position, reset position integrator. pbio_position_integrator_reset(&ctl->position_integrator, &ctl->settings, time_now); } else { @@ -568,7 +568,7 @@ static pbio_error_t _pbio_control_start_position_control(pbio_control_t *ctl, ui } // Activate control type and reset integrators if needed. - pbio_control_set_control_type(ctl, time_now, PBIO_CONTROL_POSITION, on_completion); + pbio_control_set_control_type(ctl, time_now, PBIO_CONTROL_TYPE_POSITION, on_completion); return PBIO_SUCCESS; } @@ -677,7 +677,7 @@ pbio_error_t pbio_control_start_position_control_hold(pbio_control_t *ctl, uint3 pbio_trajectory_make_constant(&ctl->trajectory, &command); // Activate control type and reset integrators if needed. - pbio_control_set_control_type(ctl, time_now, PBIO_CONTROL_POSITION, PBIO_CONTROL_ON_COMPLETION_HOLD); + pbio_control_set_control_type(ctl, time_now, PBIO_CONTROL_TYPE_POSITION, PBIO_CONTROL_ON_COMPLETION_HOLD); return PBIO_SUCCESS; } @@ -774,7 +774,7 @@ pbio_error_t pbio_control_start_timed_control(pbio_control_t *ctl, uint32_t time } // Activate control type and reset integrators if needed. - pbio_control_set_control_type(ctl, time_now, PBIO_CONTROL_TIMED, on_completion); + pbio_control_set_control_type(ctl, time_now, PBIO_CONTROL_TYPE_TIMED, on_completion); return PBIO_SUCCESS; } @@ -807,7 +807,7 @@ uint32_t pbio_control_get_ref_time(const pbio_control_t *ctl, uint32_t time_now) * @return True if active (position or time), false if not. */ bool pbio_control_is_active(const pbio_control_t *ctl) { - return ctl->type != PBIO_CONTROL_NONE; + return ctl->type != PBIO_CONTROL_TYPE_NONE; } /** @@ -817,7 +817,7 @@ bool pbio_control_is_active(const pbio_control_t *ctl) { * @return True if position control is active, false if not. */ bool pbio_control_type_is_position(const pbio_control_t *ctl) { - return ctl->type == PBIO_CONTROL_POSITION; + return ctl->type == PBIO_CONTROL_TYPE_POSITION; } /** @@ -827,7 +827,7 @@ bool pbio_control_type_is_position(const pbio_control_t *ctl) { * @return True if timed control is active, false if not. */ bool pbio_control_type_is_time(const pbio_control_t *ctl) { - return ctl->type == PBIO_CONTROL_TIMED; + return ctl->type == PBIO_CONTROL_TYPE_TIMED; } /** @@ -846,7 +846,7 @@ bool pbio_control_is_stalled(const pbio_control_t *ctl, uint32_t *stall_duration } // Get time since stalling began. - uint32_t time_pause_begin = ctl->type == PBIO_CONTROL_POSITION ? ctl->position_integrator.time_pause_begin : ctl->speed_integrator.time_pause_begin; + uint32_t time_pause_begin = pbio_control_type_is_position(ctl) ? ctl->position_integrator.time_pause_begin : ctl->speed_integrator.time_pause_begin; *stall_duration = pbio_control_get_time_ticks() - time_pause_begin; return true; @@ -861,5 +861,5 @@ bool pbio_control_is_stalled(const pbio_control_t *ctl, uint32_t *stall_duration * @return True if the controller is done, false if not. */ bool pbio_control_is_done(const pbio_control_t *ctl) { - return ctl->type == PBIO_CONTROL_NONE || pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_ON_TARGET); + return ctl->type == PBIO_CONTROL_TYPE_NONE || pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_ON_TARGET); } From 11ce5b717c017fff21d18cd59a010f21ed6f4642 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 10 May 2023 21:39:42 +0200 Subject: [PATCH 34/59] pbio/control: Generalize control type. This prepares for adding additional control objectives as flags. --- lib/pbio/include/pbio/control.h | 14 +++++++----- lib/pbio/src/control.c | 38 ++++++++++++++++----------------- 2 files changed, 28 insertions(+), 24 deletions(-) diff --git a/lib/pbio/include/pbio/control.h b/lib/pbio/include/pbio/control.h index b475ebb5b..15dc8f412 100644 --- a/lib/pbio/include/pbio/control.h +++ b/lib/pbio/include/pbio/control.h @@ -85,20 +85,24 @@ typedef enum { /** * No control is active. */ - PBIO_CONTROL_TYPE_NONE, + PBIO_CONTROL_TYPE_NONE = 0, /** * Run at a given speed for a given amount of time. The exact position * during and after the maneuver is not important. This uses PI control * on the speed error, which is implemented as PD control on a position * signal whose reference pauses when it is blocked. */ - PBIO_CONTROL_TYPE_TIMED, + PBIO_CONTROL_TYPE_TIMED = 1, /** * Run run at a given speed to a given position, however long it takes. This * uses classical PID control, except that D uses the estimated speed. It * uses anti-windup schemes to prevent P and I from growing when blocked. */ - PBIO_CONTROL_TYPE_POSITION, + PBIO_CONTROL_TYPE_POSITION = 2, + /** + * Mask to extract the type of controller from the control type. + */ + PBIO_CONTROL_TYPE_MASK = 3, } pbio_control_type_t; /** @@ -107,8 +111,8 @@ typedef enum { typedef enum { /** The controller is stalled. */ PBIO_CONTROL_STATUS_STALLED = 1 << 0, - /** The control command is complete. */ - PBIO_CONTROL_STATUS_ON_TARGET = 1 << 1, + /** The control objective is complete/achieved. */ + PBIO_CONTROL_STATUS_COMPLETE = 1 << 1, } pbio_control_status_flag_t; /** diff --git a/lib/pbio/src/control.c b/lib/pbio/src/control.c index 36afab651..846e57c76 100644 --- a/lib/pbio/src/control.c +++ b/lib/pbio/src/control.c @@ -82,6 +82,14 @@ static pbio_control_on_completion_t pbio_control_on_completion_discard_smart(pbi return on_completion; } +static void pbio_control_status_set(pbio_control_t *ctl, pbio_control_status_flag_t flag, bool set) { + ctl->status = set ? ctl->status | flag : ctl->status & ~flag; +} + +static bool pbio_control_status_test(const pbio_control_t *ctl, pbio_control_status_flag_t flag) { + return ctl->status & flag; +} + static bool pbio_control_check_completion(const pbio_control_t *ctl, uint32_t time, const pbio_control_state_t *state, const pbio_trajectory_reference_t *end) { // If no control is active, then all targets are complete. @@ -128,14 +136,6 @@ static bool pbio_control_check_completion(const pbio_control_t *ctl, uint32_t ti return pbio_int_math_abs(position_remaining) <= ctl->settings.position_tolerance; } -static void pbio_control_status_set(pbio_control_t *ctl, pbio_control_status_flag_t flag, bool set) { - ctl->status = set ? ctl->status | flag : ctl->status & ~flag; -} - -static bool pbio_control_status_test(const pbio_control_t *ctl, pbio_control_status_flag_t flag) { - return ctl->status & flag; -} - /** * Gets the position, speed, and acceleration, on the reference trajectory. * @@ -328,7 +328,7 @@ void pbio_control_update( pbio_speed_integrator_stalled(&ctl->speed_integrator, time_now, state->speed, ref->speed)); // Check if we are on target, and set the status. - pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_ON_TARGET, + pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_COMPLETE, pbio_control_check_completion(ctl, ref->time, state, &ref_end)); // Save (low-pass filtered) load for diagnostics @@ -336,7 +336,7 @@ void pbio_control_update( // Decide actuation based on control status. if (// Not on target yet, so keep actuating. - !pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_ON_TARGET) || + !pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_COMPLETE) || // Active completion type, so keep actuating. pbio_control_on_completion_is_active(ctl->on_completion) || // Smart passive mode, and we're only just complete, so keep actuating. @@ -357,7 +357,7 @@ void pbio_control_update( // Handling hold after running for time requires an extra step because // it can only be done by starting a new position based command. - if (pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_ON_TARGET) && + if (pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_COMPLETE) && pbio_control_type_is_time(ctl) && ctl->on_completion == PBIO_CONTROL_ON_COMPLETION_HOLD) { // Use current state as target for holding. @@ -412,7 +412,7 @@ void pbio_control_update( */ void pbio_control_stop(pbio_control_t *ctl) { ctl->type = PBIO_CONTROL_TYPE_NONE; - pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_ON_TARGET, true); + pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_COMPLETE, true); pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_STALLED, false); ctl->pid_average = 0; } @@ -429,7 +429,7 @@ void pbio_control_stop(pbio_control_t *ctl) { static void pbio_control_set_control_type(pbio_control_t *ctl, uint32_t time_now, pbio_control_type_t type, pbio_control_on_completion_t on_completion) { // Setting none control type is the same as stopping. - if (type == PBIO_CONTROL_TYPE_NONE) { + if ((type & PBIO_CONTROL_TYPE_MASK) == PBIO_CONTROL_TYPE_NONE) { pbio_control_stop(ctl); return; } @@ -442,7 +442,7 @@ static void pbio_control_set_control_type(pbio_control_t *ctl, uint32_t time_now // Reset done state. It will get the correct value during the next control // update. REVISIT: Evaluate it here. - pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_ON_TARGET, false); + pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_COMPLETE, false); // Exit if control type already set. if (ctl->type == type) { @@ -454,7 +454,7 @@ static void pbio_control_set_control_type(pbio_control_t *ctl, uint32_t time_now pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_STALLED, false); // Reset integrator for new control type. - if (type == PBIO_CONTROL_TYPE_POSITION) { + if ((type & PBIO_CONTROL_TYPE_MASK) == PBIO_CONTROL_TYPE_POSITION) { // If the new type is position, reset position integrator. pbio_position_integrator_reset(&ctl->position_integrator, &ctl->settings, time_now); } else { @@ -807,7 +807,7 @@ uint32_t pbio_control_get_ref_time(const pbio_control_t *ctl, uint32_t time_now) * @return True if active (position or time), false if not. */ bool pbio_control_is_active(const pbio_control_t *ctl) { - return ctl->type != PBIO_CONTROL_TYPE_NONE; + return (ctl->type & PBIO_CONTROL_TYPE_MASK) != PBIO_CONTROL_TYPE_NONE; } /** @@ -817,7 +817,7 @@ bool pbio_control_is_active(const pbio_control_t *ctl) { * @return True if position control is active, false if not. */ bool pbio_control_type_is_position(const pbio_control_t *ctl) { - return ctl->type == PBIO_CONTROL_TYPE_POSITION; + return (ctl->type & PBIO_CONTROL_TYPE_MASK) == PBIO_CONTROL_TYPE_POSITION; } /** @@ -827,7 +827,7 @@ bool pbio_control_type_is_position(const pbio_control_t *ctl) { * @return True if timed control is active, false if not. */ bool pbio_control_type_is_time(const pbio_control_t *ctl) { - return ctl->type == PBIO_CONTROL_TYPE_TIMED; + return (ctl->type & PBIO_CONTROL_TYPE_MASK) == PBIO_CONTROL_TYPE_TIMED; } /** @@ -861,5 +861,5 @@ bool pbio_control_is_stalled(const pbio_control_t *ctl, uint32_t *stall_duration * @return True if the controller is done, false if not. */ bool pbio_control_is_done(const pbio_control_t *ctl) { - return ctl->type == PBIO_CONTROL_TYPE_NONE || pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_ON_TARGET); + return !pbio_control_is_active(ctl) || pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_COMPLETE); } From 80b8f83f1a22a2c0bc99f5070e70b8a9ed5d5a1c Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 10 May 2023 21:49:49 +0200 Subject: [PATCH 35/59] pbio/control: Add stalling as control objective. This implements running until stalled. --- lib/pbio/include/pbio/control.h | 4 ++++ lib/pbio/src/control.c | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/lib/pbio/include/pbio/control.h b/lib/pbio/include/pbio/control.h index 15dc8f412..0d4132c3f 100644 --- a/lib/pbio/include/pbio/control.h +++ b/lib/pbio/include/pbio/control.h @@ -103,6 +103,10 @@ typedef enum { * Mask to extract the type of controller from the control type. */ PBIO_CONTROL_TYPE_MASK = 3, + /** + * Flag indicating controller should stop (complete) when it stalls. + */ + PBIO_CONTROL_TYPE_FLAG_STOP_ON_STALL = 1 << 2, } pbio_control_type_t; /** diff --git a/lib/pbio/src/control.c b/lib/pbio/src/control.c index 846e57c76..ea6f32cde 100644 --- a/lib/pbio/src/control.c +++ b/lib/pbio/src/control.c @@ -97,6 +97,12 @@ static bool pbio_control_check_completion(const pbio_control_t *ctl, uint32_t ti return true; } + // If stall is set as an objective (usually with run-until-stalled) and + // it is currently stalled, then the maneuver is complete. + if (ctl->type & PBIO_CONTROL_TYPE_FLAG_STOP_ON_STALL && pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_STALLED)) { + return true; + } + // Check if we are passed the nominal maneuver time. bool time_completed = pbio_control_settings_time_is_later(time, end->time); From 4040fe1307af499f3000e8d239034b5890b7803e Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 10 May 2023 22:11:06 +0200 Subject: [PATCH 36/59] pbio/servo: Implement run_until_stalled. This will be used to replace the module-level wrapper around running forever and awaiting a stall condition. This simplifies those routines because it can just wait for completion just like any other run_* method. It is also useful to have this functionality (back) in pbio so that non-MicroPython implementation can still use this. --- lib/pbio/include/pbio/servo.h | 1 + lib/pbio/src/servo.c | 36 +++++++++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+) diff --git a/lib/pbio/include/pbio/servo.h b/lib/pbio/include/pbio/servo.h index f7fed7111..497801554 100644 --- a/lib/pbio/include/pbio/servo.h +++ b/lib/pbio/include/pbio/servo.h @@ -161,6 +161,7 @@ pbio_error_t pbio_servo_stop(pbio_servo_t *srv, pbio_control_on_completion_t on_ pbio_error_t pbio_servo_reset_angle(pbio_servo_t *srv, int32_t reset_angle, bool reset_to_abs); pbio_error_t pbio_servo_run_forever(pbio_servo_t *srv, int32_t speed); pbio_error_t pbio_servo_run_time(pbio_servo_t *srv, int32_t speed, uint32_t duration, pbio_control_on_completion_t on_completion); +pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit_pct, pbio_control_on_completion_t on_completion); pbio_error_t pbio_servo_run_angle(pbio_servo_t *srv, int32_t speed, int32_t angle, pbio_control_on_completion_t on_completion); pbio_error_t pbio_servo_run_target(pbio_servo_t *srv, int32_t speed, int32_t target, pbio_control_on_completion_t on_completion); pbio_error_t pbio_servo_track_target(pbio_servo_t *srv, int32_t target); diff --git a/lib/pbio/src/servo.c b/lib/pbio/src/servo.c index 0f3db3e5f..2b6eee330 100644 --- a/lib/pbio/src/servo.c +++ b/lib/pbio/src/servo.c @@ -642,6 +642,42 @@ pbio_error_t pbio_servo_run_time(pbio_servo_t *srv, int32_t speed, uint32_t dura return pbio_servo_run_time_common(srv, speed, duration, on_completion); } +/** + * Runs the servo at a given speed until it stalls, then stops there. + * + * @param [in] srv The servo instance. + * @param [in] speed Angular velocity in degrees per second. + * @param [in] torque_limit_pct Percentage of maximum feedback torque to use. + * It will still use the default feedforward torque. + * @param [in] on_completion What to do once stalled. + * @return Error code. + */ +pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit_pct, pbio_control_on_completion_t on_completion) { + + if (on_completion == PBIO_CONTROL_ON_COMPLETION_CONTINUE) { + // Can't continue after stall. + return PBIO_ERROR_INVALID_ARG; + } + + // Start an infinite maneuver. + pbio_error_t err = pbio_servo_run_forever(srv, speed); + if (err != PBIO_SUCCESS) { + return err; + } + + // Infinite maneuvers continue on completion, but here we want to set the + // user specified on_completion behavior. + srv->control.on_completion = on_completion; + + // We add the objective of stopping on stall. + srv->control.type |= PBIO_CONTROL_TYPE_FLAG_STOP_ON_STALL; + + // Set the temporary torque limit used during this maneuver. + srv->control.settings.actuation_max_temporary = srv->control.settings.actuation_max * torque_limit_pct / 100; + + return PBIO_SUCCESS; +} + /** * Runs the servo at a given speed to a given target angle and stops there. * From d53c3005ffd479ddc7f3bad854f3bc975f20435b Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 11 May 2023 09:15:52 +0200 Subject: [PATCH 37/59] pbio/control: Add stalling as completion option. Stopping on stall and running until stall are different things. --- lib/pbio/include/pbio/control.h | 8 +++++++- lib/pbio/src/control.c | 8 ++++++-- lib/pbio/src/servo.c | 2 +- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/lib/pbio/include/pbio/control.h b/lib/pbio/include/pbio/control.h index 0d4132c3f..64e9376b6 100644 --- a/lib/pbio/include/pbio/control.h +++ b/lib/pbio/include/pbio/control.h @@ -104,9 +104,15 @@ typedef enum { */ PBIO_CONTROL_TYPE_MASK = 3, /** - * Flag indicating controller should stop (complete) when it stalls. + * Flag indicating that controller should stop (complete) when it stalls. + * Can be used to have an always-active stall protection. */ PBIO_CONTROL_TYPE_FLAG_STOP_ON_STALL = 1 << 2, + /** + * Flag indicating controller should run until it stalls. Can be used to + * explicitly run until it stalls to find a mechanism endpoint. + */ + PBIO_CONTROL_TYPE_FLAG_OBJECTIVE_IS_STALL = 1 << 3, } pbio_control_type_t; /** diff --git a/lib/pbio/src/control.c b/lib/pbio/src/control.c index ea6f32cde..2a6fa18e2 100644 --- a/lib/pbio/src/control.c +++ b/lib/pbio/src/control.c @@ -97,8 +97,12 @@ static bool pbio_control_check_completion(const pbio_control_t *ctl, uint32_t ti return true; } - // If stall is set as an objective (usually with run-until-stalled) and - // it is currently stalled, then the maneuver is complete. + // If stalling is the *objective*, stall state is completion state. + if (ctl->type & PBIO_CONTROL_TYPE_FLAG_OBJECTIVE_IS_STALL) { + return pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_STALLED); + } + + // If request to stop on stall, return if stalled but proceed with other checks. if (ctl->type & PBIO_CONTROL_TYPE_FLAG_STOP_ON_STALL && pbio_control_status_test(ctl, PBIO_CONTROL_STATUS_STALLED)) { return true; } diff --git a/lib/pbio/src/servo.c b/lib/pbio/src/servo.c index 2b6eee330..2cdf7be98 100644 --- a/lib/pbio/src/servo.c +++ b/lib/pbio/src/servo.c @@ -670,7 +670,7 @@ pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int3 srv->control.on_completion = on_completion; // We add the objective of stopping on stall. - srv->control.type |= PBIO_CONTROL_TYPE_FLAG_STOP_ON_STALL; + srv->control.type |= PBIO_CONTROL_TYPE_FLAG_OBJECTIVE_IS_STALL; // Set the temporary torque limit used during this maneuver. srv->control.settings.actuation_max_temporary = srv->control.settings.actuation_max * torque_limit_pct / 100; From 236e21bc7d5e9e64cee6dee25287be063dcb7137 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 11 May 2023 09:18:48 +0200 Subject: [PATCH 38/59] pybricks.common.Motor: Update run_until_stall. This uses the new pbio implementation. It also renames the duty_limit kwarg but allows the original name for backwards compatibility. --- CHANGELOG.md | 2 ++ pybricks/common/pb_type_motor.c | 40 ++++++++++++--------------------- 2 files changed, 16 insertions(+), 26 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 717ef5a74..541b7f2fd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,8 @@ ### Changed - Updated MicroPython to v1.20.0. +- Changed `duty_limit` argument of `Motor.run_until_stalled` to `torque_limit`. + The original name continues to work as well. ### Fixed - Fixed stdin containing `0x06` command byte ([support#1052]). diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 3f7213c41..b5f980302 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -242,24 +242,18 @@ STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, m } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_time_obj, 1, common_Motor_run_time); +// REVISIT: Split return value and share completion method with other methods. STATIC mp_obj_t common_Motor_stall_test_completion(mp_obj_t self_in, uint32_t start_time) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); - // Restore original voltage limit. - pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, self->max_voltage_last)); - // Handle I/O exceptions like port unplugged. if (!pbio_servo_update_loop_is_running(self->srv)) { pb_assert(PBIO_ERROR_NO_DEV); } - bool stalled; - uint32_t stall_duration; - pb_assert(pbio_servo_is_stalled(self->srv, &stalled, &stall_duration)); - - // Keep going if not stalled. - if (!stalled) { + // Keep going if not done. + if (!pbio_control_is_done(&self->srv->control)) { return mp_const_none; } @@ -267,9 +261,6 @@ STATIC mp_obj_t common_Motor_stall_test_completion(mp_obj_t self_in, uint32_t st int32_t stall_angle, stall_speed; pb_assert(pbio_servo_get_state_user(self->srv, &stall_angle, &stall_speed)); - // Stop moving. - pb_assert(pbio_servo_stop(self->srv, self->on_stall)); - // Raise stop iteration with angle to return the final position. return mp_make_stop_iteration(mp_obj_new_int(stall_angle)); } @@ -286,28 +277,25 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po common_Motor_obj_t, self, PB_ARG_REQUIRED(speed), PB_ARG_DEFAULT_OBJ(then, pb_Stop_COAST_obj), + PB_ARG_DEFAULT_NONE(torque_limit), PB_ARG_DEFAULT_NONE(duty_limit)); mp_int_t speed = pb_obj_get_int(speed_in); + pbio_control_on_completion_t then = pb_type_enum_get_value(then_in, &pb_enum_type_Stop); - // Read original voltage limit and completion type so we can restore it when we're done. - pbio_dcmotor_get_settings(self->srv->dcmotor, &self->max_voltage_last); - self->on_stall = pb_type_enum_get_value(then_in, &pb_enum_type_Stop); - - // If duty_limit argument given, limit duty during this maneuver. + // Backwards compatibility <= v3.2: allow duty_limit arg as torque_limit. if (duty_limit_in != mp_const_none) { - // Internally, the use of a duty cycle limit has been deprecated and - // replaced by a voltage limit. Since we can't break the user API, we - // convert the user duty limit (0--100) to a voltage by scaling it with - // the battery voltage level, giving the same behavior as before. - uint32_t max_voltage = pbio_battery_get_voltage_from_duty_pct(pb_obj_get_pct(duty_limit_in)); - - // Apply the user limit - pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, max_voltage)); + torque_limit_in = duty_limit_in; + } + + // Get torque limit as percentage of max torque. + int32_t torque_limit_pct = 100; + if (torque_limit_in != mp_const_none) { + torque_limit_pct = pb_obj_get_pct(torque_limit_in); } // Start moving. - pb_assert(pbio_servo_run_forever(self->srv, speed)); + pb_assert(pbio_servo_run_until_stalled(self->srv, speed, torque_limit_pct, then)); // Handle completion by awaiting or blocking. return pb_type_awaitable_await_or_block(pos_args[0], &motor_stalled_awaitable_config, self->first_awaitable); From 93b15c7a515e164eff41f149c09dc5aac2e746de Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 11 May 2023 11:18:35 +0200 Subject: [PATCH 39/59] pybricks/tools/pb_type_awaitable: Generalize return value. In many cases, the await part is the same across methods and only the return value differs. This generalizes the awaitable by splitting the completion test and return value. This will be particularly useful for sensors later on. All methods await mode switches in the same way, but they have different return values. --- pybricks/common/pb_type_motor.c | 26 ++++++++------------------ pybricks/common/pb_type_speaker.c | 16 +++++++++------- pybricks/robotics/pb_type_drivebase.c | 5 +++-- pybricks/tools/pb_module_tools.c | 4 ++-- pybricks/tools/pb_type_awaitable.c | 27 ++++++++++++++++++++------- pybricks/tools/pb_type_awaitable.h | 24 +++++++++++++++++++++--- 6 files changed, 63 insertions(+), 39 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index b5f980302..737668a9d 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -129,7 +129,7 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, return MP_OBJ_FROM_PTR(self); } -STATIC mp_obj_t common_Motor_test_completion(mp_obj_t self_in, uint32_t start_time) { +STATIC bool common_Motor_test_completion(mp_obj_t self_in, uint32_t start_time) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); // Handle I/O exceptions like port unplugged. if (!pbio_servo_update_loop_is_running(self->srv)) { @@ -137,7 +137,7 @@ STATIC mp_obj_t common_Motor_test_completion(mp_obj_t self_in, uint32_t start_ti } // Get completion state. - return pbio_control_is_done(&self->srv->control) ? MP_OBJ_STOP_ITERATION : mp_const_none; + return pbio_control_is_done(&self->srv->control); } STATIC void common_Motor_cancel(mp_obj_t self_in) { @@ -149,6 +149,7 @@ STATIC void common_Motor_cancel(mp_obj_t self_in) { STATIC const pb_type_awaitable_config_t motor_awaitable_config = { .test_completion_func = common_Motor_test_completion, + .return_value_func = NULL, .cancel_func = common_Motor_cancel, .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE, }; @@ -242,31 +243,20 @@ STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, m } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_time_obj, 1, common_Motor_run_time); -// REVISIT: Split return value and share completion method with other methods. -STATIC mp_obj_t common_Motor_stall_test_completion(mp_obj_t self_in, uint32_t start_time) { +STATIC mp_obj_t common_Motor_stall_return_value(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); - // Handle I/O exceptions like port unplugged. - if (!pbio_servo_update_loop_is_running(self->srv)) { - pb_assert(PBIO_ERROR_NO_DEV); - } - - // Keep going if not done. - if (!pbio_control_is_done(&self->srv->control)) { - return mp_const_none; - } - - // Read the angle upon completion of the stall maneuver. + // Return the angle upon completion of the stall maneuver. int32_t stall_angle, stall_speed; pb_assert(pbio_servo_get_state_user(self->srv, &stall_angle, &stall_speed)); - // Raise stop iteration with angle to return the final position. - return mp_make_stop_iteration(mp_obj_new_int(stall_angle)); + return mp_obj_new_int(stall_angle); } STATIC const pb_type_awaitable_config_t motor_stalled_awaitable_config = { - .test_completion_func = common_Motor_stall_test_completion, + .test_completion_func = common_Motor_test_completion, + .return_value_func = common_Motor_stall_return_value, .cancel_func = common_Motor_cancel, .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE, }; diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 117d6c8fd..862830260 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -128,13 +128,13 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg return MP_OBJ_FROM_PTR(self); } -STATIC mp_obj_t pb_type_Speaker_beep_test_completion(mp_obj_t self_in, uint32_t start_time) { +STATIC bool pb_type_Speaker_beep_test_completion(mp_obj_t self_in, uint32_t start_time) { pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); if (mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX) { pb_type_Speaker_stop_beep(); - return MP_OBJ_STOP_ITERATION; + return true; } - return mp_const_none; + return false; } STATIC void pb_type_Speaker_cancel(mp_obj_t self_in) { @@ -147,6 +147,7 @@ STATIC void pb_type_Speaker_cancel(mp_obj_t self_in) { STATIC const pb_type_awaitable_config_t speaker_beep_awaitable_config = { .test_completion_func = pb_type_Speaker_beep_test_completion, + .return_value_func = NULL, .cancel_func = pb_type_Speaker_cancel, .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK, }; @@ -340,7 +341,7 @@ STATIC void pb_type_Speaker_play_note(pb_type_Speaker_obj_t *self, mp_obj_t obj, self->beep_end_time = release ? time_now + 7 * duration / 8 : time_now + duration; } -STATIC mp_obj_t pb_type_Speaker_notes_test_completion(mp_obj_t self_in, uint32_t start_time) { +STATIC bool pb_type_Speaker_notes_test_completion(mp_obj_t self_in, uint32_t start_time) { pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); bool release_done = mp_hal_ticks_ms() - self->release_end_time < (uint32_t)INT32_MAX; @@ -352,12 +353,12 @@ STATIC mp_obj_t pb_type_Speaker_notes_test_completion(mp_obj_t self_in, uint32_t // If there is no next note, generator is done. if (item == MP_OBJ_STOP_ITERATION) { - return MP_OBJ_STOP_ITERATION; + return true; } // Start the note. pb_type_Speaker_play_note(self, item, self->note_duration); - return mp_const_none; + return false; } if (beep_done) { @@ -365,11 +366,12 @@ STATIC mp_obj_t pb_type_Speaker_notes_test_completion(mp_obj_t self_in, uint32_t pb_type_Speaker_stop_beep(); } - return mp_const_none; + return false; } STATIC const pb_type_awaitable_config_t speaker_notes_awaitable_config = { .test_completion_func = pb_type_Speaker_notes_test_completion, + .return_value_func = NULL, .cancel_func = pb_type_Speaker_cancel, .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK, }; diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index fe8212970..6a1524969 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -90,7 +90,7 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a return MP_OBJ_FROM_PTR(self); } -STATIC mp_obj_t pb_type_DriveBase_test_completion(mp_obj_t self_in, uint32_t start_time) { +STATIC bool pb_type_DriveBase_test_completion(mp_obj_t self_in, uint32_t start_time) { pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); @@ -100,7 +100,7 @@ STATIC mp_obj_t pb_type_DriveBase_test_completion(mp_obj_t self_in, uint32_t sta } // Get completion state. - return pbio_drivebase_is_done(self->db) ? MP_OBJ_STOP_ITERATION : mp_const_none; + return pbio_drivebase_is_done(self->db); } STATIC void pb_type_DriveBase_cancel(mp_obj_t self_in) { @@ -110,6 +110,7 @@ STATIC void pb_type_DriveBase_cancel(mp_obj_t self_in) { STATIC const pb_type_awaitable_config_t drivebase_awaitable_config = { .test_completion_func = pb_type_DriveBase_test_completion, + .return_value_func = NULL, .cancel_func = pb_type_DriveBase_cancel, .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE, }; diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 04e5fcebd..bc374a67c 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -27,9 +27,9 @@ // cleared when the module initializes. STATIC pb_type_awaitable_obj_t *first_wait_awaitable; -STATIC mp_obj_t pb_module_tools_wait_test_completion(mp_obj_t obj, uint32_t start_time) { +STATIC bool pb_module_tools_wait_test_completion(mp_obj_t obj, uint32_t start_time) { // obj was validated to be small int, so we can do a cheap comparison here. - return mp_hal_ticks_ms() - start_time >= (uint32_t)MP_OBJ_SMALL_INT_VALUE(obj) ? MP_OBJ_STOP_ITERATION : mp_const_none; + return mp_hal_ticks_ms() - start_time >= (uint32_t)MP_OBJ_SMALL_INT_VALUE(obj); } STATIC const pb_type_awaitable_config_t pb_module_tools_wait_config = { diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 9450b7411..81d374b0c 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -7,6 +7,7 @@ #include "py/mphal.h" #include "py/mpstate.h" +#include "py/runtime.h" #include #include @@ -26,6 +27,10 @@ struct _pb_type_awaitable_obj_t { * which means that it can be used again. */ pb_type_awaitable_test_completion_t test_completion; + /** + * Gets the return value of the awaitable. + */ + pb_type_awaitable_return_t return_value; /** * Called on cancellation. */ @@ -51,19 +56,26 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_1(pb_type_awaitable_close_obj, pb_type_awaitable_ STATIC mp_obj_t pb_type_awaitable_iternext(mp_obj_t self_in) { pb_type_awaitable_obj_t *self = MP_OBJ_TO_PTR(self_in); - // If completed callback was unset, then we are done. + // If completed callback was unset, then we completed previously. if (self->test_completion == NULL) { return MP_OBJ_STOP_ITERATION; } - // Test completion status. - mp_obj_t completion = self->test_completion(self->obj, self->start_time); + // Keep going if not completed by returning None. + if (!self->test_completion(self->obj, self->start_time)) { + return mp_const_none; + } - // none means keep going, everything else means done so finalize. - if (completion != mp_const_none) { - self->test_completion = NULL; + // Complete, so unset callback. + self->test_completion = NULL; + + // For no return value, return basic stop iteration. + if (!self->return_value) { + return MP_OBJ_STOP_ITERATION; } - return completion; + + // Otherwise, set return value via stop iteration. + return mp_make_stop_iteration(self->return_value(self->obj)); } STATIC const mp_rom_map_elem_t pb_type_awaitable_locals_dict_table[] = { @@ -152,6 +164,7 @@ STATIC mp_obj_t pb_type_awaitable_new(mp_obj_t obj, const pb_type_awaitable_conf pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(first_awaitable); awaitable->obj = obj; awaitable->test_completion = config->test_completion_func; + awaitable->return_value = config->return_value_func; awaitable->cancel = config->cancel_func; awaitable->start_time = mp_hal_ticks_ms(); return MP_OBJ_FROM_PTR(awaitable); diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index 281db6ad8..30addd323 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -36,18 +36,32 @@ typedef enum _pb_type_awaitable_cancel_opt_t { typedef struct _pb_type_awaitable_obj_t pb_type_awaitable_obj_t; /** - * Tests if awaitable operation is complete. Returns MP_OBJ_STOP_ITERATION if - * done, possibly with return argument, else mp_const_none. + * Tests if awaitable operation is complete. * * On completion, this function is expected to close/stop hardware * operations as needed (hold a motor, etc.). This is not the same as cancel * below, which always stops the relevant hardware (i.e. always coast). + * + * @param [in] obj The object associated with this awaitable. + * @param [in] start_time The time when the awaitable was created. + * @return True if operation is complete, False otherwise. + */ +typedef bool (*pb_type_awaitable_test_completion_t)(mp_obj_t obj, uint32_t start_time); + +/** + * Gets the return value of the awaitable. If it always returns None, providing + * this function is not necessary. + * + * @param [in] obj The object associated with this awaitable. + * @return The return value of the awaitable. */ -typedef mp_obj_t (*pb_type_awaitable_test_completion_t)(mp_obj_t obj, uint32_t start_time); +typedef mp_obj_t (*pb_type_awaitable_return_t)(mp_obj_t obj); /** * Called on cancel/close. Used to stop hardware operation in unhandled * conditions. + * + * @param [in] obj The object associated with this awaitable. */ typedef void (*pb_type_awaitable_cancel_t)(mp_obj_t obj); @@ -60,6 +74,10 @@ typedef struct _pb_type_awaitable_config_t { * Tests if operation is complete. */ pb_type_awaitable_test_completion_t test_completion_func; + /** + * Gets the return value of the awaitable. + */ + pb_type_awaitable_return_t return_value_func; /** * Called on cancellation. */ From 87d9ab099136119ecedc7857bace1684c4af8e6d Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 11 May 2023 11:30:32 +0200 Subject: [PATCH 40/59] pybricks/tools/pb_type_awaitable: Simplify blocking call. With the return value available separately, we can get it directly instead of through iterating. This reinstates behavior close to the way it was before introducing async, in an effort not to reduce performance in the normal use case. --- pybricks/tools/pb_type_awaitable.c | 25 ++++++++----------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 81d374b0c..058cb0f4e 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -189,24 +189,15 @@ mp_obj_t pb_type_awaitable_await_or_block(mp_obj_t obj, const pb_type_awaitable_ return generator; } - // Otherwise block and wait for it to complete. - nlr_buf_t nlr; - mp_obj_t ret = MP_OBJ_NULL; - if (nlr_push(&nlr) == 0) { - while (pb_type_awaitable_iternext(generator) == mp_const_none) { - mp_hal_delay_ms(5); - } - ret = MP_STATE_THREAD(stop_iteration_arg); - nlr_pop(); - } else { - // Cancel the operation if an exception was raised. - pb_type_awaitable_obj_t *self = MP_OBJ_TO_PTR(generator); - if (self->cancel) { - self->cancel(self->obj); - } - nlr_jump(nlr.ret_val); + // Outside run loop, block until the operation is complete. + pb_type_awaitable_obj_t *awaitable = MP_OBJ_TO_PTR(generator); + while (!awaitable->test_completion(awaitable->obj, awaitable->start_time)) { + mp_hal_delay_ms(1); + } + if (!awaitable->return_value) { + return mp_const_none; } - return ret == MP_OBJ_NULL ? mp_const_none : ret; + return awaitable->return_value(awaitable->obj); } #endif // PYBRICKS_PY_TOOLS From f961d55a4bef85e07b2a25ebd9374223e77d938f Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 11 May 2023 11:53:47 +0200 Subject: [PATCH 41/59] pybricks.common.Motor: Drop temporary stall states. No longer needed now that run_until_stalled is handled in pbio. --- pybricks/common.h | 2 -- pybricks/common/pb_type_motor.c | 2 -- 2 files changed, 4 deletions(-) diff --git a/pybricks/common.h b/pybricks/common.h index 4c517295f..40cd18d14 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -103,8 +103,6 @@ typedef struct _common_Motor_obj_t common_Motor_obj_t; struct _common_Motor_obj_t { mp_obj_base_t base; pbio_servo_t *srv; - int32_t max_voltage_last; // TODO: Move to pbio, used to restore state after run_until_stalled - pbio_control_on_completion_t on_stall; // TODO: move to pbio, used to restore state after run_until_stalled #if PYBRICKS_PY_COMMON_MOTOR_MODEL mp_obj_t model; #endif diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 737668a9d..d7a374851 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -142,8 +142,6 @@ STATIC bool common_Motor_test_completion(mp_obj_t self_in, uint32_t start_time) STATIC void common_Motor_cancel(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); - // TODO: Can drop next line if moved to pbio - pb_assert(pbio_dcmotor_set_settings(self->srv->dcmotor, self->max_voltage_last)); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_COAST)); } From 994e72a4bc304b888886111bec48d09dc0467cd5 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 11 May 2023 13:34:21 +0200 Subject: [PATCH 42/59] pybricks/tools/pb_type_awaitable: Drop config struct. These configs were only used to group function parameters. Since they will not be constant generally (each method may have its own return value getter), it is better to just keep the lengthy function args. As done in the drivebase and motor, they can still have a common function that populates the awaitable to avoid code repetition, which keeps code size about the same. --- pybricks/common/pb_type_motor.c | 45 +++++++++-------- pybricks/common/pb_type_speaker.c | 34 ++++++------- pybricks/robotics/pb_type_drivebase.c | 33 +++++++++---- pybricks/tools/pb_module_tools.c | 14 +++--- pybricks/tools/pb_type_awaitable.c | 69 ++++++++++++++------------- pybricks/tools/pb_type_awaitable.h | 37 +++++--------- 6 files changed, 118 insertions(+), 114 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index d7a374851..031912ac3 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -145,12 +145,16 @@ STATIC void common_Motor_cancel(mp_obj_t self_in) { pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_COAST)); } -STATIC const pb_type_awaitable_config_t motor_awaitable_config = { - .test_completion_func = common_Motor_test_completion, - .return_value_func = NULL, - .cancel_func = common_Motor_cancel, - .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE, -}; +// Common awaitable used for most motor methods. +STATIC mp_obj_t await_or_wait(common_Motor_obj_t *self) { + return pb_type_awaitable_await_or_wait( + MP_OBJ_FROM_PTR(self), + self->first_awaitable, + common_Motor_test_completion, + pb_type_awaitable_return_none, + common_Motor_cancel, + PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); +} // pybricks._common.Motor.angle STATIC mp_obj_t common_Motor_angle(mp_obj_t self_in) { @@ -176,7 +180,7 @@ STATIC mp_obj_t common_Motor_reset_angle(size_t n_args, const mp_obj_t *pos_args // Set the new angle pb_assert(pbio_servo_reset_angle(self->srv, reset_angle, reset_to_abs)); - pb_type_awaitable_cancel_all(pos_args[0], motor_awaitable_config.cancel_opt, self->first_awaitable); + pb_type_awaitable_cancel_all(pos_args[0], self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_reset_angle_obj, 1, common_Motor_reset_angle); @@ -201,7 +205,7 @@ STATIC mp_obj_t common_Motor_run(size_t n_args, const mp_obj_t *pos_args, mp_map mp_int_t speed = pb_obj_get_int(speed_in); pb_assert(pbio_servo_run_forever(self->srv, speed)); - pb_type_awaitable_cancel_all(pos_args[0], motor_awaitable_config.cancel_opt, self->first_awaitable); + pb_type_awaitable_cancel_all(pos_args[0], self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); @@ -210,7 +214,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); STATIC mp_obj_t common_Motor_hold(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_HOLD)); - pb_type_awaitable_cancel_all(self_in, motor_awaitable_config.cancel_opt, self->first_awaitable); + pb_type_awaitable_cancel_all(self_in, self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_hold_obj, common_Motor_hold); @@ -237,7 +241,7 @@ STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, m return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_awaitable_await_or_block(pos_args[0], &motor_awaitable_config, self->first_awaitable); + return await_or_wait(self); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_time_obj, 1, common_Motor_run_time); @@ -252,13 +256,6 @@ STATIC mp_obj_t common_Motor_stall_return_value(mp_obj_t self_in) { return mp_obj_new_int(stall_angle); } -STATIC const pb_type_awaitable_config_t motor_stalled_awaitable_config = { - .test_completion_func = common_Motor_test_completion, - .return_value_func = common_Motor_stall_return_value, - .cancel_func = common_Motor_cancel, - .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE, -}; - // pybricks._common.Motor.run_until_stalled STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, @@ -286,7 +283,13 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po pb_assert(pbio_servo_run_until_stalled(self->srv, speed, torque_limit_pct, then)); // Handle completion by awaiting or blocking. - return pb_type_awaitable_await_or_block(pos_args[0], &motor_stalled_awaitable_config, self->first_awaitable); + return pb_type_awaitable_await_or_wait( + MP_OBJ_FROM_PTR(self), + self->first_awaitable, + common_Motor_test_completion, + common_Motor_stall_return_value, + common_Motor_cancel, + PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_until_stalled_obj, 1, common_Motor_run_until_stalled); @@ -311,7 +314,7 @@ STATIC mp_obj_t common_Motor_run_angle(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_awaitable_await_or_block(pos_args[0], &motor_awaitable_config, self->first_awaitable); + return await_or_wait(self); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_angle_obj, 1, common_Motor_run_angle); @@ -336,7 +339,7 @@ STATIC mp_obj_t common_Motor_run_target(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_awaitable_await_or_block(pos_args[0], &motor_awaitable_config, self->first_awaitable); + return await_or_wait(self); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_target_obj, 1, common_Motor_run_target); @@ -348,7 +351,7 @@ STATIC mp_obj_t common_Motor_track_target(size_t n_args, const mp_obj_t *pos_arg mp_int_t target_angle = pb_obj_get_int(target_angle_in); pb_assert(pbio_servo_track_target(self->srv, target_angle)); - pb_type_awaitable_cancel_all(pos_args[0], motor_awaitable_config.cancel_opt, self->first_awaitable); + pb_type_awaitable_cancel_all(pos_args[0], self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_track_target_obj, 1, common_Motor_track_target); diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 862830260..43e9d459f 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -123,8 +123,6 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg self->volume = 100; self->sample_attenuator = INT16_MAX; - - return MP_OBJ_FROM_PTR(self); } @@ -145,13 +143,6 @@ STATIC void pb_type_Speaker_cancel(mp_obj_t self_in) { self->notes_generator = MP_OBJ_NULL; } -STATIC const pb_type_awaitable_config_t speaker_beep_awaitable_config = { - .test_completion_func = pb_type_Speaker_beep_test_completion, - .return_value_func = NULL, - .cancel_func = pb_type_Speaker_cancel, - .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK, -}; - STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pb_type_Speaker_obj_t, self, @@ -170,7 +161,14 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp self->beep_end_time = mp_hal_ticks_ms() + (uint32_t)duration; self->release_end_time = self->beep_end_time; self->notes_generator = MP_OBJ_NULL; - return pb_type_awaitable_await_or_block(pos_args[0], &speaker_beep_awaitable_config, self->first_awaitable); + + return pb_type_awaitable_await_or_wait( + MP_OBJ_FROM_PTR(self), + self->first_awaitable, + pb_type_Speaker_beep_test_completion, + pb_type_awaitable_return_none, + pb_type_Speaker_cancel, + PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_beep_obj, 1, pb_type_Speaker_beep); @@ -369,13 +367,6 @@ STATIC bool pb_type_Speaker_notes_test_completion(mp_obj_t self_in, uint32_t sta return false; } -STATIC const pb_type_awaitable_config_t speaker_notes_awaitable_config = { - .test_completion_func = pb_type_Speaker_notes_test_completion, - .return_value_func = NULL, - .cancel_func = pb_type_Speaker_cancel, - .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK, -}; - STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pb_type_Speaker_obj_t, self, @@ -386,8 +377,13 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar self->note_duration = 4 * 60 * 1000 / pb_obj_get_int(tempo_in); self->beep_end_time = mp_hal_ticks_ms(); self->release_end_time = self->beep_end_time; - return pb_type_awaitable_await_or_block(pos_args[0], &speaker_notes_awaitable_config, self->first_awaitable); - + return pb_type_awaitable_await_or_wait( + MP_OBJ_FROM_PTR(self), + self->first_awaitable, + pb_type_Speaker_notes_test_completion, + pb_type_awaitable_return_none, + pb_type_Speaker_cancel, + PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_play_notes_obj, 1, pb_type_Speaker_play_notes); diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 6a1524969..3e5ebf7e0 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -108,12 +108,16 @@ STATIC void pb_type_DriveBase_cancel(mp_obj_t self_in) { pb_assert(pbio_drivebase_stop(self->db, PBIO_CONTROL_ON_COMPLETION_COAST)); } -STATIC const pb_type_awaitable_config_t drivebase_awaitable_config = { - .test_completion_func = pb_type_DriveBase_test_completion, - .return_value_func = NULL, - .cancel_func = pb_type_DriveBase_cancel, - .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_AWAITABLE, -}; +// All drive base methods use the same kind of completion awaitable. +STATIC mp_obj_t await_or_wait(pb_type_DriveBase_obj_t *self) { + return pb_type_awaitable_await_or_wait( + MP_OBJ_FROM_PTR(self), + self->first_awaitable, + pb_type_DriveBase_test_completion, + pb_type_awaitable_return_none, + pb_type_DriveBase_cancel, + PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); +} // pybricks.robotics.DriveBase.straight STATIC mp_obj_t pb_type_DriveBase_straight(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -133,7 +137,7 @@ STATIC mp_obj_t pb_type_DriveBase_straight(size_t n_args, const mp_obj_t *pos_ar return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_awaitable_await_or_block(pos_args[0], &drivebase_awaitable_config, self->first_awaitable); + return await_or_wait(self); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_straight_obj, 1, pb_type_DriveBase_straight); @@ -156,7 +160,7 @@ STATIC mp_obj_t pb_type_DriveBase_turn(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_awaitable_await_or_block(pos_args[0], &drivebase_awaitable_config, self->first_awaitable); + return await_or_wait(self); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_turn_obj, 1, pb_type_DriveBase_turn); @@ -180,7 +184,7 @@ STATIC mp_obj_t pb_type_DriveBase_curve(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } // Handle completion by awaiting or blocking. - return pb_type_awaitable_await_or_block(pos_args[0], &drivebase_awaitable_config, self->first_awaitable); + return await_or_wait(self); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_curve_obj, 1, pb_type_DriveBase_curve); @@ -195,15 +199,24 @@ STATIC mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, mp_int_t speed = pb_obj_get_int(speed_in); mp_int_t turn_rate = pb_obj_get_int(turn_rate_in); + // Cancel awaitables but not hardware. Drive forever will handle this. + pb_type_awaitable_cancel_all(pos_args[0], self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_assert(pbio_drivebase_drive_forever(self->db, speed, turn_rate)); - pb_type_awaitable_cancel_all(pos_args[0], drivebase_awaitable_config.cancel_opt, self->first_awaitable); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_drive_obj, 1, pb_type_DriveBase_drive); // pybricks.robotics.DriveBase.stop STATIC mp_obj_t pb_type_DriveBase_stop(mp_obj_t self_in) { + + // Cancel awaitables. + pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); + pb_type_awaitable_cancel_all(self_in, self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + + // Stop hardware. pb_type_DriveBase_cancel(self_in); + return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_stop_obj, pb_type_DriveBase_stop); diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index bc374a67c..67a6eced1 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -32,12 +32,6 @@ STATIC bool pb_module_tools_wait_test_completion(mp_obj_t obj, uint32_t start_ti return mp_hal_ticks_ms() - start_time >= (uint32_t)MP_OBJ_SMALL_INT_VALUE(obj); } -STATIC const pb_type_awaitable_config_t pb_module_tools_wait_config = { - .test_completion_func = pb_module_tools_wait_test_completion, - .cancel_func = NULL, - .cancel_opt = PB_TYPE_AWAITABLE_CANCEL_NONE, -}; - STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, PB_ARG_REQUIRED(time)); @@ -56,7 +50,13 @@ STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp // test completion state in iteration loop. time = pbio_int_math_bind(time, 0, INT32_MAX >> 2); - return pb_type_awaitable_await_or_block(MP_OBJ_NEW_SMALL_INT(time), &pb_module_tools_wait_config, first_wait_awaitable); + return pb_type_awaitable_await_or_wait( + MP_OBJ_NEW_SMALL_INT(time), + first_wait_awaitable, + pb_module_tools_wait_test_completion, + pb_type_awaitable_return_none, + pb_type_awaitable_cancel_none, + PB_TYPE_AWAITABLE_CANCEL_NONE); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_module_tools_wait_obj, 0, pb_module_tools_wait); diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 058cb0f4e..0eb1e9cf0 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -125,14 +125,14 @@ STATIC pb_type_awaitable_obj_t *pb_type_awaitable_get(pb_type_awaitable_obj_t *f * This is normally used by the function that makes a new awaitable, but it can * also be called independently to cancel without starting a new awaitable. * - * @param [in] obj The object. - * @param [in] cancel_opt Cancellation type. - * @param [in] first_awaitable The first awaitable in the linked list of awaitables from @p obj. + * @param [in] obj The object whose method we want to wait for completion. + * @param [in] first_awaitable The first awaitable in the linked list of awaitables from @p obj. + * @param [in] cancel_opt Whether to cancel linked awaitables, hardware, or both. */ -void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_cancel_opt_t cancel_opt, pb_type_awaitable_obj_t *first_awaitable) { +void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_awaitable, pb_type_awaitable_cancel_opt_t cancel_opt) { // Exit if nothing to do. - if (cancel_opt == PB_TYPE_AWAITABLE_CANCEL_NONE || !first_awaitable) { + if (!pb_module_tools_run_loop_is_active() || cancel_opt == PB_TYPE_AWAITABLE_CANCEL_NONE || !first_awaitable) { return; } @@ -154,50 +154,53 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_cancel_opt_t c } } -// Get an available awaitable, add callbacks, and return as object so user can await it. -STATIC mp_obj_t pb_type_awaitable_new(mp_obj_t obj, const pb_type_awaitable_config_t *config, pb_type_awaitable_obj_t *first_awaitable) { - - // First cancel linked awaitables if requested. - pb_type_awaitable_cancel_all(obj, config->cancel_opt, first_awaitable); - - // Get and initialize awaitable. - pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(first_awaitable); - awaitable->obj = obj; - awaitable->test_completion = config->test_completion_func; - awaitable->return_value = config->return_value_func; - awaitable->cancel = config->cancel_func; - awaitable->start_time = mp_hal_ticks_ms(); - return MP_OBJ_FROM_PTR(awaitable); -} - /** * Get a new awaitable in async mode or block and wait for it to complete in sync mode. * * Automatically cancels any previous awaitables associated with the object if requested. * - * @param [in] obj The object whose method we want to wait for completion. - * @param [in] config Configuration for the awaitable. - * @param [in] first_awaitable The first awaitable in the linked list of awaitables from @p obj. + * @param [in] obj The object whose method we want to wait for completion. + * @param [in] first_awaitable The first awaitable in the linked list of awaitables from @p obj. + * @param [in] test_completion_func Function to test if the operation is complete. + * @param [in] return_value_func Function that gets the return value for the awaitable. + * @param [in] cancel_func Function to cancel the hardware operation. + * @param [in] cancel_opt Whether to cancel linked awaitables, hardware, or both. */ -mp_obj_t pb_type_awaitable_await_or_block(mp_obj_t obj, const pb_type_awaitable_config_t *config, pb_type_awaitable_obj_t *first_awaitable) { +mp_obj_t pb_type_awaitable_await_or_wait( + mp_obj_t obj, + pb_type_awaitable_obj_t *first_awaitable, + pb_type_awaitable_test_completion_t test_completion_func, + pb_type_awaitable_return_t return_value_func, + pb_type_awaitable_cancel_t cancel_func, + pb_type_awaitable_cancel_opt_t cancel_opt) { - // Make an awaitable object for the given operation. - mp_obj_t generator = pb_type_awaitable_new(obj, config, first_awaitable); + uint32_t start_time = mp_hal_ticks_ms(); - // Within run loop, just return the generator that user program will iterate. + // Within run loop, return the generator that user program will iterate. if (pb_module_tools_run_loop_is_active()) { - return generator; + // First cancel linked awaitables if requested. + pb_type_awaitable_cancel_all(obj, first_awaitable, cancel_opt); + + // Gets existing awaitable or creates a new one. + pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(first_awaitable); + + // Initialize awaitable. + awaitable->obj = obj; + awaitable->test_completion = test_completion_func; + awaitable->return_value = return_value_func; + awaitable->cancel = cancel_func; + awaitable->start_time = start_time; + return MP_OBJ_FROM_PTR(awaitable); } // Outside run loop, block until the operation is complete. - pb_type_awaitable_obj_t *awaitable = MP_OBJ_TO_PTR(generator); - while (!awaitable->test_completion(awaitable->obj, awaitable->start_time)) { + while (!test_completion_func(obj, start_time)) { mp_hal_delay_ms(1); } - if (!awaitable->return_value) { + if (!return_value_func) { return mp_const_none; } - return awaitable->return_value(awaitable->obj); + return return_value_func(obj); } #endif // PYBRICKS_PY_TOOLS diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index 30addd323..b35583797 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -65,32 +65,21 @@ typedef mp_obj_t (*pb_type_awaitable_return_t)(mp_obj_t obj); */ typedef void (*pb_type_awaitable_cancel_t)(mp_obj_t obj); -/** - * Constant configuration of functions and settings that govern the behavior - * of an awaitable associated with a particular method or function. - */ -typedef struct _pb_type_awaitable_config_t { - /** - * Tests if operation is complete. - */ - pb_type_awaitable_test_completion_t test_completion_func; - /** - * Gets the return value of the awaitable. - */ - pb_type_awaitable_return_t return_value_func; - /** - * Called on cancellation. - */ - pb_type_awaitable_cancel_t cancel_func; - /** - * Cancellation options. - */ - pb_type_awaitable_cancel_opt_t cancel_opt; -} pb_type_awaitable_config_t; +#define pb_type_awaitable_completed (NULL) + +#define pb_type_awaitable_return_none (NULL) + +#define pb_type_awaitable_cancel_none (NULL) -void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_cancel_opt_t cancel_opt, pb_type_awaitable_obj_t *first_awaitable); +void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_awaitable, pb_type_awaitable_cancel_opt_t cancel_opt); -mp_obj_t pb_type_awaitable_await_or_block(mp_obj_t obj, const pb_type_awaitable_config_t *config, pb_type_awaitable_obj_t *first_awaitable); +mp_obj_t pb_type_awaitable_await_or_wait( + mp_obj_t obj, + pb_type_awaitable_obj_t *first_awaitable, + pb_type_awaitable_test_completion_t test_completion_func, + pb_type_awaitable_return_t return_value_func, + pb_type_awaitable_cancel_t cancel_func, + pb_type_awaitable_cancel_opt_t cancel_opt); #endif // PYBRICKS_PY_TOOLS From 692df864d5ccd52a936133816569b086d8115c9a Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 11 May 2023 14:08:47 +0200 Subject: [PATCH 43/59] pybricks/tools/pb_type_awaitable: Fix allocation of first awaitable. This broke while refactoring the awaitable code previously. --- pybricks/common/pb_type_motor.c | 4 ++-- pybricks/common/pb_type_speaker.c | 4 ++-- pybricks/robotics/pb_type_drivebase.c | 2 +- pybricks/tools/pb_module_tools.c | 4 ++-- pybricks/tools/pb_type_awaitable.c | 20 ++++++++++---------- pybricks/tools/pb_type_awaitable.h | 2 +- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 031912ac3..9f33ddf6f 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -149,7 +149,7 @@ STATIC void common_Motor_cancel(mp_obj_t self_in) { STATIC mp_obj_t await_or_wait(common_Motor_obj_t *self) { return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - self->first_awaitable, + &self->first_awaitable, common_Motor_test_completion, pb_type_awaitable_return_none, common_Motor_cancel, @@ -285,7 +285,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po // Handle completion by awaiting or blocking. return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - self->first_awaitable, + &self->first_awaitable, common_Motor_test_completion, common_Motor_stall_return_value, common_Motor_cancel, diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 43e9d459f..5415477ff 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -164,7 +164,7 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - self->first_awaitable, + &self->first_awaitable, pb_type_Speaker_beep_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, @@ -379,7 +379,7 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar self->release_end_time = self->beep_end_time; return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - self->first_awaitable, + &self->first_awaitable, pb_type_Speaker_notes_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 3e5ebf7e0..8264eccd2 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -112,7 +112,7 @@ STATIC void pb_type_DriveBase_cancel(mp_obj_t self_in) { STATIC mp_obj_t await_or_wait(pb_type_DriveBase_obj_t *self) { return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - self->first_awaitable, + &self->first_awaitable, pb_type_DriveBase_test_completion, pb_type_awaitable_return_none, pb_type_DriveBase_cancel, diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 67a6eced1..cc20fdf9e 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -25,7 +25,7 @@ // The awaitable for the wait() function has no object associated with // it (unlike e.g. a motor), so we make a starting point here. This gets // cleared when the module initializes. -STATIC pb_type_awaitable_obj_t *first_wait_awaitable; +STATIC pb_type_awaitable_obj_t *first_wait_awaitable = NULL; STATIC bool pb_module_tools_wait_test_completion(mp_obj_t obj, uint32_t start_time) { // obj was validated to be small int, so we can do a cheap comparison here. @@ -52,7 +52,7 @@ STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp return pb_type_awaitable_await_or_wait( MP_OBJ_NEW_SMALL_INT(time), - first_wait_awaitable, + &first_wait_awaitable, pb_module_tools_wait_test_completion, pb_type_awaitable_return_none, pb_type_awaitable_cancel_none, diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 0eb1e9cf0..f141cadf9 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -93,13 +93,6 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_awaitable, STATIC pb_type_awaitable_obj_t *pb_type_awaitable_get(pb_type_awaitable_obj_t *first_awaitable) { - // If the first awaitable was not yet created, do so now. - if (!first_awaitable) { - first_awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); - first_awaitable->test_completion = NULL; - first_awaitable->next_awaitable = NULL; - } - // Find next available awaitable that exists and is not used. pb_type_awaitable_obj_t *awaitable = first_awaitable; while (awaitable->next_awaitable && awaitable->test_completion) { @@ -168,7 +161,7 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_a */ mp_obj_t pb_type_awaitable_await_or_wait( mp_obj_t obj, - pb_type_awaitable_obj_t *first_awaitable, + pb_type_awaitable_obj_t **first_awaitable, pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, pb_type_awaitable_cancel_t cancel_func, @@ -179,10 +172,17 @@ mp_obj_t pb_type_awaitable_await_or_wait( // Within run loop, return the generator that user program will iterate. if (pb_module_tools_run_loop_is_active()) { // First cancel linked awaitables if requested. - pb_type_awaitable_cancel_all(obj, first_awaitable, cancel_opt); + pb_type_awaitable_cancel_all(obj, *first_awaitable, cancel_opt); + + // If the first awaitable was not yet created, do so now. + if (!*first_awaitable) { + *first_awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); + (*first_awaitable)->test_completion = NULL; + (*first_awaitable)->next_awaitable = NULL; + } // Gets existing awaitable or creates a new one. - pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(first_awaitable); + pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(*first_awaitable); // Initialize awaitable. awaitable->obj = obj; diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index b35583797..6e1ebab67 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -75,7 +75,7 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_a mp_obj_t pb_type_awaitable_await_or_wait( mp_obj_t obj, - pb_type_awaitable_obj_t *first_awaitable, + pb_type_awaitable_obj_t **first_awaitable, pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, pb_type_awaitable_cancel_t cancel_func, From b758ac12474d19c45a382f1539422e0b1c1dd0a3 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 11 May 2023 16:12:06 +0200 Subject: [PATCH 44/59] pybricks/tools/pb_type_awaitable: Fix cancellation of linked objects. --- pybricks/tools/pb_type_awaitable.c | 74 ++++++++++++++++++------------ pybricks/tools/pb_type_awaitable.h | 2 - 2 files changed, 45 insertions(+), 31 deletions(-) diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index f141cadf9..130988fe8 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -12,6 +12,9 @@ #include #include +// The awaitable object is free to be reused. +#define AWAITABLE_FREE (NULL) + struct _pb_type_awaitable_obj_t { mp_obj_base_t base; /** @@ -23,8 +26,8 @@ struct _pb_type_awaitable_obj_t { */ uint32_t start_time; /** - * Tests if operation is complete. Gets reset to NULL on completion, - * which means that it can be used again. + * Tests if operation is complete. Gets reset to AWAITABLE_FREE + * on completion, which means that it can be used again. */ pb_type_awaitable_test_completion_t test_completion; /** @@ -44,7 +47,7 @@ struct _pb_type_awaitable_obj_t { // close() cancels the awaitable. STATIC mp_obj_t pb_type_awaitable_close(mp_obj_t self_in) { pb_type_awaitable_obj_t *self = MP_OBJ_TO_PTR(self_in); - self->test_completion = NULL; + self->test_completion = AWAITABLE_FREE; // Handle optional clean up/cancelling of hardware operation. if (self->cancel) { self->cancel(self->obj); @@ -57,7 +60,7 @@ STATIC mp_obj_t pb_type_awaitable_iternext(mp_obj_t self_in) { pb_type_awaitable_obj_t *self = MP_OBJ_TO_PTR(self_in); // If completed callback was unset, then we completed previously. - if (self->test_completion == NULL) { + if (self->test_completion == AWAITABLE_FREE) { return MP_OBJ_STOP_ITERATION; } @@ -67,7 +70,7 @@ STATIC mp_obj_t pb_type_awaitable_iternext(mp_obj_t self_in) { } // Complete, so unset callback. - self->test_completion = NULL; + self->test_completion = AWAITABLE_FREE; // For no return value, return basic stop iteration. if (!self->return_value) { @@ -95,23 +98,36 @@ STATIC pb_type_awaitable_obj_t *pb_type_awaitable_get(pb_type_awaitable_obj_t *f // Find next available awaitable that exists and is not used. pb_type_awaitable_obj_t *awaitable = first_awaitable; - while (awaitable->next_awaitable && awaitable->test_completion) { + while (awaitable->test_completion != AWAITABLE_FREE && awaitable->next_awaitable) { awaitable = awaitable->next_awaitable; } - // Above loop stops if a) there is no next awaitable or b) the current - // awaitable is not in use. Only case a) requires allocating another. - if (!awaitable->next_awaitable) { - // Attach to the previous one. - awaitable->next_awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); - // Initialize the new awaitable. - awaitable = awaitable->next_awaitable; - awaitable->test_completion = NULL; - awaitable->next_awaitable = NULL; + // Loop stopped because available awaitable was found, so return it. + if (awaitable->test_completion == AWAITABLE_FREE) { + return awaitable; } + + // Otherwise no available awaitable was found, so allocate a new one + awaitable->next_awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); + + // Initialize the new awaitable. + awaitable = awaitable->next_awaitable; + awaitable->test_completion = AWAITABLE_FREE; + awaitable->next_awaitable = NULL; return awaitable; } +/** + * Completion checker that is always true. + * + * Linked awaitables are gracefully cancelled by setting this as the completion + * checker. This allows MicroPython to handle completion during the next call + * to iternext. + */ +STATIC bool pb_type_awaitable_completed(mp_obj_t self_in, uint32_t start_time) { + return true; +} + /** * Cancels all awaitables associated with an object. * @@ -130,18 +146,18 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_a } pb_type_awaitable_obj_t *awaitable = first_awaitable; - while (awaitable->next_awaitable) { - // Don't cancel if already done. - if (!awaitable->test_completion) { - continue; - } - // Cancel hardware operation if requested and available. - if (cancel_opt & PB_TYPE_AWAITABLE_CANCEL_CALLBACK && awaitable->cancel) { - awaitable->cancel(awaitable->obj); - } - // Set awaitable to done in order to cancel it gracefully. - if (cancel_opt & PB_TYPE_AWAITABLE_CANCEL_AWAITABLE) { - awaitable->test_completion = NULL; + while (awaitable) { + // Only cancel awaitables that are in use. + if (awaitable->test_completion) { + // Cancel hardware operation if requested and available. + if (cancel_opt & PB_TYPE_AWAITABLE_CANCEL_CALLBACK && awaitable->cancel) { + awaitable->cancel(awaitable->obj); + } + // Set awaitable to done so it gets cancelled it gracefully on the + // next iteration. + if (cancel_opt & PB_TYPE_AWAITABLE_CANCEL_AWAITABLE) { + awaitable->test_completion = pb_type_awaitable_completed; + } } awaitable = awaitable->next_awaitable; } @@ -177,7 +193,7 @@ mp_obj_t pb_type_awaitable_await_or_wait( // If the first awaitable was not yet created, do so now. if (!*first_awaitable) { *first_awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); - (*first_awaitable)->test_completion = NULL; + (*first_awaitable)->test_completion = AWAITABLE_FREE; (*first_awaitable)->next_awaitable = NULL; } @@ -194,7 +210,7 @@ mp_obj_t pb_type_awaitable_await_or_wait( } // Outside run loop, block until the operation is complete. - while (!test_completion_func(obj, start_time)) { + while (test_completion_func && !test_completion_func(obj, start_time)) { mp_hal_delay_ms(1); } if (!return_value_func) { diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index 6e1ebab67..b1c400435 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -65,8 +65,6 @@ typedef mp_obj_t (*pb_type_awaitable_return_t)(mp_obj_t obj); */ typedef void (*pb_type_awaitable_cancel_t)(mp_obj_t obj); -#define pb_type_awaitable_completed (NULL) - #define pb_type_awaitable_return_none (NULL) #define pb_type_awaitable_cancel_none (NULL) From cd7304ae8b8972382b4390a8d7fc29d6e2f9cff2 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 12 May 2023 08:59:56 +0200 Subject: [PATCH 45/59] pybricks/tools/pb_type_awaitable: Use mp_obj_list_t instead of our own linked list. This avoids issues with garbage collected items of the linked list. It also makes the code easier to follow by using standard MicroPython tools. --- pybricks/common.h | 2 +- pybricks/common/pb_type_motor.c | 16 +++--- pybricks/common/pb_type_speaker.c | 11 +++-- pybricks/robotics/pb_type_drivebase.c | 12 +++-- pybricks/tools/pb_module_tools.c | 40 ++++++++------- pybricks/tools/pb_type_awaitable.c | 71 +++++++++++++-------------- pybricks/tools/pb_type_awaitable.h | 4 +- 7 files changed, 81 insertions(+), 75 deletions(-) diff --git a/pybricks/common.h b/pybricks/common.h index 40cd18d14..246dd881e 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -113,7 +113,7 @@ struct _common_Motor_obj_t { mp_obj_t logger; #endif pbio_port_id_t port; - pb_type_awaitable_obj_t *first_awaitable; + mp_obj_t awaitables; }; extern const mp_obj_type_t pb_type_Motor; diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 9f33ddf6f..3f0a7058e 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -124,7 +124,9 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, self->logger = common_Logger_obj_make_new(&self->srv->log, PBIO_SERVO_LOGGER_NUM_COLS); #endif - self->first_awaitable = NULL; + // List of awaitables associated with this motor. By keeping track, + // we can cancel them as needed when a new movement is started. + self->awaitables = mp_obj_new_list(0, NULL); return MP_OBJ_FROM_PTR(self); } @@ -149,7 +151,7 @@ STATIC void common_Motor_cancel(mp_obj_t self_in) { STATIC mp_obj_t await_or_wait(common_Motor_obj_t *self) { return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - &self->first_awaitable, + self->awaitables, common_Motor_test_completion, pb_type_awaitable_return_none, common_Motor_cancel, @@ -180,7 +182,7 @@ STATIC mp_obj_t common_Motor_reset_angle(size_t n_args, const mp_obj_t *pos_args // Set the new angle pb_assert(pbio_servo_reset_angle(self->srv, reset_angle, reset_to_abs)); - pb_type_awaitable_cancel_all(pos_args[0], self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_reset_angle_obj, 1, common_Motor_reset_angle); @@ -205,7 +207,7 @@ STATIC mp_obj_t common_Motor_run(size_t n_args, const mp_obj_t *pos_args, mp_map mp_int_t speed = pb_obj_get_int(speed_in); pb_assert(pbio_servo_run_forever(self->srv, speed)); - pb_type_awaitable_cancel_all(pos_args[0], self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); @@ -214,7 +216,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); STATIC mp_obj_t common_Motor_hold(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_HOLD)); - pb_type_awaitable_cancel_all(self_in, self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self_in, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_hold_obj, common_Motor_hold); @@ -285,7 +287,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po // Handle completion by awaiting or blocking. return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - &self->first_awaitable, + self->awaitables, common_Motor_test_completion, common_Motor_stall_return_value, common_Motor_cancel, @@ -351,7 +353,7 @@ STATIC mp_obj_t common_Motor_track_target(size_t n_args, const mp_obj_t *pos_arg mp_int_t target_angle = pb_obj_get_int(target_angle_in); pb_assert(pbio_servo_track_target(self->srv, target_angle)); - pb_type_awaitable_cancel_all(pos_args[0], self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_track_target_obj, 1, common_Motor_track_target); diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 5415477ff..22049c046 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -34,7 +34,7 @@ typedef struct { uint32_t note_duration; uint32_t beep_end_time; uint32_t release_end_time; - pb_type_awaitable_obj_t *first_awaitable; + mp_obj_t awaitables; // volume in 0..100 range uint8_t volume; @@ -115,7 +115,10 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg if (!self->initialized) { self->base.type = &pb_type_Speaker; self->initialized = true; - self->first_awaitable = NULL; + + // List of awaitables associated with speaker. By keeping track, + // we can cancel them as needed when a new sound is started. + self->awaitables = mp_obj_new_list(0, NULL); } // REVISIT: If a user creates two Speaker instances, this will reset the volume settings for both. @@ -164,7 +167,7 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - &self->first_awaitable, + self->awaitables, pb_type_Speaker_beep_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, @@ -379,7 +382,7 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar self->release_end_time = self->beep_end_time; return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - &self->first_awaitable, + self->awaitables, pb_type_Speaker_notes_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 8264eccd2..14cc8dc1e 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -35,7 +35,7 @@ struct _pb_type_DriveBase_obj_t { mp_obj_t heading_control; mp_obj_t distance_control; #endif - pb_type_awaitable_obj_t *first_awaitable; + mp_obj_t awaitables; }; // pybricks.robotics.DriveBase.reset @@ -85,7 +85,9 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a // Reset drivebase state pb_type_DriveBase_reset(MP_OBJ_FROM_PTR(self)); - self->first_awaitable = NULL; + // List of awaitables associated with this drivebase. By keeping track, + // we can cancel them as needed when a new movement is started. + self->awaitables = mp_obj_new_list(0, NULL); return MP_OBJ_FROM_PTR(self); } @@ -112,7 +114,7 @@ STATIC void pb_type_DriveBase_cancel(mp_obj_t self_in) { STATIC mp_obj_t await_or_wait(pb_type_DriveBase_obj_t *self) { return pb_type_awaitable_await_or_wait( MP_OBJ_FROM_PTR(self), - &self->first_awaitable, + self->awaitables, pb_type_DriveBase_test_completion, pb_type_awaitable_return_none, pb_type_DriveBase_cancel, @@ -200,7 +202,7 @@ STATIC mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, mp_int_t turn_rate = pb_obj_get_int(turn_rate_in); // Cancel awaitables but not hardware. Drive forever will handle this. - pb_type_awaitable_cancel_all(pos_args[0], self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); pb_assert(pbio_drivebase_drive_forever(self->db, speed, turn_rate)); return mp_const_none; @@ -212,7 +214,7 @@ STATIC mp_obj_t pb_type_DriveBase_stop(mp_obj_t self_in) { // Cancel awaitables. pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_type_awaitable_cancel_all(self_in, self->first_awaitable, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self_in, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); // Stop hardware. pb_type_DriveBase_cancel(self_in); diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index cc20fdf9e..f0022578e 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -22,10 +22,26 @@ #include #include -// The awaitable for the wait() function has no object associated with -// it (unlike e.g. a motor), so we make a starting point here. This gets -// cleared when the module initializes. -STATIC pb_type_awaitable_obj_t *first_wait_awaitable = NULL; + +// Global state of the run loop for async user programs. Gets set when run_task +// is called and cleared when it completes +STATIC bool run_loop_is_active; + +bool pb_module_tools_run_loop_is_active() { + return run_loop_is_active; +} + +// The awaitables for the wait() function have no object associated with +// it (unlike e.g. a motor), so we make a starting point here. These never +// have to cancel each other so shouldn't need to be in a list, but this lets +// us share the same code with other awaitables. It also minimizes allocation. +MP_REGISTER_ROOT_POINTER(mp_obj_t wait_awaitables); + +// Reset global state when user program starts. +void pb_module_tools_init(void) { + MP_STATE_PORT(wait_awaitables) = mp_obj_new_list(0, NULL); + run_loop_is_active = false; +} STATIC bool pb_module_tools_wait_test_completion(mp_obj_t obj, uint32_t start_time) { // obj was validated to be small int, so we can do a cheap comparison here. @@ -52,7 +68,7 @@ STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp return pb_type_awaitable_await_or_wait( MP_OBJ_NEW_SMALL_INT(time), - &first_wait_awaitable, + MP_STATE_PORT(wait_awaitables), pb_module_tools_wait_test_completion, pb_type_awaitable_return_none, pb_type_awaitable_cancel_none, @@ -60,20 +76,6 @@ STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_module_tools_wait_obj, 0, pb_module_tools_wait); -// Global state of the run loop for async user programs. Gets set when run_task -// is called and cleared when it completes -STATIC bool run_loop_is_active; - -bool pb_module_tools_run_loop_is_active() { - return run_loop_is_active; -} - -// Reset global state when user program starts. -void pb_module_tools_init(void) { - first_wait_awaitable = NULL; - run_loop_is_active = false; -} - STATIC mp_obj_t pb_module_tools_run_task(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_FUNCTION(n_args, pos_args, kw_args, PB_ARG_REQUIRED(task), diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 130988fe8..e5ff8c96a 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -7,6 +7,7 @@ #include "py/mphal.h" #include "py/mpstate.h" +#include "py/obj.h" #include "py/runtime.h" #include @@ -38,10 +39,6 @@ struct _pb_type_awaitable_obj_t { * Called on cancellation. */ pb_type_awaitable_cancel_t cancel; - /** - * Linked list of awaitables. - */ - pb_type_awaitable_obj_t *next_awaitable; }; // close() cancels the awaitable. @@ -94,26 +91,31 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_awaitable, iter, pb_type_awaitable_iternext, locals_dict, &pb_type_awaitable_locals_dict); -STATIC pb_type_awaitable_obj_t *pb_type_awaitable_get(pb_type_awaitable_obj_t *first_awaitable) { +/** + * Gets an awaitable object that is not in use, or makes a new one. + * + * @param [in] awaitables_in List of awaitables associated with @p obj. + */ +STATIC pb_type_awaitable_obj_t *pb_type_awaitable_get(mp_obj_t awaitables_in) { - // Find next available awaitable that exists and is not used. - pb_type_awaitable_obj_t *awaitable = first_awaitable; - while (awaitable->test_completion != AWAITABLE_FREE && awaitable->next_awaitable) { - awaitable = awaitable->next_awaitable; - } + mp_obj_list_t *awaitables = MP_OBJ_TO_PTR(awaitables_in); - // Loop stopped because available awaitable was found, so return it. - if (awaitable->test_completion == AWAITABLE_FREE) { - return awaitable; - } + for (size_t i = 0; i < awaitables->len; i++) { + pb_type_awaitable_obj_t *awaitable = MP_OBJ_TO_PTR(awaitables->items[i]); - // Otherwise no available awaitable was found, so allocate a new one - awaitable->next_awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); + // Return awaitable if it is not in use. + if (awaitable->test_completion == AWAITABLE_FREE) { + return awaitable; + } + } - // Initialize the new awaitable. - awaitable = awaitable->next_awaitable; + // Otherwise allocate a new one. + pb_type_awaitable_obj_t *awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); awaitable->test_completion = AWAITABLE_FREE; - awaitable->next_awaitable = NULL; + + // Add to list of awaitables. + mp_obj_list_append(awaitables_in, MP_OBJ_FROM_PTR(awaitable)); + return awaitable; } @@ -135,18 +137,20 @@ STATIC bool pb_type_awaitable_completed(mp_obj_t self_in, uint32_t start_time) { * also be called independently to cancel without starting a new awaitable. * * @param [in] obj The object whose method we want to wait for completion. - * @param [in] first_awaitable The first awaitable in the linked list of awaitables from @p obj. + * @param [in] awaitables_in List of awaitables associated with @p obj. * @param [in] cancel_opt Whether to cancel linked awaitables, hardware, or both. */ -void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_awaitable, pb_type_awaitable_cancel_opt_t cancel_opt) { +void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_awaitable_cancel_opt_t cancel_opt) { // Exit if nothing to do. - if (!pb_module_tools_run_loop_is_active() || cancel_opt == PB_TYPE_AWAITABLE_CANCEL_NONE || !first_awaitable) { + if (!pb_module_tools_run_loop_is_active() || cancel_opt == PB_TYPE_AWAITABLE_CANCEL_NONE) { return; } - pb_type_awaitable_obj_t *awaitable = first_awaitable; - while (awaitable) { + mp_obj_list_t *awaitables = MP_OBJ_TO_PTR(awaitables_in); + + for (size_t i = 0; i < awaitables->len; i++) { + pb_type_awaitable_obj_t *awaitable = MP_OBJ_TO_PTR(awaitables->items[i]); // Only cancel awaitables that are in use. if (awaitable->test_completion) { // Cancel hardware operation if requested and available. @@ -159,7 +163,6 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_a awaitable->test_completion = pb_type_awaitable_completed; } } - awaitable = awaitable->next_awaitable; } } @@ -169,7 +172,7 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_a * Automatically cancels any previous awaitables associated with the object if requested. * * @param [in] obj The object whose method we want to wait for completion. - * @param [in] first_awaitable The first awaitable in the linked list of awaitables from @p obj. + * @param [in] awaitables_in List of awaitables associated with @p obj. * @param [in] test_completion_func Function to test if the operation is complete. * @param [in] return_value_func Function that gets the return value for the awaitable. * @param [in] cancel_func Function to cancel the hardware operation. @@ -177,7 +180,7 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_a */ mp_obj_t pb_type_awaitable_await_or_wait( mp_obj_t obj, - pb_type_awaitable_obj_t **first_awaitable, + mp_obj_t awaitables_in, pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, pb_type_awaitable_cancel_t cancel_func, @@ -187,18 +190,12 @@ mp_obj_t pb_type_awaitable_await_or_wait( // Within run loop, return the generator that user program will iterate. if (pb_module_tools_run_loop_is_active()) { - // First cancel linked awaitables if requested. - pb_type_awaitable_cancel_all(obj, *first_awaitable, cancel_opt); - // If the first awaitable was not yet created, do so now. - if (!*first_awaitable) { - *first_awaitable = mp_obj_malloc(pb_type_awaitable_obj_t, &pb_type_awaitable); - (*first_awaitable)->test_completion = AWAITABLE_FREE; - (*first_awaitable)->next_awaitable = NULL; - } + // First cancel linked awaitables if requested. + pb_type_awaitable_cancel_all(obj, awaitables_in, cancel_opt); - // Gets existing awaitable or creates a new one. - pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(*first_awaitable); + // Gets free existing awaitable or creates a new one. + pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(awaitables_in); // Initialize awaitable. awaitable->obj = obj; diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index b1c400435..7e80ef14f 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -69,11 +69,11 @@ typedef void (*pb_type_awaitable_cancel_t)(mp_obj_t obj); #define pb_type_awaitable_cancel_none (NULL) -void pb_type_awaitable_cancel_all(mp_obj_t obj, pb_type_awaitable_obj_t *first_awaitable, pb_type_awaitable_cancel_opt_t cancel_opt); +void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_awaitable_cancel_opt_t cancel_opt); mp_obj_t pb_type_awaitable_await_or_wait( mp_obj_t obj, - pb_type_awaitable_obj_t **first_awaitable, + mp_obj_t awaitables_in, pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, pb_type_awaitable_cancel_t cancel_func, From 8893a4200c105c627733ab6ba8845dbf10627c5f Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 15 May 2023 11:12:17 +0200 Subject: [PATCH 46/59] pybricks/tools/pb_type_awaitable: Generalize object type. This allows us to await processes without going through an explicit MicroPython object. This doesn't make a difference for the awaitables implemented so far, but this will be useful for Bluetooth and sensor awaitables. For example, for pupdevices the void *object can be the *iodev so that the same awaitable type can be used for all sensors, even though each of the sensor_obj_t are all slightly different. --- pybricks/common/pb_type_motor.c | 33 ++++++++++++++------------- pybricks/common/pb_type_speaker.c | 16 ++++++------- pybricks/robotics/pb_type_drivebase.c | 22 +++++++++--------- pybricks/tools/pb_module_tools.c | 13 ++++------- pybricks/tools/pb_type_awaitable.c | 32 +++++++++++++------------- pybricks/tools/pb_type_awaitable.h | 16 ++++++------- 6 files changed, 65 insertions(+), 67 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 3f0a7058e..3eeb3023a 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -131,26 +131,27 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, return MP_OBJ_FROM_PTR(self); } -STATIC bool common_Motor_test_completion(mp_obj_t self_in, uint32_t start_time) { - common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); +STATIC bool common_Motor_test_completion(void *object, uint32_t start_time) { + pbio_servo_t *srv = object; + // Handle I/O exceptions like port unplugged. - if (!pbio_servo_update_loop_is_running(self->srv)) { + if (!pbio_servo_update_loop_is_running(srv)) { pb_assert(PBIO_ERROR_NO_DEV); } // Get completion state. - return pbio_control_is_done(&self->srv->control); + return pbio_control_is_done(&srv->control); } -STATIC void common_Motor_cancel(mp_obj_t self_in) { - common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_COAST)); +STATIC void common_Motor_cancel(void *object) { + pbio_servo_t *srv = object; + pb_assert(pbio_servo_stop(srv, PBIO_CONTROL_ON_COMPLETION_COAST)); } // Common awaitable used for most motor methods. STATIC mp_obj_t await_or_wait(common_Motor_obj_t *self) { return pb_type_awaitable_await_or_wait( - MP_OBJ_FROM_PTR(self), + self->srv, self->awaitables, common_Motor_test_completion, pb_type_awaitable_return_none, @@ -182,7 +183,7 @@ STATIC mp_obj_t common_Motor_reset_angle(size_t n_args, const mp_obj_t *pos_args // Set the new angle pb_assert(pbio_servo_reset_angle(self->srv, reset_angle, reset_to_abs)); - pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_reset_angle_obj, 1, common_Motor_reset_angle); @@ -207,7 +208,7 @@ STATIC mp_obj_t common_Motor_run(size_t n_args, const mp_obj_t *pos_args, mp_map mp_int_t speed = pb_obj_get_int(speed_in); pb_assert(pbio_servo_run_forever(self->srv, speed)); - pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); @@ -216,7 +217,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); STATIC mp_obj_t common_Motor_hold(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_HOLD)); - pb_type_awaitable_cancel_all(self_in, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_hold_obj, common_Motor_hold); @@ -247,13 +248,13 @@ STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, m } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_time_obj, 1, common_Motor_run_time); -STATIC mp_obj_t common_Motor_stall_return_value(mp_obj_t self_in) { +STATIC mp_obj_t common_Motor_stall_return_value(void *object) { - common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); + pbio_servo_t *srv = object; // Return the angle upon completion of the stall maneuver. int32_t stall_angle, stall_speed; - pb_assert(pbio_servo_get_state_user(self->srv, &stall_angle, &stall_speed)); + pb_assert(pbio_servo_get_state_user(srv, &stall_angle, &stall_speed)); return mp_obj_new_int(stall_angle); } @@ -286,7 +287,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po // Handle completion by awaiting or blocking. return pb_type_awaitable_await_or_wait( - MP_OBJ_FROM_PTR(self), + self->srv, self->awaitables, common_Motor_test_completion, common_Motor_stall_return_value, @@ -353,7 +354,7 @@ STATIC mp_obj_t common_Motor_track_target(size_t n_args, const mp_obj_t *pos_arg mp_int_t target_angle = pb_obj_get_int(target_angle_in); pb_assert(pbio_servo_track_target(self->srv, target_angle)); - pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_track_target_obj, 1, common_Motor_track_target); diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 22049c046..d0d60191c 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -129,8 +129,8 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg return MP_OBJ_FROM_PTR(self); } -STATIC bool pb_type_Speaker_beep_test_completion(mp_obj_t self_in, uint32_t start_time) { - pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); +STATIC bool pb_type_Speaker_beep_test_completion(void *object, uint32_t start_time) { + pb_type_Speaker_obj_t *self = object; if (mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX) { pb_type_Speaker_stop_beep(); return true; @@ -138,9 +138,9 @@ STATIC bool pb_type_Speaker_beep_test_completion(mp_obj_t self_in, uint32_t star return false; } -STATIC void pb_type_Speaker_cancel(mp_obj_t self_in) { +STATIC void pb_type_Speaker_cancel(void *object) { pb_type_Speaker_stop_beep(); - pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); + pb_type_Speaker_obj_t *self = object; self->beep_end_time = mp_hal_ticks_ms(); self->release_end_time = self->beep_end_time; self->notes_generator = MP_OBJ_NULL; @@ -166,7 +166,7 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp self->notes_generator = MP_OBJ_NULL; return pb_type_awaitable_await_or_wait( - MP_OBJ_FROM_PTR(self), + self, self->awaitables, pb_type_Speaker_beep_test_completion, pb_type_awaitable_return_none, @@ -342,8 +342,8 @@ STATIC void pb_type_Speaker_play_note(pb_type_Speaker_obj_t *self, mp_obj_t obj, self->beep_end_time = release ? time_now + 7 * duration / 8 : time_now + duration; } -STATIC bool pb_type_Speaker_notes_test_completion(mp_obj_t self_in, uint32_t start_time) { - pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); +STATIC bool pb_type_Speaker_notes_test_completion(void *object, uint32_t start_time) { + pb_type_Speaker_obj_t *self = object; bool release_done = mp_hal_ticks_ms() - self->release_end_time < (uint32_t)INT32_MAX; bool beep_done = mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX; @@ -381,7 +381,7 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar self->beep_end_time = mp_hal_ticks_ms(); self->release_end_time = self->beep_end_time; return pb_type_awaitable_await_or_wait( - MP_OBJ_FROM_PTR(self), + self, self->awaitables, pb_type_Speaker_notes_test_completion, pb_type_awaitable_return_none, diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 14cc8dc1e..0f14a6f34 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -92,28 +92,28 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a return MP_OBJ_FROM_PTR(self); } -STATIC bool pb_type_DriveBase_test_completion(mp_obj_t self_in, uint32_t start_time) { +STATIC bool pb_type_DriveBase_test_completion(void *object, uint32_t start_time) { - pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); + pbio_drivebase_t *db = object; // Handle I/O exceptions like port unplugged. - if (!pbio_drivebase_update_loop_is_running(self->db)) { + if (!pbio_drivebase_update_loop_is_running(db)) { pb_assert(PBIO_ERROR_NO_DEV); } // Get completion state. - return pbio_drivebase_is_done(self->db); + return pbio_drivebase_is_done(db); } -STATIC void pb_type_DriveBase_cancel(mp_obj_t self_in) { - pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_assert(pbio_drivebase_stop(self->db, PBIO_CONTROL_ON_COMPLETION_COAST)); +STATIC void pb_type_DriveBase_cancel(void *object) { + pbio_drivebase_t *db = object; + pb_assert(pbio_drivebase_stop(db, PBIO_CONTROL_ON_COMPLETION_COAST)); } // All drive base methods use the same kind of completion awaitable. STATIC mp_obj_t await_or_wait(pb_type_DriveBase_obj_t *self) { return pb_type_awaitable_await_or_wait( - MP_OBJ_FROM_PTR(self), + self->db, self->awaitables, pb_type_DriveBase_test_completion, pb_type_awaitable_return_none, @@ -202,7 +202,7 @@ STATIC mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, mp_int_t turn_rate = pb_obj_get_int(turn_rate_in); // Cancel awaitables but not hardware. Drive forever will handle this. - pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); pb_assert(pbio_drivebase_drive_forever(self->db, speed, turn_rate)); return mp_const_none; @@ -214,10 +214,10 @@ STATIC mp_obj_t pb_type_DriveBase_stop(mp_obj_t self_in) { // Cancel awaitables. pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_type_awaitable_cancel_all(self_in, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); // Stop hardware. - pb_type_DriveBase_cancel(self_in); + pb_type_DriveBase_cancel(self->db); return mp_const_none; } diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index f0022578e..025b84201 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -43,9 +43,10 @@ void pb_module_tools_init(void) { run_loop_is_active = false; } -STATIC bool pb_module_tools_wait_test_completion(mp_obj_t obj, uint32_t start_time) { - // obj was validated to be small int, so we can do a cheap comparison here. - return mp_hal_ticks_ms() - start_time >= (uint32_t)MP_OBJ_SMALL_INT_VALUE(obj); +STATIC bool pb_module_tools_wait_test_completion(void *object, uint32_t start_time) { + // There is no object associated with wait. The object pointer encodes the duration. + uint32_t duration = (uintptr_t)(object); + return mp_hal_ticks_ms() - start_time >= duration; } STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -62,12 +63,8 @@ STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp return mp_const_none; } - // Require that duration is nonnegative small int. This makes it cheaper to - // test completion state in iteration loop. - time = pbio_int_math_bind(time, 0, INT32_MAX >> 2); - return pb_type_awaitable_await_or_wait( - MP_OBJ_NEW_SMALL_INT(time), + (void *)(uintptr_t)(time < 0 ? 0 : time), MP_STATE_PORT(wait_awaitables), pb_module_tools_wait_test_completion, pb_type_awaitable_return_none, diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index e5ff8c96a..39e44f34f 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -21,7 +21,7 @@ struct _pb_type_awaitable_obj_t { /** * Object associated with this awaitable, such as the motor we wait on. */ - mp_obj_t obj; + void *object; /** * Start time. Gets passed to completion test to allow for graceful timeout. */ @@ -47,7 +47,7 @@ STATIC mp_obj_t pb_type_awaitable_close(mp_obj_t self_in) { self->test_completion = AWAITABLE_FREE; // Handle optional clean up/cancelling of hardware operation. if (self->cancel) { - self->cancel(self->obj); + self->cancel(self->object); } return mp_const_none; } @@ -62,7 +62,7 @@ STATIC mp_obj_t pb_type_awaitable_iternext(mp_obj_t self_in) { } // Keep going if not completed by returning None. - if (!self->test_completion(self->obj, self->start_time)) { + if (!self->test_completion(self->object, self->start_time)) { return mp_const_none; } @@ -75,7 +75,7 @@ STATIC mp_obj_t pb_type_awaitable_iternext(mp_obj_t self_in) { } // Otherwise, set return value via stop iteration. - return mp_make_stop_iteration(self->return_value(self->obj)); + return mp_make_stop_iteration(self->return_value(self->object)); } STATIC const mp_rom_map_elem_t pb_type_awaitable_locals_dict_table[] = { @@ -94,7 +94,7 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_awaitable, /** * Gets an awaitable object that is not in use, or makes a new one. * - * @param [in] awaitables_in List of awaitables associated with @p obj. + * @param [in] awaitables_in List of awaitables associated with @p object. */ STATIC pb_type_awaitable_obj_t *pb_type_awaitable_get(mp_obj_t awaitables_in) { @@ -136,11 +136,11 @@ STATIC bool pb_type_awaitable_completed(mp_obj_t self_in, uint32_t start_time) { * This is normally used by the function that makes a new awaitable, but it can * also be called independently to cancel without starting a new awaitable. * - * @param [in] obj The object whose method we want to wait for completion. - * @param [in] awaitables_in List of awaitables associated with @p obj. + * @param [in] object The object whose method we want to wait for completion. + * @param [in] awaitables_in List of awaitables associated with @p object. * @param [in] cancel_opt Whether to cancel linked awaitables, hardware, or both. */ -void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_awaitable_cancel_opt_t cancel_opt) { +void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_awaitable_cancel_opt_t cancel_opt) { // Exit if nothing to do. if (!pb_module_tools_run_loop_is_active() || cancel_opt == PB_TYPE_AWAITABLE_CANCEL_NONE) { @@ -155,7 +155,7 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_ if (awaitable->test_completion) { // Cancel hardware operation if requested and available. if (cancel_opt & PB_TYPE_AWAITABLE_CANCEL_CALLBACK && awaitable->cancel) { - awaitable->cancel(awaitable->obj); + awaitable->cancel(awaitable->object); } // Set awaitable to done so it gets cancelled it gracefully on the // next iteration. @@ -171,15 +171,15 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_ * * Automatically cancels any previous awaitables associated with the object if requested. * - * @param [in] obj The object whose method we want to wait for completion. - * @param [in] awaitables_in List of awaitables associated with @p obj. + * @param [in] object The object whose method we want to wait for completion. + * @param [in] awaitables_in List of awaitables associated with @p object. * @param [in] test_completion_func Function to test if the operation is complete. * @param [in] return_value_func Function that gets the return value for the awaitable. * @param [in] cancel_func Function to cancel the hardware operation. * @param [in] cancel_opt Whether to cancel linked awaitables, hardware, or both. */ mp_obj_t pb_type_awaitable_await_or_wait( - mp_obj_t obj, + void *object, mp_obj_t awaitables_in, pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, @@ -192,13 +192,13 @@ mp_obj_t pb_type_awaitable_await_or_wait( if (pb_module_tools_run_loop_is_active()) { // First cancel linked awaitables if requested. - pb_type_awaitable_cancel_all(obj, awaitables_in, cancel_opt); + pb_type_awaitable_cancel_all(object, awaitables_in, cancel_opt); // Gets free existing awaitable or creates a new one. pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(awaitables_in); // Initialize awaitable. - awaitable->obj = obj; + awaitable->object = object; awaitable->test_completion = test_completion_func; awaitable->return_value = return_value_func; awaitable->cancel = cancel_func; @@ -207,13 +207,13 @@ mp_obj_t pb_type_awaitable_await_or_wait( } // Outside run loop, block until the operation is complete. - while (test_completion_func && !test_completion_func(obj, start_time)) { + while (test_completion_func && !test_completion_func(object, start_time)) { mp_hal_delay_ms(1); } if (!return_value_func) { return mp_const_none; } - return return_value_func(obj); + return return_value_func(object); } #endif // PYBRICKS_PY_TOOLS diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index 7e80ef14f..15ea4caee 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -42,37 +42,37 @@ typedef struct _pb_type_awaitable_obj_t pb_type_awaitable_obj_t; * operations as needed (hold a motor, etc.). This is not the same as cancel * below, which always stops the relevant hardware (i.e. always coast). * - * @param [in] obj The object associated with this awaitable. + * @param [in] object The object/device associated with this awaitable. * @param [in] start_time The time when the awaitable was created. * @return True if operation is complete, False otherwise. */ -typedef bool (*pb_type_awaitable_test_completion_t)(mp_obj_t obj, uint32_t start_time); +typedef bool (*pb_type_awaitable_test_completion_t)(void *object, uint32_t start_time); /** * Gets the return value of the awaitable. If it always returns None, providing * this function is not necessary. * - * @param [in] obj The object associated with this awaitable. + * @param [in] object The object/device associated with this awaitable. * @return The return value of the awaitable. */ -typedef mp_obj_t (*pb_type_awaitable_return_t)(mp_obj_t obj); +typedef mp_obj_t (*pb_type_awaitable_return_t)(void *object); /** * Called on cancel/close. Used to stop hardware operation in unhandled * conditions. * - * @param [in] obj The object associated with this awaitable. + * @param [in] object The object/device associated with this awaitable. */ -typedef void (*pb_type_awaitable_cancel_t)(mp_obj_t obj); +typedef void (*pb_type_awaitable_cancel_t)(void *object); #define pb_type_awaitable_return_none (NULL) #define pb_type_awaitable_cancel_none (NULL) -void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_awaitable_cancel_opt_t cancel_opt); +void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_awaitable_cancel_opt_t cancel_opt); mp_obj_t pb_type_awaitable_await_or_wait( - mp_obj_t obj, + void *object, mp_obj_t awaitables_in, pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, From 9c910ea79e9b51f7889218892a56af94485fd581 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 15 May 2023 11:47:15 +0200 Subject: [PATCH 47/59] pybricks/tools/pb_type_awaitable: Refactor options. This generalizes it to awaitable options instead of options strictly about cancellation. No code functionality is changed in this commit. --- pybricks/common/pb_type_motor.c | 12 ++++++------ pybricks/common/pb_type_speaker.c | 4 ++-- pybricks/robotics/pb_type_drivebase.c | 6 +++--- pybricks/tools/pb_module_tools.c | 2 +- pybricks/tools/pb_type_awaitable.c | 16 ++++++++-------- pybricks/tools/pb_type_awaitable.h | 22 +++++++++++----------- 6 files changed, 31 insertions(+), 31 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 3eeb3023a..1f9bc9013 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -156,7 +156,7 @@ STATIC mp_obj_t await_or_wait(common_Motor_obj_t *self) { common_Motor_test_completion, pb_type_awaitable_return_none, common_Motor_cancel, - PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + PB_TYPE_AWAITABLE_CANCEL_LINKED); } // pybricks._common.Motor.angle @@ -183,7 +183,7 @@ STATIC mp_obj_t common_Motor_reset_angle(size_t n_args, const mp_obj_t *pos_args // Set the new angle pb_assert(pbio_servo_reset_angle(self->srv, reset_angle, reset_to_abs)); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_reset_angle_obj, 1, common_Motor_reset_angle); @@ -208,7 +208,7 @@ STATIC mp_obj_t common_Motor_run(size_t n_args, const mp_obj_t *pos_args, mp_map mp_int_t speed = pb_obj_get_int(speed_in); pb_assert(pbio_servo_run_forever(self->srv, speed)); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); @@ -217,7 +217,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); STATIC mp_obj_t common_Motor_hold(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_HOLD)); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_hold_obj, common_Motor_hold); @@ -292,7 +292,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po common_Motor_test_completion, common_Motor_stall_return_value, common_Motor_cancel, - PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + PB_TYPE_AWAITABLE_CANCEL_LINKED); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_until_stalled_obj, 1, common_Motor_run_until_stalled); @@ -354,7 +354,7 @@ STATIC mp_obj_t common_Motor_track_target(size_t n_args, const mp_obj_t *pos_arg mp_int_t target_angle = pb_obj_get_int(target_angle_in); pb_assert(pbio_servo_track_target(self->srv, target_angle)); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_track_target_obj, 1, common_Motor_track_target); diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index d0d60191c..25f029446 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -171,7 +171,7 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp pb_type_Speaker_beep_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, - PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK); + PB_TYPE_AWAITABLE_CANCEL_LINKED | PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_beep_obj, 1, pb_type_Speaker_beep); @@ -386,7 +386,7 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar pb_type_Speaker_notes_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, - PB_TYPE_AWAITABLE_CANCEL_AWAITABLE | PB_TYPE_AWAITABLE_CANCEL_CALLBACK); + PB_TYPE_AWAITABLE_CANCEL_LINKED | PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_play_notes_obj, 1, pb_type_Speaker_play_notes); diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 0f14a6f34..e501196f3 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -118,7 +118,7 @@ STATIC mp_obj_t await_or_wait(pb_type_DriveBase_obj_t *self) { pb_type_DriveBase_test_completion, pb_type_awaitable_return_none, pb_type_DriveBase_cancel, - PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + PB_TYPE_AWAITABLE_CANCEL_LINKED); } // pybricks.robotics.DriveBase.straight @@ -202,7 +202,7 @@ STATIC mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, mp_int_t turn_rate = pb_obj_get_int(turn_rate_in); // Cancel awaitables but not hardware. Drive forever will handle this. - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); pb_assert(pbio_drivebase_drive_forever(self->db, speed, turn_rate)); return mp_const_none; @@ -214,7 +214,7 @@ STATIC mp_obj_t pb_type_DriveBase_stop(mp_obj_t self_in) { // Cancel awaitables. pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_AWAITABLE); + pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); // Stop hardware. pb_type_DriveBase_cancel(self->db); diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 025b84201..58324e23c 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -69,7 +69,7 @@ STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp pb_module_tools_wait_test_completion, pb_type_awaitable_return_none, pb_type_awaitable_cancel_none, - PB_TYPE_AWAITABLE_CANCEL_NONE); + PB_TYPE_AWAITABLE_OPT_NONE); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_module_tools_wait_obj, 0, pb_module_tools_wait); diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 39e44f34f..f1cf5b331 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -138,12 +138,12 @@ STATIC bool pb_type_awaitable_completed(mp_obj_t self_in, uint32_t start_time) { * * @param [in] object The object whose method we want to wait for completion. * @param [in] awaitables_in List of awaitables associated with @p object. - * @param [in] cancel_opt Whether to cancel linked awaitables, hardware, or both. + * @param [in] options Controls awaitable behavior. */ -void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_awaitable_cancel_opt_t cancel_opt) { +void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_awaitable_opt_t options) { // Exit if nothing to do. - if (!pb_module_tools_run_loop_is_active() || cancel_opt == PB_TYPE_AWAITABLE_CANCEL_NONE) { + if (!pb_module_tools_run_loop_is_active() || options == PB_TYPE_AWAITABLE_OPT_NONE) { return; } @@ -154,12 +154,12 @@ void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_ // Only cancel awaitables that are in use. if (awaitable->test_completion) { // Cancel hardware operation if requested and available. - if (cancel_opt & PB_TYPE_AWAITABLE_CANCEL_CALLBACK && awaitable->cancel) { + if (options & PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK && awaitable->cancel) { awaitable->cancel(awaitable->object); } // Set awaitable to done so it gets cancelled it gracefully on the // next iteration. - if (cancel_opt & PB_TYPE_AWAITABLE_CANCEL_AWAITABLE) { + if (options & PB_TYPE_AWAITABLE_CANCEL_LINKED) { awaitable->test_completion = pb_type_awaitable_completed; } } @@ -176,7 +176,7 @@ void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_ * @param [in] test_completion_func Function to test if the operation is complete. * @param [in] return_value_func Function that gets the return value for the awaitable. * @param [in] cancel_func Function to cancel the hardware operation. - * @param [in] cancel_opt Whether to cancel linked awaitables, hardware, or both. + * @param [in] options Controls awaitable behavior. */ mp_obj_t pb_type_awaitable_await_or_wait( void *object, @@ -184,7 +184,7 @@ mp_obj_t pb_type_awaitable_await_or_wait( pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, pb_type_awaitable_cancel_t cancel_func, - pb_type_awaitable_cancel_opt_t cancel_opt) { + pb_type_awaitable_opt_t options) { uint32_t start_time = mp_hal_ticks_ms(); @@ -192,7 +192,7 @@ mp_obj_t pb_type_awaitable_await_or_wait( if (pb_module_tools_run_loop_is_active()) { // First cancel linked awaitables if requested. - pb_type_awaitable_cancel_all(object, awaitables_in, cancel_opt); + pb_type_awaitable_cancel_all(object, awaitables_in, options); // Gets free existing awaitable or creates a new one. pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(awaitables_in); diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index 15ea4caee..3efafbb82 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -13,22 +13,22 @@ /** * Options for canceling an awaitable. */ -typedef enum _pb_type_awaitable_cancel_opt_t { - /** Do nothing to linked awaitables. */ - PB_TYPE_AWAITABLE_CANCEL_NONE = 0, +typedef enum _pb_type_awaitable_opt_t { + /** No options. */ + PB_TYPE_AWAITABLE_OPT_NONE = 0, /** * Makes all linked awaitables end gracefully. Can be used if awaitables * running in parallel are using the same resources. This way, the newly * started operation "wins" and everything else is cancelled. */ - PB_TYPE_AWAITABLE_CANCEL_AWAITABLE = 1 << 1, + PB_TYPE_AWAITABLE_CANCEL_LINKED = 1 << 1, /** - * Calls the cancel function for each linked awaitable that is not already - * done. Only used to close hardware resources that aren't already cleaned - * up by lower level drivers. + * On cancelling the linked awaitables, also call their cancel function + * to stop hardware. Only used to close hardware resources that aren't + * already cleaned up by lower level drivers (so not needed for motors). */ - PB_TYPE_AWAITABLE_CANCEL_CALLBACK = 1 << 2, -} pb_type_awaitable_cancel_opt_t; + PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK = 1 << 2, +} pb_type_awaitable_opt_t; /** * A generator-like type for waiting on some operation to complete. @@ -69,7 +69,7 @@ typedef void (*pb_type_awaitable_cancel_t)(void *object); #define pb_type_awaitable_cancel_none (NULL) -void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_awaitable_cancel_opt_t cancel_opt); +void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_awaitable_opt_t options); mp_obj_t pb_type_awaitable_await_or_wait( void *object, @@ -77,7 +77,7 @@ mp_obj_t pb_type_awaitable_await_or_wait( pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, pb_type_awaitable_cancel_t cancel_func, - pb_type_awaitable_cancel_opt_t cancel_opt); + pb_type_awaitable_opt_t options); #endif // PYBRICKS_PY_TOOLS From 54db661d61143315d6d293e9d6a37b4716fb0cfe Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 15 May 2023 11:54:51 +0200 Subject: [PATCH 48/59] pybricks/tools/pb_type_awaitable: Add force block option. --- pybricks/tools/pb_type_awaitable.c | 5 +++++ pybricks/tools/pb_type_awaitable.h | 10 ++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index f1cf5b331..c35eb49d2 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -191,6 +191,11 @@ mp_obj_t pb_type_awaitable_await_or_wait( // Within run loop, return the generator that user program will iterate. if (pb_module_tools_run_loop_is_active()) { + // Some operations are not allowed in async mode. + if (options & PB_TYPE_AWAITABLE_OPT_FORCE_BLOCK) { + mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("This can only be called before multitasking starts.")); + } + // First cancel linked awaitables if requested. pb_type_awaitable_cancel_all(object, awaitables_in, options); diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index 3efafbb82..b087afbdb 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -16,18 +16,24 @@ typedef enum _pb_type_awaitable_opt_t { /** No options. */ PB_TYPE_AWAITABLE_OPT_NONE = 0, + /** + * Forces the awaitable to block until completion. Raises RuntimeError if + * called inside the run loop. This can be used to wait for operations like + * initializing a sensor or connecting to a remote. + */ + PB_TYPE_AWAITABLE_OPT_FORCE_BLOCK = 1 << 1, /** * Makes all linked awaitables end gracefully. Can be used if awaitables * running in parallel are using the same resources. This way, the newly * started operation "wins" and everything else is cancelled. */ - PB_TYPE_AWAITABLE_CANCEL_LINKED = 1 << 1, + PB_TYPE_AWAITABLE_CANCEL_LINKED = 1 << 2, /** * On cancelling the linked awaitables, also call their cancel function * to stop hardware. Only used to close hardware resources that aren't * already cleaned up by lower level drivers (so not needed for motors). */ - PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK = 1 << 2, + PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK = 1 << 3, } pb_type_awaitable_opt_t; /** From c62b0a9212309fd250eda0d8eb8363ac19825a48 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 15 May 2023 20:35:42 +0200 Subject: [PATCH 49/59] pybricks/tools/pb_type_awaitable: Use end time. Instead of start time. This allows for implementing a timeout without increasing the allocated size of the awaitable. It also means we can correctly store the time for a wait instead of hacking the duration into a pointer type. --- pybricks/common/pb_type_motor.c | 4 +++- pybricks/common/pb_type_speaker.c | 6 ++++-- pybricks/robotics/pb_type_drivebase.c | 3 ++- pybricks/tools/pb_module_tools.c | 12 ++++++------ pybricks/tools/pb_type_awaitable.c | 16 +++++++++------- pybricks/tools/pb_type_awaitable.h | 5 ++++- 6 files changed, 28 insertions(+), 18 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 1f9bc9013..1f885bc79 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -131,7 +131,7 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, return MP_OBJ_FROM_PTR(self); } -STATIC bool common_Motor_test_completion(void *object, uint32_t start_time) { +STATIC bool common_Motor_test_completion(void *object, uint32_t end_time) { pbio_servo_t *srv = object; // Handle I/O exceptions like port unplugged. @@ -153,6 +153,7 @@ STATIC mp_obj_t await_or_wait(common_Motor_obj_t *self) { return pb_type_awaitable_await_or_wait( self->srv, self->awaitables, + pb_type_awaitable_end_time_none, common_Motor_test_completion, pb_type_awaitable_return_none, common_Motor_cancel, @@ -289,6 +290,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po return pb_type_awaitable_await_or_wait( self->srv, self->awaitables, + pb_type_awaitable_end_time_none, common_Motor_test_completion, common_Motor_stall_return_value, common_Motor_cancel, diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 25f029446..944d9ba2f 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -129,7 +129,7 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg return MP_OBJ_FROM_PTR(self); } -STATIC bool pb_type_Speaker_beep_test_completion(void *object, uint32_t start_time) { +STATIC bool pb_type_Speaker_beep_test_completion(void *object, uint32_t end_time) { pb_type_Speaker_obj_t *self = object; if (mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX) { pb_type_Speaker_stop_beep(); @@ -168,6 +168,7 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp return pb_type_awaitable_await_or_wait( self, self->awaitables, + pb_type_awaitable_end_time_none, pb_type_Speaker_beep_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, @@ -342,7 +343,7 @@ STATIC void pb_type_Speaker_play_note(pb_type_Speaker_obj_t *self, mp_obj_t obj, self->beep_end_time = release ? time_now + 7 * duration / 8 : time_now + duration; } -STATIC bool pb_type_Speaker_notes_test_completion(void *object, uint32_t start_time) { +STATIC bool pb_type_Speaker_notes_test_completion(void *object, uint32_t end_time) { pb_type_Speaker_obj_t *self = object; bool release_done = mp_hal_ticks_ms() - self->release_end_time < (uint32_t)INT32_MAX; @@ -383,6 +384,7 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar return pb_type_awaitable_await_or_wait( self, self->awaitables, + pb_type_awaitable_end_time_none, pb_type_Speaker_notes_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index e501196f3..2c31a77d7 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -92,7 +92,7 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a return MP_OBJ_FROM_PTR(self); } -STATIC bool pb_type_DriveBase_test_completion(void *object, uint32_t start_time) { +STATIC bool pb_type_DriveBase_test_completion(void *object, uint32_t end_time) { pbio_drivebase_t *db = object; @@ -115,6 +115,7 @@ STATIC mp_obj_t await_or_wait(pb_type_DriveBase_obj_t *self) { return pb_type_awaitable_await_or_wait( self->db, self->awaitables, + pb_type_awaitable_end_time_none, pb_type_DriveBase_test_completion, pb_type_awaitable_return_none, pb_type_DriveBase_cancel, diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 58324e23c..395b51ae7 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -43,10 +43,8 @@ void pb_module_tools_init(void) { run_loop_is_active = false; } -STATIC bool pb_module_tools_wait_test_completion(void *object, uint32_t start_time) { - // There is no object associated with wait. The object pointer encodes the duration. - uint32_t duration = (uintptr_t)(object); - return mp_hal_ticks_ms() - start_time >= duration; +STATIC bool pb_module_tools_wait_test_completion(void *object, uint32_t end_time) { + return mp_hal_ticks_ms() - end_time < UINT32_MAX / 2; } STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -55,7 +53,8 @@ STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp mp_int_t time = pb_obj_get_int(time_in); - // outside run loop, do blocking wait. + // outside run loop, do blocking wait. This would be handled below as well, + // but for this very simple call we'd rather avoid the overhead. if (!pb_module_tools_run_loop_is_active()) { if (time > 0) { mp_hal_delay_ms(time); @@ -64,8 +63,9 @@ STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp } return pb_type_awaitable_await_or_wait( - (void *)(uintptr_t)(time < 0 ? 0 : time), + NULL, // wait functions are not associated with an object MP_STATE_PORT(wait_awaitables), + mp_hal_ticks_ms() + (time < 0 ? 0 : time), pb_module_tools_wait_test_completion, pb_type_awaitable_return_none, pb_type_awaitable_cancel_none, diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index c35eb49d2..37b47b628 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -23,9 +23,10 @@ struct _pb_type_awaitable_obj_t { */ void *object; /** - * Start time. Gets passed to completion test to allow for graceful timeout. + * End time. Gets passed to completion test to allow for graceful timeout + * or raise timeout errors if desired. */ - uint32_t start_time; + uint32_t end_time; /** * Tests if operation is complete. Gets reset to AWAITABLE_FREE * on completion, which means that it can be used again. @@ -62,7 +63,7 @@ STATIC mp_obj_t pb_type_awaitable_iternext(mp_obj_t self_in) { } // Keep going if not completed by returning None. - if (!self->test_completion(self->object, self->start_time)) { + if (!self->test_completion(self->object, self->end_time)) { return mp_const_none; } @@ -173,6 +174,8 @@ void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_ * * @param [in] object The object whose method we want to wait for completion. * @param [in] awaitables_in List of awaitables associated with @p object. + * @param [in] end_time Wall time in milliseconds when the operation should end. + * May be arbitrary if completion function does not need it. * @param [in] test_completion_func Function to test if the operation is complete. * @param [in] return_value_func Function that gets the return value for the awaitable. * @param [in] cancel_func Function to cancel the hardware operation. @@ -181,13 +184,12 @@ void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_ mp_obj_t pb_type_awaitable_await_or_wait( void *object, mp_obj_t awaitables_in, + uint32_t end_time, pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, pb_type_awaitable_cancel_t cancel_func, pb_type_awaitable_opt_t options) { - uint32_t start_time = mp_hal_ticks_ms(); - // Within run loop, return the generator that user program will iterate. if (pb_module_tools_run_loop_is_active()) { @@ -207,12 +209,12 @@ mp_obj_t pb_type_awaitable_await_or_wait( awaitable->test_completion = test_completion_func; awaitable->return_value = return_value_func; awaitable->cancel = cancel_func; - awaitable->start_time = start_time; + awaitable->end_time = end_time; return MP_OBJ_FROM_PTR(awaitable); } // Outside run loop, block until the operation is complete. - while (test_completion_func && !test_completion_func(object, start_time)) { + while (test_completion_func && !test_completion_func(object, end_time)) { mp_hal_delay_ms(1); } if (!return_value_func) { diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index b087afbdb..e098d4629 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -52,7 +52,7 @@ typedef struct _pb_type_awaitable_obj_t pb_type_awaitable_obj_t; * @param [in] start_time The time when the awaitable was created. * @return True if operation is complete, False otherwise. */ -typedef bool (*pb_type_awaitable_test_completion_t)(void *object, uint32_t start_time); +typedef bool (*pb_type_awaitable_test_completion_t)(void *object, uint32_t end_time); /** * Gets the return value of the awaitable. If it always returns None, providing @@ -71,6 +71,8 @@ typedef mp_obj_t (*pb_type_awaitable_return_t)(void *object); */ typedef void (*pb_type_awaitable_cancel_t)(void *object); +#define pb_type_awaitable_end_time_none (0) + #define pb_type_awaitable_return_none (NULL) #define pb_type_awaitable_cancel_none (NULL) @@ -80,6 +82,7 @@ void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_ mp_obj_t pb_type_awaitable_await_or_wait( void *object, mp_obj_t awaitables_in, + uint32_t end_time, pb_type_awaitable_test_completion_t test_completion_func, pb_type_awaitable_return_t return_value_func, pb_type_awaitable_cancel_t cancel_func, From 1f7e87f78c6c8ff9f8550ac48a89eb2d54d481dd Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 16 May 2023 11:16:26 +0200 Subject: [PATCH 50/59] pybricks.common.ColorLight: Callbacks return objects. Prepares for handling via async. --- pybricks/common.h | 3 ++- pybricks/common/pb_type_colorlight_external.c | 8 ++------ pybricks/nxtdevices/pb_type_nxtdevices_colorsensor.c | 3 ++- .../pupdevices/pb_type_pupdevices_colordistancesensor.c | 3 ++- pybricks/pupdevices/pb_type_pupdevices_remote.c | 3 ++- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/pybricks/common.h b/pybricks/common.h index 246dd881e..8216d6050 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -42,8 +42,9 @@ mp_obj_t pb_type_Charger_obj_new(void); * Device-specific callback for controlling a color light. * @param [in] context The instance-specific context. * @param [in] hsv The HSV color for the light. + * @return None or awaitable. */ -typedef void (*pb_type_ColorLight_on_t)(void *context, const pbio_color_hsv_t *hsv); +typedef mp_obj_t (*pb_type_ColorLight_on_t)(void *context, const pbio_color_hsv_t *hsv); // pybricks._common.ColorLight() mp_obj_t pb_type_ColorLight_external_obj_new(void *context, pb_type_ColorLight_on_t on); diff --git a/pybricks/common/pb_type_colorlight_external.c b/pybricks/common/pb_type_colorlight_external.c index 3e9e5a974..0e792518c 100644 --- a/pybricks/common/pb_type_colorlight_external.c +++ b/pybricks/common/pb_type_colorlight_external.c @@ -32,18 +32,14 @@ STATIC mp_obj_t common_ColorLight_external_on(size_t n_args, const mp_obj_t *pos common_ColorLight_external_obj_t, self, PB_ARG_REQUIRED(color)); - self->on(self->context, pb_type_Color_get_hsv(color_in)); - - return mp_const_none; + return self->on(self->context, pb_type_Color_get_hsv(color_in)); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_ColorLight_external_on_obj, 1, common_ColorLight_external_on); // pybricks._common.ColorLight.off STATIC mp_obj_t common_ColorLight_external_off(mp_obj_t self_in) { common_ColorLight_external_obj_t *self = MP_OBJ_TO_PTR(self_in); - - self->on(self->context, &pb_Color_NONE_obj.hsv); - return mp_const_none; + return self->on(self->context, &pb_Color_NONE_obj.hsv); } STATIC MP_DEFINE_CONST_FUN_OBJ_1(common_ColorLight_external_off_obj, common_ColorLight_external_off); diff --git a/pybricks/nxtdevices/pb_type_nxtdevices_colorsensor.c b/pybricks/nxtdevices/pb_type_nxtdevices_colorsensor.c index e41c9c58c..52dc8b8fa 100644 --- a/pybricks/nxtdevices/pb_type_nxtdevices_colorsensor.c +++ b/pybricks/nxtdevices/pb_type_nxtdevices_colorsensor.c @@ -22,7 +22,7 @@ typedef struct _nxtdevices_ColorSensor_obj_t { pb_device_t *pbdev; } nxtdevices_ColorSensor_obj_t; -STATIC void nxtdevices_ColorSensor_light_on(void *context, const pbio_color_hsv_t *hsv) { +STATIC mp_obj_t nxtdevices_ColorSensor_light_on(void *context, const pbio_color_hsv_t *hsv) { pb_device_t *pbdev = context; uint8_t mode; @@ -37,6 +37,7 @@ STATIC void nxtdevices_ColorSensor_light_on(void *context, const pbio_color_hsv_ } int32_t unused; pb_device_get_values(pbdev, mode, &unused); + return mp_const_none; } // pybricks.nxtdevices.ColorSensor.__init__ diff --git a/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c b/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c index 017efedf8..4c09c099e 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c @@ -65,9 +65,10 @@ static uint8_t get_mode_for_color(const pbio_color_hsv_t *hsv) { return PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__REFLT; // red } -STATIC void pupdevices_ColorDistanceSensor_light_on(void *context, const pbio_color_hsv_t *hsv) { +STATIC mp_obj_t pupdevices_ColorDistanceSensor_light_on(void *context, const pbio_color_hsv_t *hsv) { pbio_iodev_t *iodev = context; pb_pup_device_get_data(iodev, get_mode_for_color(hsv)); + return mp_const_none; } // pybricks.pupdevices.ColorDistanceSensor.__init__ diff --git a/pybricks/pupdevices/pb_type_pupdevices_remote.c b/pybricks/pupdevices/pb_type_pupdevices_remote.c index 985c902a5..fbdbaea93 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_remote.c +++ b/pybricks/pupdevices/pb_type_pupdevices_remote.c @@ -86,7 +86,7 @@ STATIC void remote_assert_connected(void) { } } -STATIC void pb_type_pupdevices_Remote_light_on(void *context, const pbio_color_hsv_t *hsv) { +STATIC mp_obj_t pb_type_pupdevices_Remote_light_on(void *context, const pbio_color_hsv_t *hsv) { pb_remote_t *remote = &pb_remote_singleton; remote_assert_connected(); @@ -122,6 +122,7 @@ STATIC void pb_type_pupdevices_Remote_light_on(void *context, const pbio_color_h pbdrv_bluetooth_write_remote(&remote->task, &msg.value); pb_wait_task(&remote->task, -1); + return mp_const_none; } STATIC void remote_connect(const char *name, mp_int_t timeout) { From 7e3a73f08cc72ea1003fd2c3d931287f11977db4 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 16 May 2023 14:25:47 +0200 Subject: [PATCH 51/59] pybricks/tools/pb_type_awaitable: Share blocking assert. We want to call this from most class inits. --- pybricks/tools.h | 2 ++ pybricks/tools/pb_module_tools.c | 6 ++++++ pybricks/tools/pb_type_awaitable.c | 2 +- 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/pybricks/tools.h b/pybricks/tools.h index 5ecc2ef58..d56a36276 100644 --- a/pybricks/tools.h +++ b/pybricks/tools.h @@ -14,6 +14,8 @@ void pb_module_tools_init(void); bool pb_module_tools_run_loop_is_active(void); +void pb_module_tools_assert_blocking(void); + extern const mp_obj_type_t pb_type_StopWatch; extern const mp_obj_type_t pb_type_Task; diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index 395b51ae7..ab279176b 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -31,6 +31,12 @@ bool pb_module_tools_run_loop_is_active() { return run_loop_is_active; } +void pb_module_tools_assert_blocking(void) { + if (run_loop_is_active) { + mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("This can only be called before multitasking starts.")); + } +} + // The awaitables for the wait() function have no object associated with // it (unlike e.g. a motor), so we make a starting point here. These never // have to cancel each other so shouldn't need to be in a list, but this lets diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 37b47b628..a32da60d2 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -195,7 +195,7 @@ mp_obj_t pb_type_awaitable_await_or_wait( // Some operations are not allowed in async mode. if (options & PB_TYPE_AWAITABLE_OPT_FORCE_BLOCK) { - mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("This can only be called before multitasking starts.")); + pb_module_tools_assert_blocking(); } // First cancel linked awaitables if requested. From 019ed1e937641a5a2c69a9c0859b2ff71ae38bbc Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 22 May 2023 15:36:58 +0200 Subject: [PATCH 52/59] pybricks.common.Motor: Restore duty_limit behavior. This is an alternate way to use temporary limits in pbio (no nlr buf) but without a breaking change in the definition of duty limit or stall. See https://github.com/pybricks/support/issues/1069 --- CHANGELOG.md | 2 -- lib/pbio/include/pbio/servo.h | 2 +- lib/pbio/src/servo.c | 17 ++++++++++++----- pybricks/common/pb_type_motor.c | 17 +++++++---------- 4 files changed, 20 insertions(+), 18 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 541b7f2fd..717ef5a74 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,8 +20,6 @@ ### Changed - Updated MicroPython to v1.20.0. -- Changed `duty_limit` argument of `Motor.run_until_stalled` to `torque_limit`. - The original name continues to work as well. ### Fixed - Fixed stdin containing `0x06` command byte ([support#1052]). diff --git a/lib/pbio/include/pbio/servo.h b/lib/pbio/include/pbio/servo.h index 497801554..cb73ee997 100644 --- a/lib/pbio/include/pbio/servo.h +++ b/lib/pbio/include/pbio/servo.h @@ -161,7 +161,7 @@ pbio_error_t pbio_servo_stop(pbio_servo_t *srv, pbio_control_on_completion_t on_ pbio_error_t pbio_servo_reset_angle(pbio_servo_t *srv, int32_t reset_angle, bool reset_to_abs); pbio_error_t pbio_servo_run_forever(pbio_servo_t *srv, int32_t speed); pbio_error_t pbio_servo_run_time(pbio_servo_t *srv, int32_t speed, uint32_t duration, pbio_control_on_completion_t on_completion); -pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit_pct, pbio_control_on_completion_t on_completion); +pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit, pbio_control_on_completion_t on_completion); pbio_error_t pbio_servo_run_angle(pbio_servo_t *srv, int32_t speed, int32_t angle, pbio_control_on_completion_t on_completion); pbio_error_t pbio_servo_run_target(pbio_servo_t *srv, int32_t speed, int32_t target, pbio_control_on_completion_t on_completion); pbio_error_t pbio_servo_track_target(pbio_servo_t *srv, int32_t target); diff --git a/lib/pbio/src/servo.c b/lib/pbio/src/servo.c index 2cdf7be98..ea7a2a86f 100644 --- a/lib/pbio/src/servo.c +++ b/lib/pbio/src/servo.c @@ -101,8 +101,16 @@ static pbio_error_t pbio_servo_update(pbio_servo_t *srv) { // Get required feedforward torque for current reference feedforward_torque = pbio_observer_get_feedforward_torque(srv->observer.model, ref.speed, ref.acceleration); + // HACK: Constrain total torque to respect temporary duty_cycle limit. + // See https://github.com/pybricks/support/issues/1069. + // The feedback torque is already constrained by the temporary limit, + // and this would be enough in a run_until_overload-like scenario. But + // for now we must limit the feedforward torque also, or it would never + // get into the stall state at high speeds. + int32_t total_torque = pbio_int_math_clamp(feedback_torque + feedforward_torque, srv->control.settings.actuation_max_temporary); + // Actuate the servo. For torque control, the torque payload is passed along. Otherwise payload is ignored. - err = pbio_servo_actuate(srv, requested_actuation, feedback_torque + feedforward_torque); + err = pbio_servo_actuate(srv, requested_actuation, total_torque); if (err != PBIO_SUCCESS) { return err; } @@ -647,12 +655,11 @@ pbio_error_t pbio_servo_run_time(pbio_servo_t *srv, int32_t speed, uint32_t dura * * @param [in] srv The servo instance. * @param [in] speed Angular velocity in degrees per second. - * @param [in] torque_limit_pct Percentage of maximum feedback torque to use. - * It will still use the default feedforward torque. + * @param [in] torque_limit Maximum torque to use. * @param [in] on_completion What to do once stalled. * @return Error code. */ -pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit_pct, pbio_control_on_completion_t on_completion) { +pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit, pbio_control_on_completion_t on_completion) { if (on_completion == PBIO_CONTROL_ON_COMPLETION_CONTINUE) { // Can't continue after stall. @@ -673,7 +680,7 @@ pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int3 srv->control.type |= PBIO_CONTROL_TYPE_FLAG_OBJECTIVE_IS_STALL; // Set the temporary torque limit used during this maneuver. - srv->control.settings.actuation_max_temporary = srv->control.settings.actuation_max * torque_limit_pct / 100; + srv->control.settings.actuation_max_temporary = torque_limit; return PBIO_SUCCESS; } diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 1f885bc79..7900490c8 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -266,25 +266,22 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po common_Motor_obj_t, self, PB_ARG_REQUIRED(speed), PB_ARG_DEFAULT_OBJ(then, pb_Stop_COAST_obj), - PB_ARG_DEFAULT_NONE(torque_limit), PB_ARG_DEFAULT_NONE(duty_limit)); mp_int_t speed = pb_obj_get_int(speed_in); pbio_control_on_completion_t then = pb_type_enum_get_value(then_in, &pb_enum_type_Stop); - // Backwards compatibility <= v3.2: allow duty_limit arg as torque_limit. + // REVISIT: Use torque limit. See https://github.com/pybricks/support/issues/1069. + int32_t torque_limit; if (duty_limit_in != mp_const_none) { - torque_limit_in = duty_limit_in; - } - - // Get torque limit as percentage of max torque. - int32_t torque_limit_pct = 100; - if (torque_limit_in != mp_const_none) { - torque_limit_pct = pb_obj_get_pct(torque_limit_in); + int32_t voltage_limit = pbio_battery_get_voltage_from_duty_pct(pb_obj_get_pct(duty_limit_in)); + torque_limit = pbio_observer_voltage_to_torque(self->srv->observer.model, voltage_limit); + } else { + torque_limit = self->srv->control.settings.actuation_max; } // Start moving. - pb_assert(pbio_servo_run_until_stalled(self->srv, speed, torque_limit_pct, then)); + pb_assert(pbio_servo_run_until_stalled(self->srv, speed, torque_limit, then)); // Handle completion by awaiting or blocking. return pb_type_awaitable_await_or_wait( From 6f099d201ddbf492d8741d54149208d88eefc38a Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 25 May 2023 09:58:27 +0200 Subject: [PATCH 53/59] .vscode/tasks: Add build task for Move Hub. This lets us assign a keyboard shortcut in user settings to build it quickly. --- .vscode/tasks.json | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 2321af2db..73cba86f3 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -13,6 +13,11 @@ "type": "shell", "command": "make ev3dev-armel -j && cp bricks/ev3dev/build-armel/pybricks-micropython /run/user/1000/gvfs/sftp:host=*/home/robot" }, + { + "label": "build movehub", + "type": "shell", + "command": "make movehub -j" + }, { "label": "build virtualhub", "type": "shell", From 65cbbec9d34bb71ea2fdee8c9d4cf1d3734bdc48 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 25 May 2023 12:56:51 +0200 Subject: [PATCH 54/59] pybricks/tools/pb_type_awaitable: Revert generalizing object type. This reverts commit 2e19eef90cf7590676d65f0e37a91b7bbb867d52. --- pybricks/common/pb_type_motor.c | 32 +++++++++++++-------------- pybricks/common/pb_type_speaker.c | 16 +++++++------- pybricks/robotics/pb_type_drivebase.c | 22 +++++++++--------- pybricks/tools/pb_module_tools.c | 6 ++++- pybricks/tools/pb_type_awaitable.c | 32 +++++++++++++-------------- pybricks/tools/pb_type_awaitable.h | 16 +++++++------- 6 files changed, 64 insertions(+), 60 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 7900490c8..0da75db37 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -131,27 +131,27 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args, return MP_OBJ_FROM_PTR(self); } -STATIC bool common_Motor_test_completion(void *object, uint32_t end_time) { - pbio_servo_t *srv = object; +STATIC bool common_Motor_test_completion(mp_obj_t self_in, uint32_t end_time) { + common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); // Handle I/O exceptions like port unplugged. - if (!pbio_servo_update_loop_is_running(srv)) { + if (!pbio_servo_update_loop_is_running(self->srv)) { pb_assert(PBIO_ERROR_NO_DEV); } // Get completion state. - return pbio_control_is_done(&srv->control); + return pbio_control_is_done(&self->srv->control); } -STATIC void common_Motor_cancel(void *object) { - pbio_servo_t *srv = object; - pb_assert(pbio_servo_stop(srv, PBIO_CONTROL_ON_COMPLETION_COAST)); +STATIC void common_Motor_cancel(mp_obj_t self_in) { + common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); + pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_COAST)); } // Common awaitable used for most motor methods. STATIC mp_obj_t await_or_wait(common_Motor_obj_t *self) { return pb_type_awaitable_await_or_wait( - self->srv, + MP_OBJ_FROM_PTR(self), self->awaitables, pb_type_awaitable_end_time_none, common_Motor_test_completion, @@ -184,7 +184,7 @@ STATIC mp_obj_t common_Motor_reset_angle(size_t n_args, const mp_obj_t *pos_args // Set the new angle pb_assert(pbio_servo_reset_angle(self->srv, reset_angle, reset_to_abs)); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_reset_angle_obj, 1, common_Motor_reset_angle); @@ -209,7 +209,7 @@ STATIC mp_obj_t common_Motor_run(size_t n_args, const mp_obj_t *pos_args, mp_map mp_int_t speed = pb_obj_get_int(speed_in); pb_assert(pbio_servo_run_forever(self->srv, speed)); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); @@ -218,7 +218,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); STATIC mp_obj_t common_Motor_hold(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_HOLD)); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_cancel_all(self_in, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_hold_obj, common_Motor_hold); @@ -249,13 +249,13 @@ STATIC mp_obj_t common_Motor_run_time(size_t n_args, const mp_obj_t *pos_args, m } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_time_obj, 1, common_Motor_run_time); -STATIC mp_obj_t common_Motor_stall_return_value(void *object) { +STATIC mp_obj_t common_Motor_stall_return_value(mp_obj_t self_in) { - pbio_servo_t *srv = object; + common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); // Return the angle upon completion of the stall maneuver. int32_t stall_angle, stall_speed; - pb_assert(pbio_servo_get_state_user(srv, &stall_angle, &stall_speed)); + pb_assert(pbio_servo_get_state_user(self->srv, &stall_angle, &stall_speed)); return mp_obj_new_int(stall_angle); } @@ -285,7 +285,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po // Handle completion by awaiting or blocking. return pb_type_awaitable_await_or_wait( - self->srv, + MP_OBJ_FROM_PTR(self), self->awaitables, pb_type_awaitable_end_time_none, common_Motor_test_completion, @@ -353,7 +353,7 @@ STATIC mp_obj_t common_Motor_track_target(size_t n_args, const mp_obj_t *pos_arg mp_int_t target_angle = pb_obj_get_int(target_angle_in); pb_assert(pbio_servo_track_target(self->srv, target_angle)); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_track_target_obj, 1, common_Motor_track_target); diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 944d9ba2f..8ded97adc 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -129,8 +129,8 @@ STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_arg return MP_OBJ_FROM_PTR(self); } -STATIC bool pb_type_Speaker_beep_test_completion(void *object, uint32_t end_time) { - pb_type_Speaker_obj_t *self = object; +STATIC bool pb_type_Speaker_beep_test_completion(mp_obj_t self_in, uint32_t end_time) { + pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); if (mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX) { pb_type_Speaker_stop_beep(); return true; @@ -138,9 +138,9 @@ STATIC bool pb_type_Speaker_beep_test_completion(void *object, uint32_t end_time return false; } -STATIC void pb_type_Speaker_cancel(void *object) { +STATIC void pb_type_Speaker_cancel(mp_obj_t self_in) { pb_type_Speaker_stop_beep(); - pb_type_Speaker_obj_t *self = object; + pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); self->beep_end_time = mp_hal_ticks_ms(); self->release_end_time = self->beep_end_time; self->notes_generator = MP_OBJ_NULL; @@ -166,7 +166,7 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp self->notes_generator = MP_OBJ_NULL; return pb_type_awaitable_await_or_wait( - self, + MP_OBJ_FROM_PTR(self), self->awaitables, pb_type_awaitable_end_time_none, pb_type_Speaker_beep_test_completion, @@ -343,8 +343,8 @@ STATIC void pb_type_Speaker_play_note(pb_type_Speaker_obj_t *self, mp_obj_t obj, self->beep_end_time = release ? time_now + 7 * duration / 8 : time_now + duration; } -STATIC bool pb_type_Speaker_notes_test_completion(void *object, uint32_t end_time) { - pb_type_Speaker_obj_t *self = object; +STATIC bool pb_type_Speaker_notes_test_completion(mp_obj_t self_in, uint32_t end_time) { + pb_type_Speaker_obj_t *self = MP_OBJ_TO_PTR(self_in); bool release_done = mp_hal_ticks_ms() - self->release_end_time < (uint32_t)INT32_MAX; bool beep_done = mp_hal_ticks_ms() - self->beep_end_time < (uint32_t)INT32_MAX; @@ -382,7 +382,7 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar self->beep_end_time = mp_hal_ticks_ms(); self->release_end_time = self->beep_end_time; return pb_type_awaitable_await_or_wait( - self, + MP_OBJ_FROM_PTR(self), self->awaitables, pb_type_awaitable_end_time_none, pb_type_Speaker_notes_test_completion, diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index 2c31a77d7..ae5de9010 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -92,28 +92,28 @@ STATIC mp_obj_t pb_type_DriveBase_make_new(const mp_obj_type_t *type, size_t n_a return MP_OBJ_FROM_PTR(self); } -STATIC bool pb_type_DriveBase_test_completion(void *object, uint32_t end_time) { +STATIC bool pb_type_DriveBase_test_completion(mp_obj_t self_in, uint32_t end_time) { - pbio_drivebase_t *db = object; + pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); // Handle I/O exceptions like port unplugged. - if (!pbio_drivebase_update_loop_is_running(db)) { + if (!pbio_drivebase_update_loop_is_running(self->db)) { pb_assert(PBIO_ERROR_NO_DEV); } // Get completion state. - return pbio_drivebase_is_done(db); + return pbio_drivebase_is_done(self->db); } -STATIC void pb_type_DriveBase_cancel(void *object) { - pbio_drivebase_t *db = object; - pb_assert(pbio_drivebase_stop(db, PBIO_CONTROL_ON_COMPLETION_COAST)); +STATIC void pb_type_DriveBase_cancel(mp_obj_t self_in) { + pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); + pb_assert(pbio_drivebase_stop(self->db, PBIO_CONTROL_ON_COMPLETION_COAST)); } // All drive base methods use the same kind of completion awaitable. STATIC mp_obj_t await_or_wait(pb_type_DriveBase_obj_t *self) { return pb_type_awaitable_await_or_wait( - self->db, + MP_OBJ_FROM_PTR(self), self->awaitables, pb_type_awaitable_end_time_none, pb_type_DriveBase_test_completion, @@ -203,7 +203,7 @@ STATIC mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, mp_int_t turn_rate = pb_obj_get_int(turn_rate_in); // Cancel awaitables but not hardware. Drive forever will handle this. - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); pb_assert(pbio_drivebase_drive_forever(self->db, speed, turn_rate)); return mp_const_none; @@ -215,10 +215,10 @@ STATIC mp_obj_t pb_type_DriveBase_stop(mp_obj_t self_in) { // Cancel awaitables. pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_type_awaitable_cancel_all(self, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_cancel_all(self_in, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); // Stop hardware. - pb_type_DriveBase_cancel(self->db); + pb_type_DriveBase_cancel(self_in); return mp_const_none; } diff --git a/pybricks/tools/pb_module_tools.c b/pybricks/tools/pb_module_tools.c index ab279176b..4fa949253 100644 --- a/pybricks/tools/pb_module_tools.c +++ b/pybricks/tools/pb_module_tools.c @@ -49,7 +49,7 @@ void pb_module_tools_init(void) { run_loop_is_active = false; } -STATIC bool pb_module_tools_wait_test_completion(void *object, uint32_t end_time) { +STATIC bool pb_module_tools_wait_test_completion(mp_obj_t obj, uint32_t end_time) { return mp_hal_ticks_ms() - end_time < UINT32_MAX / 2; } @@ -68,6 +68,10 @@ STATIC mp_obj_t pb_module_tools_wait(size_t n_args, const mp_obj_t *pos_args, mp return mp_const_none; } + // Require that duration is nonnegative small int. This makes it cheaper to + // test completion state in iteration loop. + time = pbio_int_math_bind(time, 0, INT32_MAX >> 2); + return pb_type_awaitable_await_or_wait( NULL, // wait functions are not associated with an object MP_STATE_PORT(wait_awaitables), diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index a32da60d2..270b31cae 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -21,7 +21,7 @@ struct _pb_type_awaitable_obj_t { /** * Object associated with this awaitable, such as the motor we wait on. */ - void *object; + mp_obj_t obj; /** * End time. Gets passed to completion test to allow for graceful timeout * or raise timeout errors if desired. @@ -48,7 +48,7 @@ STATIC mp_obj_t pb_type_awaitable_close(mp_obj_t self_in) { self->test_completion = AWAITABLE_FREE; // Handle optional clean up/cancelling of hardware operation. if (self->cancel) { - self->cancel(self->object); + self->cancel(self->obj); } return mp_const_none; } @@ -63,7 +63,7 @@ STATIC mp_obj_t pb_type_awaitable_iternext(mp_obj_t self_in) { } // Keep going if not completed by returning None. - if (!self->test_completion(self->object, self->end_time)) { + if (!self->test_completion(self->obj, self->end_time)) { return mp_const_none; } @@ -76,7 +76,7 @@ STATIC mp_obj_t pb_type_awaitable_iternext(mp_obj_t self_in) { } // Otherwise, set return value via stop iteration. - return mp_make_stop_iteration(self->return_value(self->object)); + return mp_make_stop_iteration(self->return_value(self->obj)); } STATIC const mp_rom_map_elem_t pb_type_awaitable_locals_dict_table[] = { @@ -95,7 +95,7 @@ MP_DEFINE_CONST_OBJ_TYPE(pb_type_awaitable, /** * Gets an awaitable object that is not in use, or makes a new one. * - * @param [in] awaitables_in List of awaitables associated with @p object. + * @param [in] awaitables_in List of awaitables associated with @p obj. */ STATIC pb_type_awaitable_obj_t *pb_type_awaitable_get(mp_obj_t awaitables_in) { @@ -137,11 +137,11 @@ STATIC bool pb_type_awaitable_completed(mp_obj_t self_in, uint32_t start_time) { * This is normally used by the function that makes a new awaitable, but it can * also be called independently to cancel without starting a new awaitable. * - * @param [in] object The object whose method we want to wait for completion. - * @param [in] awaitables_in List of awaitables associated with @p object. + * @param [in] obj The object whose method we want to wait for completion. + * @param [in] awaitables_in List of awaitables associated with @p obj. * @param [in] options Controls awaitable behavior. */ -void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_awaitable_opt_t options) { +void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_awaitable_opt_t options) { // Exit if nothing to do. if (!pb_module_tools_run_loop_is_active() || options == PB_TYPE_AWAITABLE_OPT_NONE) { @@ -156,7 +156,7 @@ void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_ if (awaitable->test_completion) { // Cancel hardware operation if requested and available. if (options & PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK && awaitable->cancel) { - awaitable->cancel(awaitable->object); + awaitable->cancel(awaitable->obj); } // Set awaitable to done so it gets cancelled it gracefully on the // next iteration. @@ -172,8 +172,8 @@ void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_ * * Automatically cancels any previous awaitables associated with the object if requested. * - * @param [in] object The object whose method we want to wait for completion. - * @param [in] awaitables_in List of awaitables associated with @p object. + * @param [in] obj The object whose method we want to wait for completion. + * @param [in] awaitables_in List of awaitables associated with @p obj. * @param [in] end_time Wall time in milliseconds when the operation should end. * May be arbitrary if completion function does not need it. * @param [in] test_completion_func Function to test if the operation is complete. @@ -182,7 +182,7 @@ void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_ * @param [in] options Controls awaitable behavior. */ mp_obj_t pb_type_awaitable_await_or_wait( - void *object, + mp_obj_t obj, mp_obj_t awaitables_in, uint32_t end_time, pb_type_awaitable_test_completion_t test_completion_func, @@ -199,13 +199,13 @@ mp_obj_t pb_type_awaitable_await_or_wait( } // First cancel linked awaitables if requested. - pb_type_awaitable_cancel_all(object, awaitables_in, options); + pb_type_awaitable_cancel_all(obj, awaitables_in, options); // Gets free existing awaitable or creates a new one. pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(awaitables_in); // Initialize awaitable. - awaitable->object = object; + awaitable->obj = obj; awaitable->test_completion = test_completion_func; awaitable->return_value = return_value_func; awaitable->cancel = cancel_func; @@ -214,13 +214,13 @@ mp_obj_t pb_type_awaitable_await_or_wait( } // Outside run loop, block until the operation is complete. - while (test_completion_func && !test_completion_func(object, end_time)) { + while (test_completion_func && !test_completion_func(obj, end_time)) { mp_hal_delay_ms(1); } if (!return_value_func) { return mp_const_none; } - return return_value_func(object); + return return_value_func(obj); } #endif // PYBRICKS_PY_TOOLS diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index e098d4629..8f0ec90b3 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -48,28 +48,28 @@ typedef struct _pb_type_awaitable_obj_t pb_type_awaitable_obj_t; * operations as needed (hold a motor, etc.). This is not the same as cancel * below, which always stops the relevant hardware (i.e. always coast). * - * @param [in] object The object/device associated with this awaitable. + * @param [in] obj The object associated with this awaitable. * @param [in] start_time The time when the awaitable was created. * @return True if operation is complete, False otherwise. */ -typedef bool (*pb_type_awaitable_test_completion_t)(void *object, uint32_t end_time); +typedef bool (*pb_type_awaitable_test_completion_t)(mp_obj_t objt, uint32_t end_time); /** * Gets the return value of the awaitable. If it always returns None, providing * this function is not necessary. * - * @param [in] object The object/device associated with this awaitable. + * @param [in] obj The object associated with this awaitable. * @return The return value of the awaitable. */ -typedef mp_obj_t (*pb_type_awaitable_return_t)(void *object); +typedef mp_obj_t (*pb_type_awaitable_return_t)(mp_obj_t obj); /** * Called on cancel/close. Used to stop hardware operation in unhandled * conditions. * - * @param [in] object The object/device associated with this awaitable. + * @param [in] obj The object associated with this awaitable. */ -typedef void (*pb_type_awaitable_cancel_t)(void *object); +typedef void (*pb_type_awaitable_cancel_t)(mp_obj_t obj); #define pb_type_awaitable_end_time_none (0) @@ -77,10 +77,10 @@ typedef void (*pb_type_awaitable_cancel_t)(void *object); #define pb_type_awaitable_cancel_none (NULL) -void pb_type_awaitable_cancel_all(void *object, mp_obj_t awaitables_in, pb_type_awaitable_opt_t options); +void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_awaitable_opt_t options); mp_obj_t pb_type_awaitable_await_or_wait( - void *object, + mp_obj_t obj, mp_obj_t awaitables_in, uint32_t end_time, pb_type_awaitable_test_completion_t test_completion_func, From 54408e9888f699b0a19f9fd766b3c52ef5d76f39 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 25 May 2023 14:02:46 +0200 Subject: [PATCH 55/59] pbio/uartdev: Fix port_data not being static. Also rename so it doesn't mask the data of the process thread. --- lib/pbio/src/uartdev.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/lib/pbio/src/uartdev.c b/lib/pbio/src/uartdev.c index 5c24ce62d..0271e2085 100644 --- a/lib/pbio/src/uartdev.c +++ b/lib/pbio/src/uartdev.c @@ -1162,6 +1162,7 @@ static PT_THREAD(pbio_uartdev_init(struct pt *pt, uint8_t id)) { PROCESS_THREAD(pbio_uartdev_process, ev, data) { static struct pt pt; static int i; + static uartdev_port_data_t *port_data; PROCESS_BEGIN(); @@ -1171,18 +1172,18 @@ PROCESS_THREAD(pbio_uartdev_process, ev, data) { while (true) { for (i = 0; i < PBIO_CONFIG_UARTDEV_NUM_DEV; i++) { - uartdev_port_data_t *data = &dev_data[i]; + port_data = &dev_data[i]; - pbio_uartdev_update(data); + pbio_uartdev_update(port_data); - if (data->status == PBIO_UARTDEV_STATUS_DATA) { - pbio_uartdev_receive_data(data); + if (port_data->status == PBIO_UARTDEV_STATUS_DATA) { + pbio_uartdev_receive_data(port_data); } - pbio_uartdev_handle_write_end(data); + pbio_uartdev_handle_write_end(port_data); - if (data->status == PBIO_UARTDEV_STATUS_DATA) { - pbio_uartdev_handle_data_set_start(data); + if (port_data->status == PBIO_UARTDEV_STATUS_DATA) { + pbio_uartdev_handle_data_set_start(port_data); } } PROCESS_WAIT_EVENT(); From e8651fa69eac330a6d23f601a7e86ffd93192215 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 25 May 2023 14:57:46 +0200 Subject: [PATCH 56/59] pybricks/pupdevices: Make sensors async-ready. --- pybricks/common.h | 3 +- pybricks/common/pb_type_lightarray.c | 12 +- .../iodevices/pb_type_iodevices_pupdevice.c | 56 +++--- pybricks/pupdevices.h | 42 ++++- pybricks/pupdevices/pb_module_pupdevices.c | 114 ++++++++++-- .../pb_type_pupdevices_colordistancesensor.c | 139 +++++++------- .../pb_type_pupdevices_colorlightmatrix.c | 16 +- .../pb_type_pupdevices_colorsensor.c | 172 +++++++++--------- .../pb_type_pupdevices_forcesensor.c | 105 +++++------ .../pb_type_pupdevices_infraredsensor.c | 56 ++---- .../pupdevices/pb_type_pupdevices_pfmotor.c | 24 +-- .../pb_type_pupdevices_tiltsensor.c | 19 +- .../pb_type_pupdevices_ultrasonicsensor.c | 32 ++-- pybricks/util_pb/pb_color_map.c | 11 +- 14 files changed, 429 insertions(+), 372 deletions(-) diff --git a/pybricks/common.h b/pybricks/common.h index 8216d6050..6070fb3fd 100644 --- a/pybricks/common.h +++ b/pybricks/common.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -52,7 +53,7 @@ mp_obj_t common_ColorLight_internal_obj_new(pbio_color_light_t *light); #if PYBRICKS_PY_COMMON_LIGHT_ARRAY // pybricks._common.LightArray() -mp_obj_t common_LightArray_obj_make_new(pbio_iodev_t *iodev, uint8_t light_mode, uint8_t number_of_lights); +mp_obj_t common_LightArray_obj_make_new(pb_pupdevices_obj_base_t *sensor, uint8_t light_mode, uint8_t number_of_lights); #endif #if PYBRICKS_PY_COMMON_LIGHT_MATRIX diff --git a/pybricks/common/pb_type_lightarray.c b/pybricks/common/pb_type_lightarray.c index dac63fe1f..db972faad 100644 --- a/pybricks/common/pb_type_lightarray.c +++ b/pybricks/common/pb_type_lightarray.c @@ -19,7 +19,7 @@ // pybricks._common.Light class object typedef struct _common_LightArray_obj_t { mp_obj_base_t base; - pbio_iodev_t *iodev; + pb_pupdevices_obj_base_t *sensor; uint8_t light_mode; uint8_t number_of_lights; } common_LightArray_obj_t; @@ -52,10 +52,8 @@ STATIC mp_obj_t common_LightArray_on(size_t n_args, const mp_obj_t *pos_args, mp } } - // Set the brightness values - pb_pup_device_set_data(self->iodev, self->light_mode, brightness_values); - - return mp_const_none; + // Set the brightness values and wait or await it. + return pb_pupdevices_set_data(self->sensor, self->light_mode, brightness_values); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_LightArray_on_obj, 1, common_LightArray_on); @@ -81,9 +79,9 @@ STATIC MP_DEFINE_CONST_OBJ_TYPE(pb_type_LightArray, locals_dict, &common_LightArray_locals_dict); // pybricks._common.LightArray.__init__ -mp_obj_t common_LightArray_obj_make_new(pbio_iodev_t *iodev, uint8_t light_mode, uint8_t number_of_lights) { +mp_obj_t common_LightArray_obj_make_new(pb_pupdevices_obj_base_t *sensor, uint8_t light_mode, uint8_t number_of_lights) { common_LightArray_obj_t *light = mp_obj_malloc(common_LightArray_obj_t, &pb_type_LightArray); - light->iodev = iodev; + light->sensor = sensor; light->light_mode = light_mode; light->number_of_lights = number_of_lights; return MP_OBJ_FROM_PTR(light); diff --git a/pybricks/iodevices/pb_type_iodevices_pupdevice.c b/pybricks/iodevices/pb_type_iodevices_pupdevice.c index 103337953..a616984de 100644 --- a/pybricks/iodevices/pb_type_iodevices_pupdevice.c +++ b/pybricks/iodevices/pb_type_iodevices_pupdevice.c @@ -6,6 +6,7 @@ #if PYBRICKS_PY_IODEVICES && PYBRICKS_PY_PUPDEVICES #include +#include #include #include "py/objstr.h" @@ -20,8 +21,11 @@ // Class structure for PUPDevice typedef struct _iodevices_PUPDevice_obj_t { - mp_obj_base_t base; - pbio_iodev_t *iodev; + pb_pupdevices_obj_base_t pup_base; + // Mode used when initiating awaitable read. REVISIT: This should be stored + // on the awaitable instead, as extra context. For now, it is safe since + // concurrent reads with the same sensor are not permitten. + uint8_t last_mode; } iodevices_PUPDevice_obj_t; // pybricks.iodevices.PUPDevice.__init__ @@ -30,43 +34,31 @@ STATIC mp_obj_t iodevices_PUPDevice_make_new(const mp_obj_type_t *type, size_t n PB_ARG_REQUIRED(port)); iodevices_PUPDevice_obj_t *self = mp_obj_malloc(iodevices_PUPDevice_obj_t, type); - - pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); - - self->iodev = pb_pup_device_get_device(port, PBIO_IODEV_TYPE_ID_LUMP_UART); - + pb_pupdevices_init_class(&self->pup_base, port_in, PBIO_IODEV_TYPE_ID_LUMP_UART); return MP_OBJ_FROM_PTR(self); } // pybricks.iodevices.PUPDevice.info STATIC mp_obj_t iodevices_PUPDevice_info(mp_obj_t self_in) { iodevices_PUPDevice_obj_t *self = MP_OBJ_TO_PTR(self_in); - mp_obj_t info_dict = mp_obj_new_dict(1); - mp_obj_dict_store(info_dict, MP_ROM_QSTR(MP_QSTR_id), MP_OBJ_NEW_SMALL_INT(self->iodev->info->type_id)); - + mp_obj_dict_store(info_dict, MP_ROM_QSTR(MP_QSTR_id), MP_OBJ_NEW_SMALL_INT(self->pup_base.iodev->info->type_id)); return info_dict; } MP_DEFINE_CONST_FUN_OBJ_1(iodevices_PUPDevice_info_obj, iodevices_PUPDevice_info); -// pybricks.iodevices.PUPDevice.read -STATIC mp_obj_t iodevices_PUPDevice_read(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { - PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, - iodevices_PUPDevice_obj_t, self, - PB_ARG_REQUIRED(mode)); +STATIC mp_obj_t get_pup_data_tuple(mp_obj_t self_in) { + iodevices_PUPDevice_obj_t *self = MP_OBJ_TO_PTR(self_in); + void *data = pb_pupdevices_get_data(self_in, self->last_mode); uint8_t num_values; pbio_iodev_data_type_t type; - - uint8_t mode = mp_obj_get_int(mode_in); - pb_assert(pbio_iodev_get_data_format(self->iodev, mode, &num_values, &type)); + pb_assert(pbio_iodev_get_data_format(self->pup_base.iodev, self->last_mode, &num_values, &type)); if (num_values == 0) { pb_assert(PBIO_ERROR_INVALID_ARG); } - void *data = pb_pup_device_get_data(self->iodev, mode); - mp_obj_t values[PBIO_IODEV_MAX_DATA_SIZE]; for (uint8_t i = 0; i < num_values; i++) { @@ -92,6 +84,24 @@ STATIC mp_obj_t iodevices_PUPDevice_read(size_t n_args, const mp_obj_t *pos_args return mp_obj_new_tuple(num_values, values); } + +// pybricks.iodevices.PUPDevice.read +STATIC mp_obj_t iodevices_PUPDevice_read(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, + iodevices_PUPDevice_obj_t, self, + PB_ARG_REQUIRED(mode)); + + self->last_mode = mp_obj_get_int(mode_in); + + // We can re-use the same code as for specific sensor types, only the mode + // is not hardcoded per call, so we create that object here. + const pb_obj_pupdevices_method_t method = { + {&pb_type_pupdevices_method}, + .mode = self->last_mode, + .get_values = get_pup_data_tuple, + }; + return pb_pupdevices_method_call(MP_OBJ_FROM_PTR(&method), 1, 0, pos_args); +} MP_DEFINE_CONST_FUN_OBJ_KW(iodevices_PUPDevice_read_obj, 1, iodevices_PUPDevice_read); // pybricks.iodevices.PUPDevice.write @@ -115,7 +125,7 @@ STATIC mp_obj_t iodevices_PUPDevice_write(size_t n_args, const mp_obj_t *pos_arg uint8_t data[PBIO_IODEV_MAX_DATA_SIZE]; uint8_t len; pbio_iodev_data_type_t type; - pb_assert(pbio_iodev_get_data_format(self->iodev, mode, &len, &type)); + pb_assert(pbio_iodev_get_data_format(self->pup_base.iodev, mode, &len, &type)); if (len != num_values) { pb_assert(PBIO_ERROR_INVALID_ARG); @@ -142,9 +152,7 @@ STATIC mp_obj_t iodevices_PUPDevice_write(size_t n_args, const mp_obj_t *pos_arg } } // Set the data. - pb_pup_device_set_data(self->iodev, mode, data); - - return mp_const_none; + return pb_pupdevices_set_data(&self->pup_base, mode, data); } MP_DEFINE_CONST_FUN_OBJ_KW(iodevices_PUPDevice_write_obj, 1, iodevices_PUPDevice_write); diff --git a/pybricks/pupdevices.h b/pybricks/pupdevices.h index dafbbacce..daf937975 100644 --- a/pybricks/pupdevices.h +++ b/pybricks/pupdevices.h @@ -10,6 +10,34 @@ #include "py/obj.h" +#include +#include + +/** + * Used in place of mp_obj_base_t in all pupdevices. This lets us share + * code for awaitables for all pupdevices. + */ +typedef struct _pb_pupdevices_obj_base_t { + mp_obj_base_t base; + pbio_iodev_t *iodev; + mp_obj_t awaitables; +} pb_pupdevices_obj_base_t; + +/** + * Callable sensor method with a particular mode and return map. + */ +typedef struct { + mp_obj_base_t base; + pb_type_awaitable_return_t get_values; + uint8_t mode; +} pb_obj_pupdevices_method_t; + +extern const mp_obj_type_t pb_type_pupdevices_method; + +#define PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(obj_name, mode_id, get_values_func) \ + const pb_obj_pupdevices_method_t obj_name = \ + {{&pb_type_pupdevices_method}, .mode = mode_id, .get_values = get_values_func} + extern const mp_obj_type_t pb_type_pupdevices_ColorDistanceSensor; extern const mp_obj_type_t pb_type_pupdevices_ColorLightMatrix; extern const mp_obj_type_t pb_type_pupdevices_ColorSensor; @@ -21,15 +49,21 @@ extern const mp_obj_type_t pb_type_pupdevices_Remote; extern const mp_obj_type_t pb_type_pupdevices_TiltSensor; extern const mp_obj_type_t pb_type_pupdevices_UltrasonicSensor; -pbio_iodev_t *pupdevices_ColorDistanceSensor__get_device(mp_obj_t obj); +mp_obj_t pb_pupdevices_method_call(mp_obj_t self_in, size_t n_args, size_t n_kw, const mp_obj_t *args); + +pb_pupdevices_obj_base_t *pupdevices_ColorDistanceSensor__get_device(mp_obj_t obj); void pb_type_Remote_cleanup(void); -pbio_iodev_t *pb_pup_device_get_device(pbio_port_id_t port, pbio_iodev_type_id_t valid_id); -void *pb_pup_device_get_data(pbio_iodev_t *iodev, uint8_t mode); -void pb_pup_device_set_data(pbio_iodev_t *iodev, uint8_t mode, const void *data); +void pb_pupdevices_init_class(pb_pupdevices_obj_base_t *self, mp_obj_t port_in, pbio_iodev_type_id_t valid_id); +mp_obj_t pb_pupdevices_set_data(pb_pupdevices_obj_base_t *sensor, uint8_t mode, const void *data); +void *pb_pupdevices_get_data(mp_obj_t self_in, uint8_t mode); +void *pb_pupdevices_get_data_blocking(pb_pupdevices_obj_base_t *sensor, uint8_t mode); void pb_pup_device_setup_motor(pbio_port_id_t port, bool is_servo); +// Delete me: refactor getter only still used for passive external light. +pbio_iodev_t *pb_pup_device_get_device(pbio_port_id_t port, pbio_iodev_type_id_t valid_id); + #endif // PYBRICKS_PY_PUPDEVICES #endif // PYBRICKS_INCLUDED_PYBRICKS_PUPDEVICES_H diff --git a/pybricks/pupdevices/pb_module_pupdevices.c b/pybricks/pupdevices/pb_module_pupdevices.c index 9a5e501af..25ee782c8 100644 --- a/pybricks/pupdevices/pb_module_pupdevices.c +++ b/pybricks/pupdevices/pb_module_pupdevices.c @@ -14,33 +14,112 @@ #include +#include #include -static void pb_pup_device_wait_ready(pbio_iodev_t *iodev) { +/** + * Non-blocking version of powered up data getter. Will raise exception if + * sensor is not already in the right mode. + * + * @param [in] sensor The powered up device. + * @param [in] mode Desired mode. + * @return Void pointer to data. + */ +void *pb_pupdevices_get_data(mp_obj_t self_in, uint8_t mode) { + pb_pupdevices_obj_base_t *sensor = MP_OBJ_TO_PTR(self_in); + void *data; + pb_assert(pbio_uartdev_get_data(sensor->iodev, mode, &data)); + return data; +} + +/** + * Always-blocking version of powered up data getter. Can be used during sensor + * or motor initialization. + * + * @param [in] sensor The powered up device. + * @param [in] mode Desired mode. + * @return Void pointer to data. + */ +void *pb_pupdevices_get_data_blocking(pb_pupdevices_obj_base_t *sensor, uint8_t mode) { + pb_assert(pbio_uartdev_set_mode(sensor->iodev, mode)); pbio_error_t err; - while ((err = pbio_uartdev_is_ready(iodev)) == PBIO_ERROR_AGAIN) { + while ((err = pbio_uartdev_is_ready(sensor->iodev)) == PBIO_ERROR_AGAIN) { MICROPY_EVENT_POLL_HOOK } pb_assert(err); + void *data; + pb_assert(pbio_uartdev_get_data(sensor->iodev, mode, &data)); + return data; } -// This will be replaced by an async handler -void *pb_pup_device_get_data(pbio_iodev_t *iodev, uint8_t mode) { - pb_assert(pbio_uartdev_set_mode(iodev, mode)); - pb_pup_device_wait_ready(iodev); +STATIC bool pb_pup_device_test_completion(mp_obj_t self_in, uint32_t end_time) { + pb_pupdevices_obj_base_t *sensor = MP_OBJ_TO_PTR(self_in); + pbio_error_t err = pbio_uartdev_is_ready(sensor->iodev); + if (err == PBIO_ERROR_AGAIN) { + return false; + } + pb_assert(err); + return true; +} - void *data; - pb_assert(pbio_uartdev_get_data(iodev, mode, &data)); +mp_obj_t pb_pupdevices_method_call(mp_obj_t self_in, size_t n_args, size_t n_kw, const mp_obj_t *args) { + assert(mp_obj_is_type(self_in, &pb_type_pupdevices_method)); + pb_obj_pupdevices_method_t *method = MP_OBJ_TO_PTR(self_in); + mp_arg_check_num(n_args, n_kw, 1, 1, false); + + mp_obj_t sensor_in = args[0]; + pb_pupdevices_obj_base_t *sensor = MP_OBJ_TO_PTR(sensor_in); + pb_assert(pbio_uartdev_set_mode(sensor->iodev, method->mode)); + + return pb_type_awaitable_await_or_wait( + sensor_in, + sensor->awaitables, + pb_type_awaitable_end_time_none, + pb_pup_device_test_completion, + method->get_values, + pb_type_awaitable_cancel_none, + PB_TYPE_AWAITABLE_OPT_NONE); +} - return data; +MP_DEFINE_CONST_OBJ_TYPE( + pb_type_pupdevices_method, MP_QSTR_function, MP_TYPE_FLAG_BINDS_SELF | MP_TYPE_FLAG_BUILTIN_FUN, + call, pb_pupdevices_method_call, + unary_op, mp_generic_unary_op + ); + +mp_obj_t pb_pupdevices_set_data(pb_pupdevices_obj_base_t *sensor, uint8_t mode, const void *data) { + pb_assert(pbio_uartdev_set_mode_with_data(sensor->iodev, mode, data)); + return pb_type_awaitable_await_or_wait( + MP_OBJ_FROM_PTR(sensor), + sensor->awaitables, + pb_type_awaitable_end_time_none, + pb_pup_device_test_completion, + pb_type_awaitable_return_none, + pb_type_awaitable_cancel_none, + PB_TYPE_AWAITABLE_OPT_NONE); // choose opt raise on busy } -// This will be replaced by an async handler -void pb_pup_device_set_data(pbio_iodev_t *iodev, uint8_t mode, const void *data) { - pb_assert(pbio_uartdev_set_mode_with_data(iodev, mode, data)); - pb_pup_device_wait_ready(iodev); +void pb_pupdevices_init_class(pb_pupdevices_obj_base_t *self, mp_obj_t port_in, pbio_iodev_type_id_t valid_id) { + + pb_module_tools_assert_blocking(); + + pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); + + pbio_error_t err; + while ((err = pbdrv_ioport_get_iodev(port, &self->iodev)) == PBIO_ERROR_AGAIN) { + mp_hal_delay_ms(50); + } + pb_assert(err); + + // Verify the ID or always allow generic LUMP device + if (self->iodev->info->type_id != valid_id && valid_id != PBIO_IODEV_TYPE_ID_LUMP_UART) { + pb_assert(PBIO_ERROR_NO_DEV); + } + + self->awaitables = mp_obj_new_list(0, NULL); } +// Delete me: refactor getter only still used for passive external light. pbio_iodev_t *pb_pup_device_get_device(pbio_port_id_t port, pbio_iodev_type_id_t valid_id) { pbio_iodev_t *iodev; @@ -58,6 +137,7 @@ pbio_iodev_t *pb_pup_device_get_device(pbio_port_id_t port, pbio_iodev_type_id_t return iodev; } +// Revisit: get rid of this abstraction void pb_pup_device_setup_motor(pbio_port_id_t port, bool is_servo) { // HACK: Built-in motors on BOOST Move hub do not have I/O ports associated // with them. @@ -67,6 +147,8 @@ void pb_pup_device_setup_motor(pbio_port_id_t port, bool is_servo) { } #endif + pb_module_tools_assert_blocking(); + // Get the iodevice pbio_iodev_t *iodev; pbio_error_t err; @@ -96,8 +178,10 @@ void pb_pup_device_setup_motor(pbio_port_id_t port, bool is_servo) { PBIO_IODEV_MODE_PUP_REL_MOTOR__POS; // Activate mode. - pb_assert(pbio_uartdev_set_mode(iodev, mode_id)); - pb_pup_device_wait_ready(iodev); + pb_pupdevices_obj_base_t device = { + .iodev = iodev, + }; + pb_pupdevices_get_data_blocking(&device, mode_id); #endif // !PYBRICKS_HUB_VIRTUALHUB } diff --git a/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c b/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c index 4c09c099e..57d728fe9 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_colordistancesensor.c @@ -18,57 +18,47 @@ // Class structure for ColorDistanceSensor. Note: first two members must match pb_ColorSensor_obj_t typedef struct _pupdevices_ColorDistanceSensor_obj_t { - mp_obj_base_t base; + pb_pupdevices_obj_base_t pup_base; mp_obj_t color_map; - pbio_iodev_t *iodev; mp_obj_t light; } pupdevices_ColorDistanceSensor_obj_t; -STATIC void raw_to_rgb(int16_t *raw, pbio_color_rgb_t *rgb) { - // Max observed value is ~440 so we scale to get a range of 0..255. - rgb->r = 1187 * raw[0] / 2048; - rgb->g = 1187 * raw[1] / 2048; - rgb->b = 1187 * raw[2] / 2048; -} - -pbio_iodev_t *pupdevices_ColorDistanceSensor__get_device(mp_obj_t obj) { - - // Assert that this is a ColorDistanceSensor +/** + * Gets base powered up object from the sensor. Used for Power Functions motor. + * + * @param [in] obj ColorDistanceSensor object. + */ +pb_pupdevices_obj_base_t *pupdevices_ColorDistanceSensor__get_device(mp_obj_t obj) { pb_assert_type(obj, &pb_type_pupdevices_ColorDistanceSensor); - - // Get and return device pointer pupdevices_ColorDistanceSensor_obj_t *self = MP_OBJ_TO_PTR(obj); - return self->iodev; + return &self->pup_base; } -// Ensures sensor is in RGB mode then converts the measured raw RGB value to HSV. -STATIC void pupdevices_ColorDistanceSensor__hsv(pupdevices_ColorDistanceSensor_obj_t *self, pbio_color_hsv_t *hsv) { - int16_t *raw = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__RGB_I); - - pbio_color_rgb_t rgb; - raw_to_rgb(raw, &rgb); - - pb_color_map_rgb_to_hsv(&rgb, hsv); -} +/** + * Callback for turning on the sensor light (red/green/blue/off), used by + * external ColorLight class instance. + * + * @param [in] context Sensor base object. + * @param [in] hsv Requested color, will be rounded to nearest color. + */ +STATIC mp_obj_t pupdevices_ColorDistanceSensor_light_on(void *context, const pbio_color_hsv_t *hsv) { + pb_pupdevices_obj_base_t *sensor = context; -// Several modes on this sensor have their own colors, so we can put it -// in a mode that will light up the color we want. -static uint8_t get_mode_for_color(const pbio_color_hsv_t *hsv) { + // Even though the mode takes a 0-10 value for color, only red, green and blue + // actually turn on the light. So we just pick the closest of these 3 to the + // requested color. + int8_t color; if (hsv->s < 50 || hsv->v < 50) { - return PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__COL_O; // off - } - if (hsv->h >= PBIO_COLOR_HUE_YELLOW && hsv->h < PBIO_COLOR_HUE_CYAN) { - return PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__PROX; // green + color = 0; // off + } else if (hsv->h >= PBIO_COLOR_HUE_YELLOW && hsv->h < PBIO_COLOR_HUE_CYAN) { + color = 5; // green } else if (hsv->h >= PBIO_COLOR_HUE_CYAN && hsv->h < PBIO_COLOR_HUE_MAGENTA) { - return PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__AMBI; // blue + color = 3; // blue + } else { + color = 9; // red } - return PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__REFLT; // red -} -STATIC mp_obj_t pupdevices_ColorDistanceSensor_light_on(void *context, const pbio_color_hsv_t *hsv) { - pbio_iodev_t *iodev = context; - pb_pup_device_get_data(iodev, get_mode_for_color(hsv)); - return mp_const_none; + return pb_pupdevices_set_data(sensor, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__COL_O, &color); } // pybricks.pupdevices.ColorDistanceSensor.__init__ @@ -77,13 +67,10 @@ STATIC mp_obj_t pupdevices_ColorDistanceSensor_make_new(const mp_obj_type_t *typ PB_ARG_REQUIRED(port)); pupdevices_ColorDistanceSensor_obj_t *self = mp_obj_malloc(pupdevices_ColorDistanceSensor_obj_t, type); - - pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); - - self->iodev = pb_pup_device_get_device(port, PBIO_IODEV_TYPE_ID_COLOR_DIST_SENSOR); + pb_pupdevices_init_class(&self->pup_base, port_in, PBIO_IODEV_TYPE_ID_COLOR_DIST_SENSOR); // Create an instance of the Light class - self->light = pb_type_ColorLight_external_obj_new(self->iodev, pupdevices_ColorDistanceSensor_light_on); + self->light = pb_type_ColorLight_external_obj_new(&self->pup_base, pupdevices_ColorDistanceSensor_light_on); // Save default color settings pb_color_map_save_default(&self->color_map); @@ -91,56 +78,56 @@ STATIC mp_obj_t pupdevices_ColorDistanceSensor_make_new(const mp_obj_type_t *typ return MP_OBJ_FROM_PTR(self); } +// Ensures sensor is in RGB mode then converts the measured raw RGB value to HSV. +STATIC void get_hsv_data(pupdevices_ColorDistanceSensor_obj_t *self, pbio_color_hsv_t *hsv) { + int16_t *raw = pb_pupdevices_get_data(&self->pup_base, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__RGB_I); + + // Max observed value is ~440 so we scale to get a range of 0..255. + pbio_color_rgb_t rgb; + rgb.r = 1187 * raw[0] / 2048; + rgb.g = 1187 * raw[1] / 2048; + rgb.b = 1187 * raw[2] / 2048; + pb_color_map_rgb_to_hsv(&rgb, hsv); +} + // pybricks.pupdevices.ColorDistanceSensor.color -STATIC mp_obj_t pupdevices_ColorDistanceSensor_color(mp_obj_t self_in) { +STATIC mp_obj_t get_color(mp_obj_t self_in) { pupdevices_ColorDistanceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - pbio_color_hsv_t hsv; - pupdevices_ColorDistanceSensor__hsv(self, &hsv); - - // Get and return discretized color based on HSV + get_hsv_data(self, &hsv); return pb_color_map_get_color(&self->color_map, &hsv); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorDistanceSensor_color_obj, pupdevices_ColorDistanceSensor_color); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_color_obj, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__RGB_I, get_color); // pybricks.pupdevices.ColorDistanceSensor.distance -STATIC mp_obj_t pupdevices_ColorDistanceSensor_distance(mp_obj_t self_in) { - pupdevices_ColorDistanceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - int8_t *distance = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__PROX); - return mp_obj_new_int(distance[0] * 10); +STATIC mp_obj_t get_distance(mp_obj_t self_in) { + int8_t *data = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__PROX); + return mp_obj_new_int(data[0] * 10); } -MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorDistanceSensor_distance_obj, pupdevices_ColorDistanceSensor_distance); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_distance_obj, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__PROX, get_distance); // pybricks.pupdevices.ColorDistanceSensor.reflection -STATIC mp_obj_t pupdevices_ColorDistanceSensor_reflection(mp_obj_t self_in) { - pupdevices_ColorDistanceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - int16_t *rgb = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__RGB_I); +STATIC mp_obj_t get_reflection(mp_obj_t self_in) { + int16_t *rgb = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__RGB_I); return mp_obj_new_int((rgb[0] + rgb[1] + rgb[2]) / 12); } -MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorDistanceSensor_reflection_obj, pupdevices_ColorDistanceSensor_reflection); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_reflection_obj, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__RGB_I, get_reflection); // pybricks.pupdevices.ColorDistanceSensor.ambient -STATIC mp_obj_t pupdevices_ColorDistanceSensor_ambient(mp_obj_t self_in) { - pupdevices_ColorDistanceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - int8_t *ambient = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__AMBI); +STATIC mp_obj_t get_ambient(mp_obj_t self_in) { + int8_t *ambient = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__AMBI); return mp_obj_new_int(ambient[0]); } -MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorDistanceSensor_ambient_obj, pupdevices_ColorDistanceSensor_ambient); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_ambient_obj, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__AMBI, get_ambient); // pybricks.pupdevices.ColorDistanceSensor.hsv -STATIC mp_obj_t pupdevices_ColorDistanceSensor_hsv(mp_obj_t self_in) { +STATIC mp_obj_t get_hsv(mp_obj_t self_in) { pupdevices_ColorDistanceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - - // Create color object pb_type_Color_obj_t *color = pb_type_Color_new_empty(); - - // Read HSV - pupdevices_ColorDistanceSensor__hsv(self, &color->hsv); - - // Return color + get_hsv_data(self, &color->hsv); return MP_OBJ_FROM_PTR(color); } -MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorDistanceSensor_hsv_obj, pupdevices_ColorDistanceSensor_hsv); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_hsv_obj, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__RGB_I, get_hsv); STATIC const pb_attr_dict_entry_t pupdevices_ColorDistanceSensor_attr_dict[] = { PB_DEFINE_CONST_ATTR_RO(MP_QSTR_light, pupdevices_ColorDistanceSensor_obj_t, light), @@ -149,11 +136,11 @@ STATIC const pb_attr_dict_entry_t pupdevices_ColorDistanceSensor_attr_dict[] = { // dir(pybricks.pupdevices.ColorDistanceSensor) STATIC const mp_rom_map_elem_t pupdevices_ColorDistanceSensor_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_color), MP_ROM_PTR(&pupdevices_ColorDistanceSensor_color_obj) }, - { MP_ROM_QSTR(MP_QSTR_reflection), MP_ROM_PTR(&pupdevices_ColorDistanceSensor_reflection_obj) }, - { MP_ROM_QSTR(MP_QSTR_ambient), MP_ROM_PTR(&pupdevices_ColorDistanceSensor_ambient_obj) }, - { MP_ROM_QSTR(MP_QSTR_distance), MP_ROM_PTR(&pupdevices_ColorDistanceSensor_distance_obj) }, - { MP_ROM_QSTR(MP_QSTR_hsv), MP_ROM_PTR(&pupdevices_ColorDistanceSensor_hsv_obj) }, + { MP_ROM_QSTR(MP_QSTR_color), MP_ROM_PTR(&get_color_obj) }, + { MP_ROM_QSTR(MP_QSTR_reflection), MP_ROM_PTR(&get_reflection_obj) }, + { MP_ROM_QSTR(MP_QSTR_ambient), MP_ROM_PTR(&get_ambient_obj) }, + { MP_ROM_QSTR(MP_QSTR_distance), MP_ROM_PTR(&get_distance_obj) }, + { MP_ROM_QSTR(MP_QSTR_hsv), MP_ROM_PTR(&get_hsv_obj) }, { MP_ROM_QSTR(MP_QSTR_detectable_colors), MP_ROM_PTR(&pb_ColorSensor_detectable_colors_obj) }, }; STATIC MP_DEFINE_CONST_DICT(pupdevices_ColorDistanceSensor_locals_dict, pupdevices_ColorDistanceSensor_locals_dict_table); diff --git a/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c b/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c index 60c20e327..279b0a106 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c +++ b/pybricks/pupdevices/pb_type_pupdevices_colorlightmatrix.c @@ -15,8 +15,7 @@ // Class structure for ColorLightMatrix typedef struct _pupdevices_ColorLightMatrix_obj_t { - mp_obj_base_t base; - pbio_iodev_t *iodev; + pb_pupdevices_obj_base_t pup_base; } pupdevices_ColorLightMatrix_obj_t; // pybricks.pupdevices.ColorLightMatrix.__init__ @@ -25,11 +24,7 @@ STATIC mp_obj_t pupdevices_ColorLightMatrix_make_new(const mp_obj_type_t *type, PB_ARG_REQUIRED(port)); pupdevices_ColorLightMatrix_obj_t *self = mp_obj_malloc(pupdevices_ColorLightMatrix_obj_t, type); - - pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); - - // Get iodevice - self->iodev = pb_pup_device_get_device(port, PBIO_IODEV_TYPE_ID_TECHNIC_COLOR_LIGHT_MATRIX); + pb_pupdevices_init_class(&self->pup_base, port_in, PBIO_IODEV_TYPE_ID_TECHNIC_COLOR_LIGHT_MATRIX); return MP_OBJ_FROM_PTR(self); } @@ -86,17 +81,14 @@ STATIC mp_obj_t pupdevices_ColorLightMatrix_on(size_t n_args, const mp_obj_t *po } // Activate all colors. - pb_pup_device_set_data(self->iodev, PBIO_IODEV_MODE_PUP_COLOR_LIGHT_MATRIX__PIX_O, color_ids); - - return mp_const_none; + return pb_pupdevices_set_data(&self->pup_base, PBIO_IODEV_MODE_PUP_COLOR_LIGHT_MATRIX__PIX_O, color_ids); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pupdevices_ColorLightMatrix_on_obj, 1, pupdevices_ColorLightMatrix_on); // pybricks.pupdevices.ColorLightMatrix.off STATIC mp_obj_t pupdevices_ColorLightMatrix_off(mp_obj_t self_in) { const mp_obj_t pos_args[] = { self_in, MP_OBJ_FROM_PTR(&pb_Color_NONE_obj) }; - pupdevices_ColorLightMatrix_on(MP_ARRAY_SIZE(pos_args), pos_args, NULL); - return mp_const_none; + return pupdevices_ColorLightMatrix_on(MP_ARRAY_SIZE(pos_args), pos_args, NULL); } MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorLightMatrix_off_obj, pupdevices_ColorLightMatrix_off); diff --git a/pybricks/pupdevices/pb_type_pupdevices_colorsensor.c b/pybricks/pupdevices/pb_type_pupdevices_colorsensor.c index d71ae7fb8..18957f651 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_colorsensor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_colorsensor.c @@ -18,35 +18,62 @@ // Class structure for ColorSensor. Note: first two members must match pb_ColorSensor_obj_t typedef struct _pupdevices_ColorSensor_obj_t { - mp_obj_base_t base; + pb_pupdevices_obj_base_t pup_base; mp_obj_t color_map; - pbio_iodev_t *iodev; mp_obj_t lights; } pupdevices_ColorSensor_obj_t; -// pybricks._common.ColorSensor._get_hsv_reflected -STATIC void pupdevices_ColorSensor__get_hsv_reflected(pbio_iodev_t *iodev, pbio_color_hsv_t *hsv) { +// pybricks.pupdevices.ColorSensor.__init__ +STATIC mp_obj_t pupdevices_ColorSensor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { + PB_PARSE_ARGS_CLASS(n_args, n_kw, args, + PB_ARG_REQUIRED(port)); + + pupdevices_ColorSensor_obj_t *self = mp_obj_malloc(pupdevices_ColorSensor_obj_t, type); + pb_pupdevices_init_class(&self->pup_base, port_in, PBIO_IODEV_TYPE_ID_SPIKE_COLOR_SENSOR); + + // Create an instance of the LightArray class + self->lights = common_LightArray_obj_make_new(&self->pup_base, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__LIGHT, 3); + + // Do one reading to make sure everything is working and to set default mode + pb_pupdevices_get_data_blocking(&self->pup_base, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I); + + // Save default settings + pb_color_map_save_default(&self->color_map); + + return MP_OBJ_FROM_PTR(self); +} + +// pybricks.pupdevices.ColorSensor.reflection +STATIC mp_obj_t get_reflection(mp_obj_t self_in) { + // Get reflection from average RGB reflection, which ranges from 0 to 3*1024 + int16_t *data = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I); + return mp_obj_new_int((data[0] + data[1] + data[2]) * 100 / 3072); +} +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_reflection_obj, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I, get_reflection); - // Read RGB - int16_t *data = pb_pup_device_get_data(iodev, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I); +// pybricks.pupdevices.ColorSensor.ambient +STATIC mp_obj_t get_ambient(mp_obj_t self_in) { + // Get ambient from "V" in SHSV (0--10000), scaled to 0 to 100 + int16_t *data = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__SHSV); + return pb_obj_new_fraction(data[2], 100); +} +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_ambient_obj, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__SHSV, get_ambient); +// Helper for getting HSV with the light on. +STATIC void get_hsv_reflected(mp_obj_t self_in, pbio_color_hsv_t *hsv) { + int16_t *data = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I); const pbio_color_rgb_t rgb = { .r = data[0] == 1024 ? 255 : data[0] >> 2, .g = data[1] == 1024 ? 255 : data[1] >> 2, .b = data[2] == 1024 ? 255 : data[2] >> 2, }; - - // Convert to HSV pb_color_map_rgb_to_hsv(&rgb, hsv); } -// pybricks._common.ColorSensor._get_hsv_ambient -STATIC void pupdevices_ColorSensor__get_hsv_ambient(pbio_iodev_t *iodev, pbio_color_hsv_t *hsv) { - - // Read SHSV mode (light off). This data is not available in RGB format - int16_t *data = pb_pup_device_get_data(iodev, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__SHSV); - - // Scale saturation and value to match 0-100% range in typical applications +// Helper for getting HSV with the light off, scale saturation and value to +// match 0-100% range in typical applications. +STATIC void get_hsv_ambient(mp_obj_t self_in, pbio_color_hsv_t *hsv) { + int16_t *data = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__SHSV); hsv->h = data[0]; hsv->s = data[1] / 10; if (hsv->s > 100) { @@ -58,94 +85,67 @@ STATIC void pupdevices_ColorSensor__get_hsv_ambient(pbio_iodev_t *iodev, pbio_co } } -// pybricks.pupdevices.ColorSensor.__init__ -STATIC mp_obj_t pupdevices_ColorSensor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { - PB_PARSE_ARGS_CLASS(n_args, n_kw, args, - PB_ARG_REQUIRED(port)); - - pupdevices_ColorSensor_obj_t *self = mp_obj_malloc(pupdevices_ColorSensor_obj_t, type); - - pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); - - // Get iodevices - self->iodev = pb_pup_device_get_device(port, PBIO_IODEV_TYPE_ID_SPIKE_COLOR_SENSOR); +// pybricks.pupdevices.ColorSensor.hsv(surface=True) +STATIC mp_obj_t get_hsv_surface_true(mp_obj_t self_in) { + pb_type_Color_obj_t *color = pb_type_Color_new_empty(); + get_hsv_reflected(self_in, &color->hsv); + return MP_OBJ_FROM_PTR(color); +} +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_hsv_surface_true_obj, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I, get_hsv_surface_true); - // Create an instance of the LightArray class - self->lights = common_LightArray_obj_make_new(self->iodev, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__LIGHT, 3); +// pybricks.pupdevices.ColorSensor.hsv(surface=False) +STATIC mp_obj_t get_hsv_surface_false(mp_obj_t self_in) { + pb_type_Color_obj_t *color = pb_type_Color_new_empty(); + get_hsv_ambient(self_in, &color->hsv); + return MP_OBJ_FROM_PTR(color); +} +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_hsv_surface_false_obj, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I, get_hsv_surface_false); - // Do one reading to make sure everything is working and to set default mode +// pybricks.pupdevices.ColorSensor.color(surface=True) +STATIC mp_obj_t get_color_surface_true(mp_obj_t self_in) { pbio_color_hsv_t hsv; - pupdevices_ColorSensor__get_hsv_reflected(self->iodev, &hsv); - - // Save default settings - pb_color_map_save_default(&self->color_map); + get_hsv_reflected(self_in, &hsv); + pupdevices_ColorSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); + return pb_color_map_get_color(&self->color_map, &hsv); +} +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_color_surface_true_obj, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I, get_color_surface_true); - return MP_OBJ_FROM_PTR(self); +// pybricks.pupdevices.ColorSensor.color(surface=False) +STATIC mp_obj_t get_color_surface_false(mp_obj_t self_in) { + pbio_color_hsv_t hsv; + get_hsv_ambient(self_in, &hsv); + pupdevices_ColorSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); + return pb_color_map_get_color(&self->color_map, &hsv); } +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_color_surface_false_obj, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I, get_color_surface_false); -// pybricks._common.ColorSensor.hsv -STATIC mp_obj_t pupdevices_ColorSensor_hsv(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +// pybricks.pupdevices.ColorSensor.hsv +STATIC mp_obj_t get_hsv(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pupdevices_ColorSensor_obj_t, self, PB_ARG_DEFAULT_TRUE(surface)); - // Create color object - pb_type_Color_obj_t *color = pb_type_Color_new_empty(); - - // Get either reflected or ambient HSV + (void)self; if (mp_obj_is_true(surface_in)) { - pupdevices_ColorSensor__get_hsv_reflected(self->iodev, &color->hsv); - } else { - pupdevices_ColorSensor__get_hsv_ambient(self->iodev, &color->hsv); + return pb_pupdevices_method_call(MP_OBJ_FROM_PTR(&get_hsv_surface_true_obj), 1, 0, pos_args); } - - // Return color - return MP_OBJ_FROM_PTR(color); + return pb_pupdevices_method_call(MP_OBJ_FROM_PTR(&get_hsv_surface_false_obj), 1, 0, pos_args); } -STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pupdevices_ColorSensor_hsv_obj, 1, pupdevices_ColorSensor_hsv); +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(get_hsv_obj, 1, get_hsv); -// pybricks._common.ColorSensor.color -STATIC mp_obj_t pupdevices_ColorSensor_color(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +// pybricks.pupdevices.ColorSensor.color +STATIC mp_obj_t get_color(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pupdevices_ColorSensor_obj_t, self, PB_ARG_DEFAULT_TRUE(surface)); - // Get either reflected or ambient HSV - pbio_color_hsv_t hsv; + (void)self; if (mp_obj_is_true(surface_in)) { - pupdevices_ColorSensor__get_hsv_reflected(self->iodev, &hsv); - } else { - pupdevices_ColorSensor__get_hsv_ambient(self->iodev, &hsv); + return pb_pupdevices_method_call(MP_OBJ_FROM_PTR(&get_color_surface_true_obj), 1, 0, pos_args); } - - // Get and return discretized color - return pb_color_map_get_color(&self->color_map, &hsv); -} -STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pupdevices_ColorSensor_color_obj, 1, pupdevices_ColorSensor_color); - -// pybricks.pupdevices.ColorSensor.reflection -STATIC mp_obj_t pupdevices_ColorSensor_reflection(mp_obj_t self_in) { - pupdevices_ColorSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - - // Get reflection from average RGB reflection, which ranges from 0 to 3*1024 - int16_t *data = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__RGB_I); - - // Return value as reflection - return mp_obj_new_int((data[0] + data[1] + data[2]) * 100 / 3072); -} -MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorSensor_reflection_obj, pupdevices_ColorSensor_reflection); - -// pybricks.pupdevices.ColorSensor.ambient -STATIC mp_obj_t pupdevices_ColorSensor_ambient(mp_obj_t self_in) { - pupdevices_ColorSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - - // Get ambient from "V" in SHSV, which ranges from 0 to 10000 - int16_t *data = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__SHSV); - - // Return scaled to 100. - return pb_obj_new_fraction(data[2], 100); + return pb_pupdevices_method_call(MP_OBJ_FROM_PTR(&get_color_surface_false_obj), 1, 0, pos_args); } -MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ColorSensor_ambient_obj, pupdevices_ColorSensor_ambient); +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(get_color_obj, 1, get_color); STATIC const pb_attr_dict_entry_t pupdevices_ColorSensor_attr_dict[] = { PB_DEFINE_CONST_ATTR_RO(MP_QSTR_lights, pupdevices_ColorSensor_obj_t, lights), @@ -154,10 +154,10 @@ STATIC const pb_attr_dict_entry_t pupdevices_ColorSensor_attr_dict[] = { // dir(pybricks.pupdevices.ColorSensor) STATIC const mp_rom_map_elem_t pupdevices_ColorSensor_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_hsv), MP_ROM_PTR(&pupdevices_ColorSensor_hsv_obj) }, - { MP_ROM_QSTR(MP_QSTR_color), MP_ROM_PTR(&pupdevices_ColorSensor_color_obj) }, - { MP_ROM_QSTR(MP_QSTR_reflection), MP_ROM_PTR(&pupdevices_ColorSensor_reflection_obj) }, - { MP_ROM_QSTR(MP_QSTR_ambient), MP_ROM_PTR(&pupdevices_ColorSensor_ambient_obj) }, + { MP_ROM_QSTR(MP_QSTR_hsv), MP_ROM_PTR(&get_hsv_obj) }, + { MP_ROM_QSTR(MP_QSTR_color), MP_ROM_PTR(&get_color_obj) }, + { MP_ROM_QSTR(MP_QSTR_reflection), MP_ROM_PTR(&get_reflection_obj) }, + { MP_ROM_QSTR(MP_QSTR_ambient), MP_ROM_PTR(&get_ambient_obj) }, { MP_ROM_QSTR(MP_QSTR_detectable_colors), MP_ROM_PTR(&pb_ColorSensor_detectable_colors_obj) }, }; STATIC MP_DEFINE_CONST_DICT(pupdevices_ColorSensor_locals_dict, pupdevices_ColorSensor_locals_dict_table); diff --git a/pybricks/pupdevices/pb_type_pupdevices_forcesensor.c b/pybricks/pupdevices/pb_type_pupdevices_forcesensor.c index fd1728b07..c3737db01 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_forcesensor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_forcesensor.c @@ -17,57 +17,28 @@ // Class structure for ForceSensor typedef struct _pupdevices_ForceSensor_obj_t { - mp_obj_base_t base; - pbio_iodev_t *iodev; + pb_pupdevices_obj_base_t pup_base; int32_t raw_released; int32_t raw_offset; int32_t raw_start; int32_t raw_end; + int32_t pressed_threshold; } pupdevices_ForceSensor_obj_t; -// pybricks.pupdevices.ForceSensor._raw -STATIC int32_t pupdevices_ForceSensor__raw(pbio_iodev_t *iodev) { - int16_t *raw = pb_pup_device_get_data(iodev, PBIO_IODEV_MODE_PUP_FORCE_SENSOR__FRAW); - return *raw; -} - -// pybricks.pupdevices.ForceSensor._force -STATIC int32_t pupdevices_ForceSensor__force(pupdevices_ForceSensor_obj_t *self) { - // Get raw sensor value - int32_t raw = pupdevices_ForceSensor__raw(self->iodev); - - // Get force in millinewtons - int32_t force = (10000 * (raw - self->raw_released - self->raw_offset)) / (self->raw_end - self->raw_released); - - // With LEGO scaling, initial section is negative, so mask it and return - return force < 0 ? 0 : force; -} - -// pybricks.pupdevices.ForceSensor._distance -STATIC int32_t pupdevices_ForceSensor__distance(pupdevices_ForceSensor_obj_t *self) { - int32_t raw = pupdevices_ForceSensor__raw(self->iodev); - - // Get distance in micrometers - return (6670 * (raw - self->raw_released)) / (self->raw_end - self->raw_released); -} - // pybricks.pupdevices.ForceSensor.__init__ STATIC mp_obj_t pupdevices_ForceSensor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { PB_PARSE_ARGS_CLASS(n_args, n_kw, args, PB_ARG_REQUIRED(port)); pupdevices_ForceSensor_obj_t *self = mp_obj_malloc(pupdevices_ForceSensor_obj_t, type); - - pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); - - // Get iodevice. - self->iodev = pb_pup_device_get_device(port, PBIO_IODEV_TYPE_ID_SPIKE_FORCE_SENSOR); + pb_pupdevices_init_class(&self->pup_base, port_in, PBIO_IODEV_TYPE_ID_SPIKE_FORCE_SENSOR); // Read scaling factors. - int16_t *calib = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_FORCE_SENSOR__CALIB); + int16_t *calib = pb_pupdevices_get_data_blocking(&self->pup_base, PBIO_IODEV_MODE_PUP_FORCE_SENSOR__CALIB); self->raw_offset = calib[1]; self->raw_released = calib[2]; self->raw_end = calib[6]; + self->pressed_threshold = 0; // Do sanity check on values to verify calibration read succeeded if (self->raw_released >= self->raw_end) { @@ -75,62 +46,78 @@ STATIC mp_obj_t pupdevices_ForceSensor_make_new(const mp_obj_type_t *type, size_ } // Do one measurement to set up mode used for all methods. - pupdevices_ForceSensor__raw(self->iodev); + pb_pupdevices_get_data_blocking(&self->pup_base, PBIO_IODEV_MODE_PUP_FORCE_SENSOR__FRAW); return MP_OBJ_FROM_PTR(self); } -// pybricks.pupdevices.ForceSensor.touched -STATIC mp_obj_t pupdevices_ForceSensor_touched(mp_obj_t self_in) { +// pybricks.pupdevices.ForceSensor._raw +STATIC int32_t get_raw(mp_obj_t self_in) { + int16_t *raw = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_FORCE_SENSOR__FRAW); + return *raw; +} + +// pybricks.pupdevices.ForceSensor._force +STATIC int32_t get_force_mN(mp_obj_t self_in) { + // Get force in millinewtons pupdevices_ForceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); + int32_t force = (10000 * (get_raw(self_in) - self->raw_released - self->raw_offset)) / (self->raw_end - self->raw_released); + // With LEGO scaling, initial section is negative, so mask it and return + return force < 0 ? 0 : force; +} +// pybricks.pupdevices.ForceSensor.touched +STATIC mp_obj_t get_touched(mp_obj_t self_in) { + pupdevices_ForceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); // Return true if raw value is just above detectable change, with a small // margin to account for small calibration tolerances. - return mp_obj_new_bool(pupdevices_ForceSensor__raw(self->iodev) > self->raw_released + 4); + return mp_obj_new_bool(get_raw(self_in) > self->raw_released + 4); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ForceSensor_touched_obj, pupdevices_ForceSensor_touched); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_touched_obj, PBIO_IODEV_MODE_PUP_FORCE_SENSOR__FRAW, get_touched); // pybricks.pupdevices.ForceSensor.force -STATIC mp_obj_t pupdevices_ForceSensor_force(mp_obj_t self_in) { - pupdevices_ForceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - - // Return force in newtons - return pb_obj_new_fraction(pupdevices_ForceSensor__force(self), 1000); +STATIC mp_obj_t get_force(mp_obj_t self_in) { + return pb_obj_new_fraction(get_force_mN(self_in), 1000); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ForceSensor_force_obj, pupdevices_ForceSensor_force); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_force_obj, PBIO_IODEV_MODE_PUP_FORCE_SENSOR__FRAW, get_force); // pybricks.pupdevices.ForceSensor.distance -STATIC mp_obj_t pupdevices_ForceSensor_distance(mp_obj_t self_in) { +STATIC mp_obj_t get_distance(mp_obj_t self_in) { pupdevices_ForceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); + int32_t distance_um = (6670 * (get_raw(self_in) - self->raw_released)) / (self->raw_end - self->raw_released); + return pb_obj_new_fraction(distance_um, 1000); +} +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_distance_obj, PBIO_IODEV_MODE_PUP_FORCE_SENSOR__FRAW, get_distance); - // Return in millimeters - return pb_obj_new_fraction(pupdevices_ForceSensor__distance(self), 1000); +// pybricks.pupdevices.ForceSensor.pressed(force=default) +STATIC mp_obj_t get_pressed_simple(mp_obj_t self_in) { + pupdevices_ForceSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); + return mp_obj_new_bool(get_force_mN(self_in) >= self->pressed_threshold); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_ForceSensor_distance_obj, pupdevices_ForceSensor_distance); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_pressed_simple_obj, PBIO_IODEV_MODE_PUP_FORCE_SENSOR__FRAW, get_pressed_simple); // pybricks.pupdevices.ForceSensor.pressed -STATIC mp_obj_t pupdevices_ForceSensor_pressed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +STATIC mp_obj_t get_pressed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pupdevices_ForceSensor_obj_t, self, PB_ARG_DEFAULT_INT(force, 3)); #if MICROPY_PY_BUILTINS_FLOAT - int32_t f_arg = (int32_t)(mp_obj_get_float(force_in) * 1000); + self->pressed_threshold = (int32_t)(mp_obj_get_float(force_in) * 1000); #else - int32_t f_arg = pb_obj_get_int(force_in) * 1000; + self->pressed_threshold = pb_obj_get_int(force_in) * 1000; #endif - // Return true if the force is bigger than given threshold - return mp_obj_new_bool(pupdevices_ForceSensor__force(self) >= f_arg); + return pb_pupdevices_method_call(MP_OBJ_FROM_PTR(&get_pressed_simple_obj), 1, 0, pos_args); } -MP_DEFINE_CONST_FUN_OBJ_KW(pupdevices_ForceSensor_pressed_obj, 1, pupdevices_ForceSensor_pressed); +MP_DEFINE_CONST_FUN_OBJ_KW(get_pressed_obj, 1, get_pressed); // dir(pybricks.pupdevices.ForceSensor) STATIC const mp_rom_map_elem_t pupdevices_ForceSensor_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_touched), MP_ROM_PTR(&pupdevices_ForceSensor_touched_obj) }, - { MP_ROM_QSTR(MP_QSTR_force), MP_ROM_PTR(&pupdevices_ForceSensor_force_obj) }, - { MP_ROM_QSTR(MP_QSTR_pressed), MP_ROM_PTR(&pupdevices_ForceSensor_pressed_obj) }, - { MP_ROM_QSTR(MP_QSTR_distance), MP_ROM_PTR(&pupdevices_ForceSensor_distance_obj) }, + { MP_ROM_QSTR(MP_QSTR_touched), MP_ROM_PTR(&get_touched_obj) }, + { MP_ROM_QSTR(MP_QSTR_force), MP_ROM_PTR(&get_force_obj) }, + { MP_ROM_QSTR(MP_QSTR_pressed), MP_ROM_PTR(&get_pressed_obj) }, + { MP_ROM_QSTR(MP_QSTR_distance), MP_ROM_PTR(&get_distance_obj) }, }; STATIC MP_DEFINE_CONST_DICT(pupdevices_ForceSensor_locals_dict, pupdevices_ForceSensor_locals_dict_table); diff --git a/pybricks/pupdevices/pb_type_pupdevices_infraredsensor.c b/pybricks/pupdevices/pb_type_pupdevices_infraredsensor.c index fae126eb3..2c322063e 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_infraredsensor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_infraredsensor.c @@ -14,70 +14,52 @@ // Class structure for InfraredSensor typedef struct _pupdevices_InfraredSensor_obj_t { - mp_obj_base_t base; - pbio_iodev_t *iodev; + pb_pupdevices_obj_base_t pup_base; int32_t count_offset; } pupdevices_InfraredSensor_obj_t; -// pybricks.pupdevices.InfraredSensor._raw -STATIC int32_t pupdevices_InfraredSensor__raw(pbio_iodev_t *iodev) { - int32_t *raw = pb_pup_device_get_data(iodev, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__CAL); - return raw[0]; -} - -// pybricks.pupdevices.InfraredSensor._count -STATIC int32_t pupdevices_InfraredSensor__count(pbio_iodev_t *iodev) { - int32_t *count = pb_pup_device_get_data(iodev, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__COUNT); - return *count; -} - // pybricks.pupdevices.InfraredSensor.__init__ STATIC mp_obj_t pupdevices_InfraredSensor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { PB_PARSE_ARGS_CLASS(n_args, n_kw, args, PB_ARG_REQUIRED(port)); pupdevices_InfraredSensor_obj_t *self = mp_obj_malloc(pupdevices_InfraredSensor_obj_t, type); - - pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); - - // Get iodevice - self->iodev = pb_pup_device_get_device(port, PBIO_IODEV_TYPE_ID_WEDO2_MOTION_SENSOR); + pb_pupdevices_init_class(&self->pup_base, port_in, PBIO_IODEV_TYPE_ID_WEDO2_MOTION_SENSOR); // Reset sensor counter and get sensor back in sensing mode - self->count_offset = pupdevices_InfraredSensor__count(self->iodev); - pupdevices_InfraredSensor__raw(self->iodev); + self->count_offset = *(int32_t *)pb_pupdevices_get_data_blocking(&self->pup_base, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__COUNT); + pb_pupdevices_get_data_blocking(&self->pup_base, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__CAL); return MP_OBJ_FROM_PTR(self); } // pybricks.pupdevices.InfraredSensor.count -STATIC mp_obj_t pupdevices_InfraredSensor_count(mp_obj_t self_in) { +STATIC mp_obj_t get_count(mp_obj_t self_in) { pupdevices_InfraredSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - return mp_obj_new_int(pupdevices_InfraredSensor__count(self->iodev) - self->count_offset); + int32_t *count = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__COUNT); + return mp_obj_new_int(count[0] - self->count_offset); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_InfraredSensor_count_obj, pupdevices_InfraredSensor_count); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_count_obj, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__COUNT, get_count); // pybricks.pupdevices.InfraredSensor.reflection -STATIC mp_obj_t pupdevices_InfraredSensor_reflection(mp_obj_t self_in) { - pupdevices_InfraredSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - int32_t raw = pupdevices_InfraredSensor__raw(self->iodev); - return pb_obj_new_fraction(raw, 5); +STATIC mp_obj_t get_reflection(mp_obj_t self_in) { + int16_t *data = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__CAL); + return pb_obj_new_fraction(data[0], 5); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_InfraredSensor_reflection_obj, pupdevices_InfraredSensor_reflection); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_reflection_obj, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__CAL, get_reflection); // pybricks.pupdevices.InfraredSensor.distance -STATIC mp_obj_t pupdevices_InfraredSensor_distance(mp_obj_t self_in) { - pupdevices_InfraredSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - int32_t raw = pupdevices_InfraredSensor__raw(self->iodev); - return mp_obj_new_int(1100 / (10 + raw)); +STATIC mp_obj_t get_distance(mp_obj_t self_in) { + int16_t *data = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__CAL); + return mp_obj_new_int(1100 / (10 + data[0])); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_InfraredSensor_distance_obj, pupdevices_InfraredSensor_distance); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_distance_obj, PBIO_IODEV_MODE_PUP_WEDO2_MOTION_SENSOR__CAL, get_distance); // dir(pybricks.pupdevices.InfraredSensor) STATIC const mp_rom_map_elem_t pupdevices_InfraredSensor_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_count), MP_ROM_PTR(&pupdevices_InfraredSensor_count_obj) }, - { MP_ROM_QSTR(MP_QSTR_reflection), MP_ROM_PTR(&pupdevices_InfraredSensor_reflection_obj) }, - { MP_ROM_QSTR(MP_QSTR_distance), MP_ROM_PTR(&pupdevices_InfraredSensor_distance_obj) }, + { MP_ROM_QSTR(MP_QSTR_count), MP_ROM_PTR(&get_count_obj) }, + { MP_ROM_QSTR(MP_QSTR_reflection), MP_ROM_PTR(&get_reflection_obj) }, + { MP_ROM_QSTR(MP_QSTR_distance), MP_ROM_PTR(&get_distance_obj) }, }; STATIC MP_DEFINE_CONST_DICT(pupdevices_InfraredSensor_locals_dict, pupdevices_InfraredSensor_locals_dict_table); diff --git a/pybricks/pupdevices/pb_type_pupdevices_pfmotor.c b/pybricks/pupdevices/pb_type_pupdevices_pfmotor.c index d854dc663..add0ba840 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_pfmotor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_pfmotor.c @@ -22,7 +22,8 @@ // Class structure for PFMotor. typedef struct _pupdevices_PFMotor_obj_t { mp_obj_base_t base; - pbio_iodev_t *iodev; + // Point to ColorDistanceSensor, this class does not manage its own device. + pb_pupdevices_obj_base_t *pup_base; uint8_t channel; bool use_blue_port; pbio_direction_t direction; @@ -36,9 +37,6 @@ STATIC mp_obj_t pupdevices_PFMotor_make_new(const mp_obj_type_t *type, size_t n_ PB_ARG_REQUIRED(color), PB_ARG_DEFAULT_OBJ(positive_direction, pb_Direction_CLOCKWISE_obj)); - // Get device - pbio_iodev_t *sensor = pupdevices_ColorDistanceSensor__get_device(sensor_in); - // Get channel mp_int_t channel = mp_obj_get_int(channel_in); if (channel < 1 || channel > 4) { @@ -57,9 +55,7 @@ STATIC mp_obj_t pupdevices_PFMotor_make_new(const mp_obj_type_t *type, size_t n_ // All checks have passed, so create the object pupdevices_PFMotor_obj_t *self = mp_obj_malloc(pupdevices_PFMotor_obj_t, type); - - // Save init arguments - self->iodev = sensor; + self->pup_base = pupdevices_ColorDistanceSensor__get_device(sensor_in); self->channel = channel; self->use_blue_port = use_blue_port; self->direction = positive_direction; @@ -67,7 +63,7 @@ STATIC mp_obj_t pupdevices_PFMotor_make_new(const mp_obj_type_t *type, size_t n_ return MP_OBJ_FROM_PTR(self); } -STATIC void pupdevices_PFMotor__send(pupdevices_PFMotor_obj_t *self, int16_t message) { +STATIC mp_obj_t pupdevices_PFMotor__send(pupdevices_PFMotor_obj_t *self, int16_t message) { // Choose blue or red output message |= (self->use_blue_port) << 4; @@ -80,7 +76,7 @@ STATIC void pupdevices_PFMotor__send(pupdevices_PFMotor_obj_t *self, int16_t mes // Send the data to the device. This automatically delays by about 250 ms // to ensure the data is properly sent and received. This also ensures that // the message will still work if two identical values are sent in a row. - pb_pup_device_set_data(self->iodev, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__IR_TX, &message); + return pb_pupdevices_set_data(self->pup_base, PBIO_IODEV_MODE_PUP_COLOR_DISTANCE_SENSOR__IR_TX, &message); } // pybricks.pupdevices.PFMotor.dc @@ -106,23 +102,19 @@ STATIC mp_obj_t pupdevices_PFMotor_dc(size_t n_args, const mp_obj_t *pos_args, m // // For forward, PWM steps 1--7 are binary 1 to 7, backward is 15--9 int32_t message = (forward || pwm == 0) ? pwm : 16 - pwm; - pupdevices_PFMotor__send(self, message); - - return mp_const_none; + return pupdevices_PFMotor__send(self, message); } MP_DEFINE_CONST_FUN_OBJ_KW(pupdevices_PFMotor_dc_obj, 1, pupdevices_PFMotor_dc); // pybricks.pupdevices.PFMotor.stop STATIC mp_obj_t pupdevices_PFMotor_stop(mp_obj_t self_in) { - pupdevices_PFMotor__send(MP_OBJ_TO_PTR(self_in), 0); - return mp_const_none; + return pupdevices_PFMotor__send(MP_OBJ_TO_PTR(self_in), 0); } MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_PFMotor_stop_obj, pupdevices_PFMotor_stop); // pybricks.pupdevices.PFMotor.brake STATIC mp_obj_t pupdevices_PFMotor_brake(mp_obj_t self_in) { - pupdevices_PFMotor__send(MP_OBJ_TO_PTR(self_in), 8); - return mp_const_none; + return pupdevices_PFMotor__send(MP_OBJ_TO_PTR(self_in), 8); } MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_PFMotor_brake_obj, pupdevices_PFMotor_brake); diff --git a/pybricks/pupdevices/pb_type_pupdevices_tiltsensor.c b/pybricks/pupdevices/pb_type_pupdevices_tiltsensor.c index 5357821c5..78d492ed8 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_tiltsensor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_tiltsensor.c @@ -14,8 +14,7 @@ // Class structure for TiltSensor typedef struct _pupdevices_TiltSensor_obj_t { - mp_obj_base_t base; - pbio_iodev_t *iodev; + pb_pupdevices_obj_base_t pup_base; } pupdevices_TiltSensor_obj_t; // pybricks.pupdevices.TiltSensor.__init__ @@ -24,29 +23,23 @@ STATIC mp_obj_t pupdevices_TiltSensor_make_new(const mp_obj_type_t *type, size_t PB_ARG_REQUIRED(port)); pupdevices_TiltSensor_obj_t *self = mp_obj_malloc(pupdevices_TiltSensor_obj_t, type); - - pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); - - // Get iodevice - self->iodev = pb_pup_device_get_device(port, PBIO_IODEV_TYPE_ID_WEDO2_TILT_SENSOR); - + pb_pupdevices_init_class(&self->pup_base, port_in, PBIO_IODEV_TYPE_ID_WEDO2_MOTION_SENSOR); return MP_OBJ_FROM_PTR(self); } // pybricks.pupdevices.TiltSensor.tilt -STATIC mp_obj_t pupdevices_TiltSensor_tilt(mp_obj_t self_in) { - pupdevices_TiltSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - int8_t *tilt = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_WEDO2_TILT_SENSOR__ANGLE); +STATIC mp_obj_t get_tilt(mp_obj_t self_in) { + int8_t *tilt = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_WEDO2_TILT_SENSOR__ANGLE); mp_obj_t ret[2]; ret[0] = mp_obj_new_int(tilt[1]); ret[1] = mp_obj_new_int(tilt[0]); return mp_obj_new_tuple(MP_ARRAY_SIZE(ret), ret); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_TiltSensor_tilt_obj, pupdevices_TiltSensor_tilt); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_tilt_obj, PBIO_IODEV_MODE_PUP_WEDO2_TILT_SENSOR__ANGLE, get_tilt); // dir(pybricks.pupdevices.TiltSensor) STATIC const mp_rom_map_elem_t pupdevices_TiltSensor_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_tilt), MP_ROM_PTR(&pupdevices_TiltSensor_tilt_obj) }, + { MP_ROM_QSTR(MP_QSTR_tilt), MP_ROM_PTR(&get_tilt_obj) }, }; STATIC MP_DEFINE_CONST_DICT(pupdevices_TiltSensor_locals_dict, pupdevices_TiltSensor_locals_dict_table); diff --git a/pybricks/pupdevices/pb_type_pupdevices_ultrasonicsensor.c b/pybricks/pupdevices/pb_type_pupdevices_ultrasonicsensor.c index a4ec02889..5841423a5 100644 --- a/pybricks/pupdevices/pb_type_pupdevices_ultrasonicsensor.c +++ b/pybricks/pupdevices/pb_type_pupdevices_ultrasonicsensor.c @@ -15,9 +15,9 @@ // Class structure for UltrasonicSensor typedef struct _pupdevices_UltrasonicSensor_obj_t { + pb_pupdevices_obj_base_t pup_base; mp_obj_base_t base; mp_obj_t lights; - pbio_iodev_t *iodev; } pupdevices_UltrasonicSensor_obj_t; // pybricks.pupdevices.UltrasonicSensor.__init__ @@ -26,33 +26,27 @@ STATIC mp_obj_t pupdevices_UltrasonicSensor_make_new(const mp_obj_type_t *type, PB_ARG_REQUIRED(port)); pupdevices_UltrasonicSensor_obj_t *self = mp_obj_malloc(pupdevices_UltrasonicSensor_obj_t, type); - - pbio_port_id_t port = pb_type_enum_get_value(port_in, &pb_enum_type_Port); - - // Get iodevices - self->iodev = pb_pup_device_get_device(port, PBIO_IODEV_TYPE_ID_SPIKE_ULTRASONIC_SENSOR); + pb_pupdevices_init_class(&self->pup_base, port_in, PBIO_IODEV_TYPE_ID_SPIKE_ULTRASONIC_SENSOR); // Create an instance of the LightArray class - self->lights = common_LightArray_obj_make_new(self->iodev, PBIO_IODEV_MODE_PUP_ULTRASONIC_SENSOR__LIGHT, 4); + self->lights = common_LightArray_obj_make_new(&self->pup_base, PBIO_IODEV_MODE_PUP_ULTRASONIC_SENSOR__LIGHT, 4); return MP_OBJ_FROM_PTR(self); } // pybricks.pupdevices.UltrasonicSensor.distance -STATIC mp_obj_t pupdevices_UltrasonicSensor_distance(mp_obj_t self_in) { - pupdevices_UltrasonicSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - int16_t *distance = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_ULTRASONIC_SENSOR__DISTL); - return mp_obj_new_int(distance[0] < 0 || distance[0] >= 2000 ? 2000 : *distance); +STATIC mp_obj_t get_distance(mp_obj_t self_in) { + int16_t *data = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_ULTRASONIC_SENSOR__DISTL); + return mp_obj_new_int(data[0] < 0 || data[0] >= 2000 ? 2000 : data[0]); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_UltrasonicSensor_distance_obj, pupdevices_UltrasonicSensor_distance); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_distance_obj, PBIO_IODEV_MODE_PUP_ULTRASONIC_SENSOR__DISTL, get_distance); // pybricks.pupdevices.UltrasonicSensor.presence -STATIC mp_obj_t pupdevices_UltrasonicSensor_presence(mp_obj_t self_in) { - pupdevices_UltrasonicSensor_obj_t *self = MP_OBJ_TO_PTR(self_in); - int8_t *presence = pb_pup_device_get_data(self->iodev, PBIO_IODEV_MODE_PUP_ULTRASONIC_SENSOR__LISTN); - return mp_obj_new_bool(presence[0]); +STATIC mp_obj_t get_presence(mp_obj_t self_in) { + int8_t *data = pb_pupdevices_get_data(self_in, PBIO_IODEV_MODE_PUP_ULTRASONIC_SENSOR__LISTN); + return mp_obj_new_bool(data[0]); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(pupdevices_UltrasonicSensor_presence_obj, pupdevices_UltrasonicSensor_presence); +STATIC PB_DEFINE_CONST_PUPDEVICES_METHOD_OBJ(get_presence_obj, PBIO_IODEV_MODE_PUP_ULTRASONIC_SENSOR__LISTN, get_presence); STATIC const pb_attr_dict_entry_t pupdevices_UltrasonicSensor_attr_dict[] = { PB_DEFINE_CONST_ATTR_RO(MP_QSTR_lights, pupdevices_UltrasonicSensor_obj_t, lights), @@ -61,8 +55,8 @@ STATIC const pb_attr_dict_entry_t pupdevices_UltrasonicSensor_attr_dict[] = { // dir(pybricks.pupdevices.UltrasonicSensor) STATIC const mp_rom_map_elem_t pupdevices_UltrasonicSensor_locals_dict_table[] = { - { MP_ROM_QSTR(MP_QSTR_distance), MP_ROM_PTR(&pupdevices_UltrasonicSensor_distance_obj) }, - { MP_ROM_QSTR(MP_QSTR_presence), MP_ROM_PTR(&pupdevices_UltrasonicSensor_presence_obj) }, + { MP_ROM_QSTR(MP_QSTR_distance), MP_ROM_PTR(&get_distance_obj) }, + { MP_ROM_QSTR(MP_QSTR_presence), MP_ROM_PTR(&get_presence_obj) }, }; STATIC MP_DEFINE_CONST_DICT(pupdevices_UltrasonicSensor_locals_dict, pupdevices_UltrasonicSensor_locals_dict_table); diff --git a/pybricks/util_pb/pb_color_map.c b/pybricks/util_pb/pb_color_map.c index c4b07a7f0..fe3623779 100644 --- a/pybricks/util_pb/pb_color_map.c +++ b/pybricks/util_pb/pb_color_map.c @@ -123,11 +123,16 @@ mp_obj_t pb_color_map_get_color(mp_obj_t *color_map, pbio_color_hsv_t *hsv) { return match; } -// Generic class structure for ColorDistanceSensor -// Any color sensor structure with a color_map -// must have base and color_map as the first two members. +#include + +// HACK: all color sensor structures must have color_map as second item +// REVISIT: Replace with a safer solution to share this method across sensors typedef struct _pb_ColorSensor_obj_t { + #if PYBRICKS_PY_PUPDEVICES + pb_pupdevices_obj_base_t pup_base; + #else mp_obj_base_t base; + #endif mp_obj_t color_map; } pb_ColorSensor_obj_t; From 3cf50b3df9a1a67c11daafbec5cd089370515803 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 26 May 2023 13:41:00 +0200 Subject: [PATCH 57/59] pybricks/tools/awaitable: Refactor cancel_all. This does not need the object itself. Also fix enum names. It will also be used for operations other than cancel, so rename it. --- pybricks/common/pb_type_motor.c | 12 ++++++------ pybricks/common/pb_type_speaker.c | 4 ++-- pybricks/robotics/pb_type_drivebase.c | 6 +++--- pybricks/tools/pb_type_awaitable.c | 16 ++++++---------- pybricks/tools/pb_type_awaitable.h | 8 ++++---- 5 files changed, 21 insertions(+), 25 deletions(-) diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 0da75db37..9244aca0f 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -157,7 +157,7 @@ STATIC mp_obj_t await_or_wait(common_Motor_obj_t *self) { common_Motor_test_completion, pb_type_awaitable_return_none, common_Motor_cancel, - PB_TYPE_AWAITABLE_CANCEL_LINKED); + PB_TYPE_AWAITABLE_OPT_CANCEL_ALL); } // pybricks._common.Motor.angle @@ -184,7 +184,7 @@ STATIC mp_obj_t common_Motor_reset_angle(size_t n_args, const mp_obj_t *pos_args // Set the new angle pb_assert(pbio_servo_reset_angle(self->srv, reset_angle, reset_to_abs)); - pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_update_all(self->awaitables, PB_TYPE_AWAITABLE_OPT_CANCEL_ALL); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_reset_angle_obj, 1, common_Motor_reset_angle); @@ -209,7 +209,7 @@ STATIC mp_obj_t common_Motor_run(size_t n_args, const mp_obj_t *pos_args, mp_map mp_int_t speed = pb_obj_get_int(speed_in); pb_assert(pbio_servo_run_forever(self->srv, speed)); - pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_update_all(self->awaitables, PB_TYPE_AWAITABLE_OPT_CANCEL_ALL); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); @@ -218,7 +218,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_obj, 1, common_Motor_run); STATIC mp_obj_t common_Motor_hold(mp_obj_t self_in) { common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in); pb_assert(pbio_servo_stop(self->srv, PBIO_CONTROL_ON_COMPLETION_HOLD)); - pb_type_awaitable_cancel_all(self_in, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_update_all(self->awaitables, PB_TYPE_AWAITABLE_OPT_CANCEL_ALL); return mp_const_none; } MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_hold_obj, common_Motor_hold); @@ -291,7 +291,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po common_Motor_test_completion, common_Motor_stall_return_value, common_Motor_cancel, - PB_TYPE_AWAITABLE_CANCEL_LINKED); + PB_TYPE_AWAITABLE_OPT_CANCEL_ALL); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_run_until_stalled_obj, 1, common_Motor_run_until_stalled); @@ -353,7 +353,7 @@ STATIC mp_obj_t common_Motor_track_target(size_t n_args, const mp_obj_t *pos_arg mp_int_t target_angle = pb_obj_get_int(target_angle_in); pb_assert(pbio_servo_track_target(self->srv, target_angle)); - pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_update_all(self->awaitables, PB_TYPE_AWAITABLE_OPT_CANCEL_ALL); return mp_const_none; } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(common_Motor_track_target_obj, 1, common_Motor_track_target); diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 8ded97adc..f155c0d4c 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -172,7 +172,7 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp pb_type_Speaker_beep_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, - PB_TYPE_AWAITABLE_CANCEL_LINKED | PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK); + PB_TYPE_AWAITABLE_OPT_CANCEL_ALL | PB_TYPE_AWAITABLE_OPT_CANCEL_HARDWARE); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_beep_obj, 1, pb_type_Speaker_beep); @@ -388,7 +388,7 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar pb_type_Speaker_notes_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, - PB_TYPE_AWAITABLE_CANCEL_LINKED | PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK); + PB_TYPE_AWAITABLE_OPT_CANCEL_ALL | PB_TYPE_AWAITABLE_OPT_CANCEL_HARDWARE); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_play_notes_obj, 1, pb_type_Speaker_play_notes); diff --git a/pybricks/robotics/pb_type_drivebase.c b/pybricks/robotics/pb_type_drivebase.c index ae5de9010..a9375a99b 100644 --- a/pybricks/robotics/pb_type_drivebase.c +++ b/pybricks/robotics/pb_type_drivebase.c @@ -119,7 +119,7 @@ STATIC mp_obj_t await_or_wait(pb_type_DriveBase_obj_t *self) { pb_type_DriveBase_test_completion, pb_type_awaitable_return_none, pb_type_DriveBase_cancel, - PB_TYPE_AWAITABLE_CANCEL_LINKED); + PB_TYPE_AWAITABLE_OPT_CANCEL_ALL); } // pybricks.robotics.DriveBase.straight @@ -203,7 +203,7 @@ STATIC mp_obj_t pb_type_DriveBase_drive(size_t n_args, const mp_obj_t *pos_args, mp_int_t turn_rate = pb_obj_get_int(turn_rate_in); // Cancel awaitables but not hardware. Drive forever will handle this. - pb_type_awaitable_cancel_all(MP_OBJ_FROM_PTR(self), self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_update_all(self->awaitables, PB_TYPE_AWAITABLE_OPT_CANCEL_ALL); pb_assert(pbio_drivebase_drive_forever(self->db, speed, turn_rate)); return mp_const_none; @@ -215,7 +215,7 @@ STATIC mp_obj_t pb_type_DriveBase_stop(mp_obj_t self_in) { // Cancel awaitables. pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in); - pb_type_awaitable_cancel_all(self_in, self->awaitables, PB_TYPE_AWAITABLE_CANCEL_LINKED); + pb_type_awaitable_update_all(self->awaitables, PB_TYPE_AWAITABLE_OPT_CANCEL_ALL); // Stop hardware. pb_type_DriveBase_cancel(self_in); diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 270b31cae..69ecc790b 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -132,16 +132,12 @@ STATIC bool pb_type_awaitable_completed(mp_obj_t self_in, uint32_t start_time) { } /** - * Cancels all awaitables associated with an object. + * Checks and updates all awaitables associated with an object. * - * This is normally used by the function that makes a new awaitable, but it can - * also be called independently to cancel without starting a new awaitable. - * - * @param [in] obj The object whose method we want to wait for completion. * @param [in] awaitables_in List of awaitables associated with @p obj. - * @param [in] options Controls awaitable behavior. + * @param [in] options Controls update behavior. */ -void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_awaitable_opt_t options) { +void pb_type_awaitable_update_all(mp_obj_t awaitables_in, pb_type_awaitable_opt_t options) { // Exit if nothing to do. if (!pb_module_tools_run_loop_is_active() || options == PB_TYPE_AWAITABLE_OPT_NONE) { @@ -155,12 +151,12 @@ void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_ // Only cancel awaitables that are in use. if (awaitable->test_completion) { // Cancel hardware operation if requested and available. - if (options & PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK && awaitable->cancel) { + if (options & PB_TYPE_AWAITABLE_OPT_CANCEL_HARDWARE && awaitable->cancel) { awaitable->cancel(awaitable->obj); } // Set awaitable to done so it gets cancelled it gracefully on the // next iteration. - if (options & PB_TYPE_AWAITABLE_CANCEL_LINKED) { + if (options & PB_TYPE_AWAITABLE_OPT_CANCEL_ALL) { awaitable->test_completion = pb_type_awaitable_completed; } } @@ -199,7 +195,7 @@ mp_obj_t pb_type_awaitable_await_or_wait( } // First cancel linked awaitables if requested. - pb_type_awaitable_cancel_all(obj, awaitables_in, options); + pb_type_awaitable_update_all(awaitables_in, options); // Gets free existing awaitable or creates a new one. pb_type_awaitable_obj_t *awaitable = pb_type_awaitable_get(awaitables_in); diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index 8f0ec90b3..2b6704ab3 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -27,13 +27,13 @@ typedef enum _pb_type_awaitable_opt_t { * running in parallel are using the same resources. This way, the newly * started operation "wins" and everything else is cancelled. */ - PB_TYPE_AWAITABLE_CANCEL_LINKED = 1 << 2, + PB_TYPE_AWAITABLE_OPT_CANCEL_ALL = 1 << 2, /** * On cancelling the linked awaitables, also call their cancel function * to stop hardware. Only used to close hardware resources that aren't * already cleaned up by lower level drivers (so not needed for motors). */ - PB_TYPE_AWAITABLE_CANCEL_LINKED_CALLBACK = 1 << 3, + PB_TYPE_AWAITABLE_OPT_CANCEL_HARDWARE = 1 << 3, } pb_type_awaitable_opt_t; /** @@ -52,7 +52,7 @@ typedef struct _pb_type_awaitable_obj_t pb_type_awaitable_obj_t; * @param [in] start_time The time when the awaitable was created. * @return True if operation is complete, False otherwise. */ -typedef bool (*pb_type_awaitable_test_completion_t)(mp_obj_t objt, uint32_t end_time); +typedef bool (*pb_type_awaitable_test_completion_t)(mp_obj_t obj, uint32_t end_time); /** * Gets the return value of the awaitable. If it always returns None, providing @@ -77,7 +77,7 @@ typedef void (*pb_type_awaitable_cancel_t)(mp_obj_t obj); #define pb_type_awaitable_cancel_none (NULL) -void pb_type_awaitable_cancel_all(mp_obj_t obj, mp_obj_t awaitables_in, pb_type_awaitable_opt_t options); +void pb_type_awaitable_update_all(mp_obj_t awaitables_in, pb_type_awaitable_opt_t options); mp_obj_t pb_type_awaitable_await_or_wait( mp_obj_t obj, From 3afe954592ffae4c9517cfec2e4b74ee4b86c874 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 26 May 2023 14:01:52 +0200 Subject: [PATCH 58/59] pybricks/tools/awaitable: Add raise on busy option. Until we work out clever ways of cancellation or queueing, we can prevent certain resources from being used on more than one task at all. --- pybricks/common/pb_type_speaker.c | 4 +-- pybricks/pupdevices/pb_module_pupdevices.c | 2 +- pybricks/tools/pb_type_awaitable.c | 31 ++++++++++++++-------- pybricks/tools/pb_type_awaitable.h | 5 ++++ 4 files changed, 28 insertions(+), 14 deletions(-) diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index f155c0d4c..25bf877ae 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -172,7 +172,7 @@ STATIC mp_obj_t pb_type_Speaker_beep(size_t n_args, const mp_obj_t *pos_args, mp pb_type_Speaker_beep_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, - PB_TYPE_AWAITABLE_OPT_CANCEL_ALL | PB_TYPE_AWAITABLE_OPT_CANCEL_HARDWARE); + PB_TYPE_AWAITABLE_OPT_RAISE_ON_BUSY); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_beep_obj, 1, pb_type_Speaker_beep); @@ -388,7 +388,7 @@ STATIC mp_obj_t pb_type_Speaker_play_notes(size_t n_args, const mp_obj_t *pos_ar pb_type_Speaker_notes_test_completion, pb_type_awaitable_return_none, pb_type_Speaker_cancel, - PB_TYPE_AWAITABLE_OPT_CANCEL_ALL | PB_TYPE_AWAITABLE_OPT_CANCEL_HARDWARE); + PB_TYPE_AWAITABLE_OPT_RAISE_ON_BUSY); } STATIC MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_Speaker_play_notes_obj, 1, pb_type_Speaker_play_notes); diff --git a/pybricks/pupdevices/pb_module_pupdevices.c b/pybricks/pupdevices/pb_module_pupdevices.c index 25ee782c8..94ea08cbc 100644 --- a/pybricks/pupdevices/pb_module_pupdevices.c +++ b/pybricks/pupdevices/pb_module_pupdevices.c @@ -96,7 +96,7 @@ mp_obj_t pb_pupdevices_set_data(pb_pupdevices_obj_base_t *sensor, uint8_t mode, pb_pup_device_test_completion, pb_type_awaitable_return_none, pb_type_awaitable_cancel_none, - PB_TYPE_AWAITABLE_OPT_NONE); // choose opt raise on busy + PB_TYPE_AWAITABLE_OPT_RAISE_ON_BUSY); } void pb_pupdevices_init_class(pb_pupdevices_obj_base_t *self, mp_obj_t port_in, pbio_iodev_type_id_t valid_id) { diff --git a/pybricks/tools/pb_type_awaitable.c b/pybricks/tools/pb_type_awaitable.c index 69ecc790b..aeb0af715 100644 --- a/pybricks/tools/pb_type_awaitable.c +++ b/pybricks/tools/pb_type_awaitable.c @@ -148,18 +148,27 @@ void pb_type_awaitable_update_all(mp_obj_t awaitables_in, pb_type_awaitable_opt_ for (size_t i = 0; i < awaitables->len; i++) { pb_type_awaitable_obj_t *awaitable = MP_OBJ_TO_PTR(awaitables->items[i]); - // Only cancel awaitables that are in use. - if (awaitable->test_completion) { - // Cancel hardware operation if requested and available. - if (options & PB_TYPE_AWAITABLE_OPT_CANCEL_HARDWARE && awaitable->cancel) { - awaitable->cancel(awaitable->obj); - } - // Set awaitable to done so it gets cancelled it gracefully on the - // next iteration. - if (options & PB_TYPE_AWAITABLE_OPT_CANCEL_ALL) { - awaitable->test_completion = pb_type_awaitable_completed; - } + + // Skip awaitables that are not in use. + if (!awaitable->test_completion) { + continue; + } + + // Raise EBUSY if requested. + if (options & PB_TYPE_AWAITABLE_OPT_RAISE_ON_BUSY) { + mp_raise_msg(&mp_type_OSError, MP_ERROR_TEXT("This resource cannot be used in two tasks at once.")); } + + // Cancel hardware operation if requested and available. + if (options & PB_TYPE_AWAITABLE_OPT_CANCEL_HARDWARE && awaitable->cancel) { + awaitable->cancel(awaitable->obj); + } + // Set awaitable to done so it gets cancelled it gracefully on the + // next iteration. + if (options & PB_TYPE_AWAITABLE_OPT_CANCEL_ALL) { + awaitable->test_completion = pb_type_awaitable_completed; + } + } } diff --git a/pybricks/tools/pb_type_awaitable.h b/pybricks/tools/pb_type_awaitable.h index 2b6704ab3..9dea9898a 100644 --- a/pybricks/tools/pb_type_awaitable.h +++ b/pybricks/tools/pb_type_awaitable.h @@ -34,6 +34,11 @@ typedef enum _pb_type_awaitable_opt_t { * already cleaned up by lower level drivers (so not needed for motors). */ PB_TYPE_AWAITABLE_OPT_CANCEL_HARDWARE = 1 << 3, + /** + * Raises EBUSY if the resource is already in use. Used for resources that + * do not support graceful cancellation. + */ + PB_TYPE_AWAITABLE_OPT_RAISE_ON_BUSY = 1 << 4, } pb_type_awaitable_opt_t; /** From 4b939b002fa86fef479f68214215211388925207 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 26 May 2023 15:17:24 +0200 Subject: [PATCH 59/59] pybricks.common.Speaker: Fix awaitable. The initialization code ran only on boot, which could cause it to get in a bad state the second time. --- pybricks/common/pb_type_speaker.c | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/pybricks/common/pb_type_speaker.c b/pybricks/common/pb_type_speaker.c index 25bf877ae..6d9961cc1 100644 --- a/pybricks/common/pb_type_speaker.c +++ b/pybricks/common/pb_type_speaker.c @@ -27,7 +27,6 @@ typedef struct { mp_obj_base_t base; - bool initialized; // State of awaitable sound mp_obj_t notes_generator; @@ -44,8 +43,6 @@ typedef struct { uint16_t sample_attenuator; } pb_type_Speaker_obj_t; -STATIC pb_type_Speaker_obj_t pb_type_Speaker_singleton; - STATIC uint16_t waveform_data[128]; STATIC mp_obj_t pb_type_Speaker_volume(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { @@ -111,15 +108,12 @@ STATIC void pb_type_Speaker_stop_beep(void) { } STATIC mp_obj_t pb_type_Speaker_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { - pb_type_Speaker_obj_t *self = &pb_type_Speaker_singleton; - if (!self->initialized) { - self->base.type = &pb_type_Speaker; - self->initialized = true; - - // List of awaitables associated with speaker. By keeping track, - // we can cancel them as needed when a new sound is started. - self->awaitables = mp_obj_new_list(0, NULL); - } + + pb_type_Speaker_obj_t *self = mp_obj_malloc(pb_type_Speaker_obj_t, type); + + // List of awaitables associated with speaker. By keeping track, + // we can cancel them as needed when a new sound is started. + self->awaitables = mp_obj_new_list(0, NULL); // REVISIT: If a user creates two Speaker instances, this will reset the volume settings for both. // If done only once per singleton, however, altered volume settings would be persisted between program runs.