From 0facfaa540c81203503aa7183e6e7de1b020f85c Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 11 Apr 2022 17:59:16 +0100 Subject: [PATCH] Updated MP bindings to newest C++ --- micropython/modules/motor/motor.c | 51 ++- micropython/modules/motor/motor.cpp | 678 ++++++++++++++++++++++++++-- micropython/modules/motor/motor.h | 25 +- 3 files changed, 692 insertions(+), 62 deletions(-) diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index 2a0acc40..d3ed6fa6 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -11,12 +11,13 @@ MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_obj, 1, Motor_speed); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_frequency_obj, 1, Motor_frequency); MP_DEFINE_CONST_FUN_OBJ_1(Motor_stop_obj, Motor_stop); MP_DEFINE_CONST_FUN_OBJ_1(Motor_coast_obj, Motor_coast); +MP_DEFINE_CONST_FUN_OBJ_1(Motor_brake_obj, Motor_brake); MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_negative_obj, Motor_full_negative); MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_positive_obj, Motor_full_positive); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_to_percent_obj, 2, Motor_to_percent); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_direction_obj, 1, Motor_direction); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_scale_obj, 1, Motor_speed_scale); -MP_DEFINE_CONST_FUN_OBJ_KW(Motor_deadzone_percent_obj, 1, Motor_deadzone_percent); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_deadzone_obj, 1, Motor_deadzone); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_decay_mode_obj, 1, Motor_decay_mode); MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster___del___obj, MotorCluster___del__); @@ -38,21 +39,23 @@ MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_stop_obj, 2, MotorCluster_stop); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_stop_all_obj, 1, MotorCluster_stop_all); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_coast_obj, 2, MotorCluster_coast); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_coast_all_obj, 1, MotorCluster_coast_all); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_brake_obj, 2, MotorCluster_brake); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_brake_all_obj, 1, MotorCluster_brake_all); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_full_negative_obj, 2, MotorCluster_full_negative); -MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_full_negative_obj, 1, MotorCluster_all_to_full_negative); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_full_negative_obj, 1, MotorCluster_all_full_negative); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_full_positive_obj, 2, MotorCluster_full_positive); -MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_full_positive_obj, 1, MotorCluster_all_to_full_positive); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_full_positive_obj, 1, MotorCluster_all_full_positive); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_to_percent_obj, 3, MotorCluster_to_percent); MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_percent_obj, 2, MotorCluster_all_to_percent); MP_DEFINE_CONST_FUN_OBJ_1(MotorCluster_load_obj, MotorCluster_load); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_direction_obj, 2, MotorCluster_direction); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_directions_obj, 1, MotorCluster_all_directions); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_speed_scale_obj, 2, MotorCluster_speed_scale); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_speed_scales_obj, 1, MotorCluster_all_speed_scales); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_deadzone_percent_obj, 2, MotorCluster_deadzone_percent); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_deadzone_percents_obj, 1, MotorCluster_all_deadzone_percents); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_decay_mode_obj, 2, MotorCluster_decay_mode); -//MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_decay_modes_obj, 1, MotorCluster_all_decay_modes); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_direction_obj, 2, MotorCluster_direction); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_direction_obj, 1, MotorCluster_all_to_direction); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_speed_scale_obj, 2, MotorCluster_speed_scale); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_speed_scale_obj, 1, MotorCluster_all_to_speed_scale); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_deadzone_obj, 2, MotorCluster_deadzone); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_deadzone_obj, 1, MotorCluster_all_to_deadzone); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_decay_mode_obj, 2, MotorCluster_decay_mode); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_to_decay_mode_obj, 1, MotorCluster_all_to_decay_mode); /***** Binding of Methods *****/ STATIC const mp_rom_map_elem_t Motor_locals_dict_table[] = { @@ -66,12 +69,13 @@ STATIC const mp_rom_map_elem_t Motor_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Motor_frequency_obj) }, { MP_ROM_QSTR(MP_QSTR_stop), MP_ROM_PTR(&Motor_stop_obj) }, { MP_ROM_QSTR(MP_QSTR_coast), MP_ROM_PTR(&Motor_coast_obj) }, + { MP_ROM_QSTR(MP_QSTR_brake), MP_ROM_PTR(&Motor_brake_obj) }, { MP_ROM_QSTR(MP_QSTR_full_negative), MP_ROM_PTR(&Motor_full_negative_obj) }, { MP_ROM_QSTR(MP_QSTR_full_positive), MP_ROM_PTR(&Motor_full_positive_obj) }, { MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&Motor_to_percent_obj) }, { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&Motor_direction_obj) }, { MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&Motor_speed_scale_obj) }, - { MP_ROM_QSTR(MP_QSTR_deadzone_percent), MP_ROM_PTR(&Motor_deadzone_percent_obj) }, + { MP_ROM_QSTR(MP_QSTR_deadzone), MP_ROM_PTR(&Motor_deadzone_obj) }, { MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&Motor_decay_mode_obj) }, }; @@ -95,21 +99,23 @@ STATIC const mp_rom_map_elem_t MotorCluster_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_stop_all), MP_ROM_PTR(&MotorCluster_stop_all_obj) }, { MP_ROM_QSTR(MP_QSTR_coast), MP_ROM_PTR(&MotorCluster_coast_obj) }, { MP_ROM_QSTR(MP_QSTR_coast_all), MP_ROM_PTR(&MotorCluster_coast_all_obj) }, + { MP_ROM_QSTR(MP_QSTR_brake), MP_ROM_PTR(&MotorCluster_brake_obj) }, + { MP_ROM_QSTR(MP_QSTR_brake_all), MP_ROM_PTR(&MotorCluster_brake_all_obj) }, { MP_ROM_QSTR(MP_QSTR_full_negative), MP_ROM_PTR(&MotorCluster_full_negative_obj) }, - { MP_ROM_QSTR(MP_QSTR_all_to_full_negative), MP_ROM_PTR(&MotorCluster_all_to_full_negative_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_full_negative), MP_ROM_PTR(&MotorCluster_all_full_negative_obj) }, { MP_ROM_QSTR(MP_QSTR_full_positive), MP_ROM_PTR(&MotorCluster_full_positive_obj) }, - { MP_ROM_QSTR(MP_QSTR_all_to_full_positive), MP_ROM_PTR(&MotorCluster_all_to_full_positive_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_full_positive), MP_ROM_PTR(&MotorCluster_all_full_positive_obj) }, { MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&MotorCluster_to_percent_obj) }, { MP_ROM_QSTR(MP_QSTR_all_to_percent), MP_ROM_PTR(&MotorCluster_all_to_percent_obj) }, { MP_ROM_QSTR(MP_QSTR_load), MP_ROM_PTR(&MotorCluster_load_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_all_directions), MP_ROM_PTR(&MotorCluster_direction_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&MotorCluster_speed_scale_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_all_speed_scales), MP_ROM_PTR(&MotorCluster_all_speed_scales_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_deadzone_percent), MP_ROM_PTR(&MotorCluster_deadzone_percent_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_all_deadzone_percents), MP_ROM_PTR(&MotorCluster_all_deadzone_percents_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&MotorCluster_decay_mode_obj) }, - //{ MP_ROM_QSTR(MP_QSTR_all_decay_modes), MP_ROM_PTR(&MotorCluster_all_decay_modes_obj) }, + { MP_ROM_QSTR(MP_QSTR_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, + { MP_ROM_QSTR(MP_QSTR_speed_scale), MP_ROM_PTR(&MotorCluster_speed_scale_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_speed_scale), MP_ROM_PTR(&MotorCluster_all_to_speed_scale_obj) }, + { MP_ROM_QSTR(MP_QSTR_deadzone), MP_ROM_PTR(&MotorCluster_deadzone_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_deadzone), MP_ROM_PTR(&MotorCluster_all_to_deadzone_obj) }, + { MP_ROM_QSTR(MP_QSTR_decay_mode), MP_ROM_PTR(&MotorCluster_decay_mode_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_to_decay_mode), MP_ROM_PTR(&MotorCluster_all_to_decay_mode_obj) }, }; STATIC MP_DEFINE_CONST_DICT(Motor_locals_dict, Motor_locals_dict_table); @@ -170,6 +176,7 @@ typedef struct _mp_obj_float_t { mp_obj_base_t base; mp_float_t value; } mp_obj_float_t; +//TODO confirm below numbers are correct mp_obj_float_t motor2040_shunt_resistor = {{&mp_type_float}, 0.47f}; mp_obj_float_t motor2040_voltage_gain = {{&mp_type_float}, 3.9f / 13.9f}; mp_obj_float_t motor2040_current_offset = {{&mp_type_float}, -0.02f}; diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 1351f793..507b99ed 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -11,6 +11,7 @@ using namespace motor; extern "C" { #include "motor.h" #include "py/builtin.h" +#include "float.h" /********** Motor **********/ @@ -41,7 +42,7 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed()), PRINT_REPR); mp_print_str(print, ", freq = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->frequency()), PRINT_REPR); - if(self->motor->direction() == MotorState::NORMAL) + if(self->motor->direction() == NORMAL) mp_print_str(print, ", direction = NORMAL"); else mp_print_str(print, ", direction = REVERSED"); @@ -49,7 +50,7 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed_scale()), PRINT_REPR); mp_print_str(print, ", deadzone = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone()), PRINT_REPR); - if(self->motor->decay_mode() == MotorState::SLOW_DECAY) + if(self->motor->decay_mode() == SLOW_DECAY) mp_print_str(print, ", decay_mode = SLOW_DECAY"); else mp_print_str(print, ", decay_mode = FAST_DECAY"); @@ -68,6 +69,7 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c { MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, }; + //TODO ^ // Parse args. mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; @@ -302,6 +304,12 @@ extern mp_obj_t Motor_coast(mp_obj_t self_in) { return mp_const_none; } +extern mp_obj_t Motor_brake(mp_obj_t self_in) { + _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); + self->motor->brake(); + return mp_const_none; +} + extern mp_obj_t Motor_full_negative(mp_obj_t self_in) { _Motor_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Motor_obj_t); self->motor->full_negative(); @@ -401,7 +409,7 @@ extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_ enum { ARG_self, ARG_direction }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, }; // Parse args. @@ -410,9 +418,11 @@ extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_ _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - int direction = mp_obj_get_int(args[ARG_direction].u_obj); - - self->motor->direction((MotorState::Direction)direction); + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + self->motor->direction((Direction)direction); return mp_const_none; } } @@ -446,13 +456,15 @@ extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_ma _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); - + if(speed_scale < FLT_EPSILON) { + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } self->motor->speed_scale(speed_scale); return mp_const_none; } } -extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { if(n_args <= 1) { enum { ARG_self }; static const mp_arg_t allowed_args[] = { @@ -481,7 +493,9 @@ extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); float deadzone = mp_obj_get_float(args[ARG_deadzone_percent].u_obj); - + if(deadzone < 0.0f || deadzone > 1.0f) { + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } self->motor->deadzone(deadzone); return mp_const_none; } @@ -506,7 +520,7 @@ extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map enum { ARG_self, ARG_decay_mode }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_decay_mode, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_decay_mode, MP_ARG_REQUIRED | MP_ARG_INT }, }; // Parse args. @@ -515,9 +529,11 @@ extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - int decay_mode = mp_obj_get_int(args[ARG_decay_mode].u_obj); - - self->motor->decay_mode((MotorState::DecayMode)decay_mode); + int mode = args[ARG_decay_mode].u_int; + if(mode < 0 || mode > 1) { + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } + self->motor->decay_mode((DecayMode)mode); return mp_const_none; } } @@ -584,6 +600,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_auto_phase, MP_ARG_BOOL, {.u_bool = true} }, }; + //TODO ^ // Parse args. mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; @@ -700,7 +717,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t //if(calib != nullptr) //cluster = new MotorCluster(pio, sm, pins, pair_count, *calib, freq, auto_phase, seq_buffer, dat_buffer); //else - cluster = new MotorCluster(pio, sm, pins, pair_count, MotorState::NORMAL, 1.0f, 0.0f, freq, MotorState::SLOW_DECAY, auto_phase, seq_buffer, dat_buffer); + cluster = new MotorCluster(pio, sm, pins, pair_count, NORMAL, 1.0f, 0.0f, freq, SLOW_DECAY, auto_phase, seq_buffer, dat_buffer); // Cleanup the pins array if(pins != nullptr) @@ -756,8 +773,14 @@ extern mp_obj_t MotorCluster_pins(size_t n_args, const mp_obj_t *pos_args, mp_ma mp_raise_ValueError("this cluster does not have any motors"); else if(motor < 0 || motor >= motor_count) mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); - else - return mp_obj_new_int(self->cluster->pins((uint)motor).positive); + else { + pin_pair pins = self->cluster->pins((uint)motor); + + mp_obj_t tuple[2]; + tuple[0] = mp_obj_new_int(pins.positive); + tuple[1] = mp_obj_new_int(pins.negative); + return mp_obj_new_tuple(2, tuple); + } return mp_const_none; } @@ -875,7 +898,7 @@ extern mp_obj_t MotorCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp } else { size_t length = 0; - mp_obj_t *items = nullptr; + mp_obj_t *items = nullptr; if(mp_obj_is_type(object, &mp_type_list)) { mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); length = list->len; @@ -997,7 +1020,7 @@ extern mp_obj_t MotorCluster_duty(size_t n_args, const mp_obj_t *pos_args, mp_ma if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1115,7 +1138,7 @@ extern mp_obj_t MotorCluster_speed(size_t n_args, const mp_obj_t *pos_args, mp_m if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1233,7 +1256,7 @@ extern mp_obj_t MotorCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_m if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1364,7 +1387,7 @@ extern mp_obj_t MotorCluster_stop(size_t n_args, const mp_obj_t *pos_args, mp_ma if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to stop const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1452,7 +1475,7 @@ extern mp_obj_t MotorCluster_coast(size_t n_args, const mp_obj_t *pos_args, mp_m if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to coast const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1522,6 +1545,94 @@ extern mp_obj_t MotorCluster_coast_all(size_t n_args, const mp_obj_t *pos_args, return mp_const_none; } +extern mp_obj_t MotorCluster_brake(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_motors, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to brake + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + self->cluster->brake((uint)motor, args[ARG_load].u_bool); + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + self->cluster->brake(motors, length, args[ARG_load].u_bool); + delete[] motors; + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_brake_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + self->cluster->brake_all(args[ARG_load].u_bool); + } + return mp_const_none; +} + extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_motors, ARG_load }; static const mp_arg_t allowed_args[] = { @@ -1540,7 +1651,7 @@ extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_ar if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to set to full negative const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1588,7 +1699,7 @@ extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_ar return mp_const_none; } -extern mp_obj_t MotorCluster_all_to_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t MotorCluster_all_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_load }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, @@ -1628,7 +1739,7 @@ extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_ar if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to set to full positive const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1676,7 +1787,7 @@ extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_ar return mp_const_none; } -extern mp_obj_t MotorCluster_all_to_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +extern mp_obj_t MotorCluster_all_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { enum { ARG_self, ARG_load }; static const mp_arg_t allowed_args[] = { { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, @@ -1718,7 +1829,7 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1788,7 +1899,7 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -1864,7 +1975,7 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - // Determine what motor(s) to enable + // Determine what motor(s) to modify const mp_obj_t object = args[ARG_motors].u_obj; if(mp_obj_is_int(object)) { int motor = mp_obj_get_int(object); @@ -2011,4 +2122,513 @@ extern mp_obj_t MotorCluster_load(mp_obj_t self_in) { self->cluster->load(); return mp_const_none; } + +extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_int((int)self->cluster->direction((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_direction }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to modify + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + self->cluster->direction((uint)motor, (Direction)direction); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + delete[] motors; + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + self->cluster->direction(motors, length, (Direction)direction); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_direction }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_direction, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + int direction = args[ARG_direction].u_int; + if(direction < 0 || direction > 1) { + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + } + self->cluster->all_to_direction((Direction)direction); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->speed_scale((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_speed_scale }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_scale, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to modify + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + if(speed_scale < FLT_EPSILON) { + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } + self->cluster->speed_scale((uint)motor, speed_scale); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + if(speed_scale < FLT_EPSILON) { + delete[] motors; + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } + + self->cluster->speed_scale(motors, length, speed_scale); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_speed_scale }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_speed_scale, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float speed_scale = mp_obj_get_float(args[ARG_speed_scale].u_obj); + if(speed_scale < FLT_EPSILON) { + mp_raise_ValueError("speed_scale out of range. Expected greater than 0.0"); + } + self->cluster->all_to_speed_scale(speed_scale); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_float(self->cluster->deadzone((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_deadzone, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_deadzone, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to modify + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); + if(deadzone < 0.0f || deadzone > 1.0f) { + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } + self->cluster->deadzone((uint)motor, deadzone, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); + if(deadzone < 0.0f || deadzone > 1.0f) { + delete[] motors; + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } + self->cluster->deadzone(motors, length, deadzone, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { +enum { ARG_self, ARG_deadzone, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_deadzone, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + float deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); + if(deadzone < 0.0f || deadzone > 1.0f) { + mp_raise_ValueError("deadzone out of range. Expected 0.0 to 1.0"); + } + self->cluster->all_to_deadzone(deadzone, args[ARG_load].u_bool); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 2) { + enum { ARG_self, ARG_motor }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motor, MP_ARG_REQUIRED | MP_ARG_INT }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor = args[ARG_motor].u_int; + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else + return mp_obj_new_int((int)self->cluster->decay_mode((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_mode, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_motors, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_mode, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + // Determine what motor(s) to modify + const mp_obj_t object = args[ARG_motors].u_obj; + if(mp_obj_is_int(object)) { + int motor = mp_obj_get_int(object); + if(motor < 0 || motor >= motor_count) + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("motor out of range. Expected 0 to %d"), motor_count - 1); + else { + int mode = args[ARG_mode].u_int; + if(mode < 0 || mode > 1) { + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } + self->cluster->decay_mode((uint)motor, (DecayMode)mode, args[ARG_load].u_bool); + } + } + else { + size_t length = 0; + mp_obj_t *items = nullptr; + if(mp_obj_is_type(object, &mp_type_list)) { + mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t); + length = list->len; + items = list->items; + } + else if(mp_obj_is_type(object, &mp_type_tuple)) { + mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t); + length = tuple->len; + items = tuple->items; + } + + if(items == nullptr) + mp_raise_TypeError("cannot convert object to a list or tuple of integers, or a single integer"); + else if(length == 0) + mp_raise_TypeError("list or tuple must contain at least one integer"); + else { + // Create and populate a local array of motor indices + uint8_t *motors = new uint8_t[length]; + for(size_t i = 0; i < length; i++) { + int motor = mp_obj_get_int(items[i]); + if(motor < 0 || motor >= motor_count) { + delete[] motors; + mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a motor in the list or tuple is out of range. Expected 0 to %d"), motor_count - 1); + } + else { + motors[i] = (uint8_t)motor; + } + } + int mode = args[ARG_mode].u_int; + if(mode < 0 || mode > 1) { + delete[] motors; + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } + self->cluster->decay_mode(motors, length, (DecayMode)mode, args[ARG_load].u_bool); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_to_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_mode, ARG_load }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_mode, MP_ARG_REQUIRED | MP_ARG_INT }, + { MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }}, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _MotorCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _MotorCluster_obj_t); + + int motor_count = (int)self->cluster->count(); + if(motor_count == 0) + mp_raise_ValueError("this cluster does not have any motors"); + else { + int mode = args[ARG_mode].u_int; + if(mode < 0 || mode > 1) { + mp_raise_ValueError("mode out of range. Expected FAST_DECAY (0) or SLOW_DECAY (1)"); + } + self->cluster->all_to_decay_mode((DecayMode)mode, args[ARG_load].u_bool); + } + return mp_const_none; +} } diff --git a/micropython/modules/motor/motor.h b/micropython/modules/motor/motor.h index 45dd0987..1dff6978 100644 --- a/micropython/modules/motor/motor.h +++ b/micropython/modules/motor/motor.h @@ -18,12 +18,13 @@ extern mp_obj_t Motor_speed(size_t n_args, const mp_obj_t *pos_args, mp_map_t *k extern mp_obj_t Motor_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_stop(mp_obj_t self_in); extern mp_obj_t Motor_coast(mp_obj_t self_in); +extern mp_obj_t Motor_brake(mp_obj_t self_in); extern mp_obj_t Motor_full_negative(mp_obj_t self_in); extern mp_obj_t Motor_full_positive(mp_obj_t self_in); extern mp_obj_t Motor_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind); @@ -47,18 +48,20 @@ extern mp_obj_t MotorCluster_stop(size_t n_args, const mp_obj_t *pos_args, mp_ma extern mp_obj_t MotorCluster_stop_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_coast(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_coast_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_brake(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_brake_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t MotorCluster_all_to_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_full_negative(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -extern mp_obj_t MotorCluster_all_to_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_full_positive(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_all_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t MotorCluster_load(mp_obj_t self_in); -//extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_all_directions(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_all_speed_scales(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_all_deadzone_percents(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); -//extern mp_obj_t MotorCluster_all_decay_modes(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_to_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);