diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index 7fc5e29a..2900fe73 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -544,7 +544,7 @@ namespace motor { } } - void MotorCluster::all_to_direction(Direction direction) { + void MotorCluster::all_directions(Direction direction) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->direction(motor, direction); @@ -574,7 +574,7 @@ namespace motor { } } - void MotorCluster::all_to_speed_scale(float speed_scale) { + void MotorCluster::all_speed_scales(float speed_scale) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->speed_scale(motor, speed_scale); @@ -604,7 +604,7 @@ namespace motor { } } - void MotorCluster::all_to_zeropoint(float zeropoint, bool load) { + void MotorCluster::all_zeropoints(float zeropoint, bool load) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->zeropoint(motor, zeropoint); @@ -635,7 +635,7 @@ namespace motor { } } - void MotorCluster::all_to_deadzone(float deadzone, bool load) { + void MotorCluster::all_deadzones(float deadzone, bool load) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->deadzone(motor, deadzone); @@ -666,7 +666,7 @@ namespace motor { } } - void MotorCluster::all_to_decay_mode(DecayMode mode, bool load) { + void MotorCluster::all_decay_modes(DecayMode mode, bool load) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->decay_mode(motor, mode); diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index 785d9c15..c1147692 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -132,36 +132,37 @@ namespace motor { void direction(uint8_t motor, Direction direction); void direction(const uint8_t *motors, uint8_t length, Direction direction); void direction(std::initializer_list motors, Direction direction); - void all_to_direction(Direction direction); + void all_directions(Direction direction); float speed_scale(uint8_t motor) const; void speed_scale(uint8_t motor, float speed_scale); void speed_scale(const uint8_t *motors, uint8_t length, float speed_scale); void speed_scale(std::initializer_list motors, float speed_scale); - void all_to_speed_scale(float speed_scale); + void all_speed_scales(float speed_scale); float zeropoint(uint8_t motor) const; void zeropoint(uint8_t motor, float zeropoint, bool load = true); void zeropoint(const uint8_t *motors, uint8_t length, float zeropoint, bool load = true); void zeropoint(std::initializer_list motors, float zeropoint, bool load = true); - void all_to_zeropoint(float zeropoint, bool load = true); + void all_zeropoints(float zeropoint, bool load = true); float deadzone(uint8_t motor) const; void deadzone(uint8_t motor, float deadzone, bool load = true); void deadzone(const uint8_t *motors, uint8_t length, float deadzone, bool load = true); void deadzone(std::initializer_list motors, float deadzone, bool load = true); - void all_to_deadzone(float deadzone, bool load = true); + void all_deadzones(float deadzone, bool load = true); DecayMode decay_mode(uint8_t motor) const; void decay_mode(uint8_t motor, DecayMode mode, bool load = true); void decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode, bool load = true); void decay_mode(std::initializer_list motors, DecayMode mode, bool load = true); - void all_to_decay_mode(DecayMode mode, bool load = true); + void all_decay_modes(DecayMode mode, bool load = true); //-------------------------------------------------- private: void apply_duty(uint8_t motor, float duty, DecayMode mode, bool load); - void create_motor_states(Direction direction, float speed_scale, float zeropoint, float deadzone, DecayMode mode, bool auto_phase); + void create_motor_states(Direction direction, float speed_scale, float zeropoint, + float deadzone, DecayMode mode, bool auto_phase); }; } \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 4eeca27e..6d0ad992 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -5,13 +5,15 @@ namespace motor { MotorState::MotorState() - : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false), - motor_direction(NORMAL_DIR), motor_scale(DEFAULT_SPEED_SCALE), motor_zeropoint(DEFAULT_ZEROPOINT), motor_deadzone(DEFAULT_DEADZONE){ + : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false) + , motor_direction(NORMAL_DIR), motor_scale(DEFAULT_SPEED_SCALE) + , motor_zeropoint(DEFAULT_ZEROPOINT), motor_deadzone(DEFAULT_DEADZONE) { } MotorState::MotorState(Direction direction, float speed_scale, float zeropoint, float deadzone) - : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false), - motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_zeropoint(CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON)), motor_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) { + : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false) + , motor_direction(direction) , motor_scale(MAX(speed_scale, FLT_EPSILON)) + , motor_zeropoint(CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON)), motor_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) { } float MotorState::enable_with_return() { @@ -48,14 +50,8 @@ namespace motor { // Clamp the duty between the hard limits last_enabled_duty = CLAMP(duty, -1.0f, 1.0f); - if(last_enabled_duty > motor_zeropoint) - motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale); - else if(last_enabled_duty < -motor_zeropoint) - motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale); - else - motor_speed = 0.0f; - - //motor_speed = last_enabled_duty * motor_scale; + // Calculate the corresponding speed + motor_speed = duty_to_speed(last_enabled_duty, motor_zeropoint, motor_scale); return enable_with_return(); } @@ -72,13 +68,8 @@ namespace motor { // Clamp the speed between the hard limits motor_speed = CLAMP(speed, 0.0f - motor_scale, motor_scale); - if(motor_speed > 0.0f) - last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f); - else if(motor_speed < 0.0f) - last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f); - else - last_enabled_duty = 0.0f; - //last_enabled_duty = motor_speed / motor_scale; + // Calculate the corresponding duty cycle + last_enabled_duty = speed_to_duty(motor_speed, motor_zeropoint, motor_scale); return enable_with_return(); } @@ -119,7 +110,7 @@ namespace motor { void MotorState::set_speed_scale(float speed_scale) { motor_scale = MAX(speed_scale, FLT_EPSILON); - motor_speed = last_enabled_duty * motor_scale; + motor_speed = duty_to_speed(last_enabled_duty, motor_zeropoint, motor_scale); } float MotorState::get_zeropoint() const { @@ -128,13 +119,7 @@ namespace motor { void MotorState::set_zeropoint(float zeropoint) { motor_zeropoint = CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON); - - if(last_enabled_duty > motor_zeropoint) - motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale); - else if(last_enabled_duty < -motor_zeropoint) - motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale); - else - motor_speed = 0.0f; + motor_speed = duty_to_speed(last_enabled_duty, motor_zeropoint, motor_scale); } float MotorState::get_deadzone() const { @@ -154,4 +139,26 @@ namespace motor { return (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min; } + float MotorState::duty_to_speed(float duty, float zeropoint, float scale) { + float speed = 0.0f; + if(duty > zeropoint) { + speed = map_float(duty, zeropoint, 1.0f, 0.0f, scale); + } + else if(duty < -zeropoint) { + speed = map_float(duty, -zeropoint, -1.0f, 0.0f, -scale); + } + return speed; + } + + float MotorState::speed_to_duty(float speed, float zeropoint, float scale) { + float duty = 0.0f; + if(speed > 0.0f) { + duty = map_float(speed, 0.0f, scale, zeropoint, 1.0f); + } + else if(speed < 0.0f) { + duty = map_float(speed, 0.0f, -scale, -zeropoint, -1.0f); + } + return duty; + } + }; \ No newline at end of file diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index e182fe77..896c8041 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -18,7 +18,7 @@ namespace motor { //-------------------------------------------------- public: static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor speed scale - static constexpr float DEFAULT_ZEROPOINT = 0.0f; // The standard motor deadzone + static constexpr float DEFAULT_ZEROPOINT = 0.0f; // The standard motor zeropoint static constexpr float DEFAULT_DEADZONE = 0.05f; // The standard motor deadzone static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; // The standard motor decay behaviour @@ -94,6 +94,9 @@ namespace motor { static int32_t duty_to_level(float duty, uint32_t resolution); static float map_float(float in, float in_min, float in_max, float out_min, float out_max); + private: + static float duty_to_speed(float duty, float zeropoint, float scale); + static float speed_to_duty(float speed, float zeropoint, float scale); }; } \ No newline at end of file diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index d7881712..f3d6ee53 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -50,13 +50,15 @@ MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_to_percent_obj, 3, MotorCluster_to_perce 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_to_direction_obj, 1, MotorCluster_all_to_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_to_speed_scale_obj, 1, MotorCluster_all_to_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_zeropoint_obj, 2, MotorCluster_zeropoint); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_zeropoints_obj, 1, MotorCluster_all_zeropoints); 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_all_deadzones_obj, 1, MotorCluster_all_deadzones); 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); +MP_DEFINE_CONST_FUN_OBJ_KW(MotorCluster_all_decay_modes_obj, 1, MotorCluster_all_decay_modes); /***** Binding of Methods *****/ STATIC const mp_rom_map_elem_t Motor_locals_dict_table[] = { @@ -110,13 +112,15 @@ STATIC const mp_rom_map_elem_t MotorCluster_locals_dict_table[] = { { 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_to_direction), MP_ROM_PTR(&MotorCluster_direction_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_directions), MP_ROM_PTR(&MotorCluster_all_directions_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_all_speed_scales), MP_ROM_PTR(&MotorCluster_all_speed_scales_obj) }, + { MP_ROM_QSTR(MP_QSTR_zeropoint), MP_ROM_PTR(&MotorCluster_zeropoint_obj) }, + { MP_ROM_QSTR(MP_QSTR_all_zeropoints), MP_ROM_PTR(&MotorCluster_all_zeropoints_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_all_deadzones), MP_ROM_PTR(&MotorCluster_all_deadzones_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) }, + { MP_ROM_QSTR(MP_QSTR_all_decay_modes), MP_ROM_PTR(&MotorCluster_all_decay_modes_obj) }, }; STATIC MP_DEFINE_CONST_DICT(Motor_locals_dict, Motor_locals_dict_table); @@ -255,8 +259,6 @@ STATIC const mp_map_elem_t motor_globals_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR_pico_motor_shim), (mp_obj_t)&pico_motor_shim_user_cmodule }, { MP_OBJ_NEW_QSTR(MP_QSTR_motor2040), (mp_obj_t)&motor2040_user_cmodule }, - { MP_ROM_QSTR(MP_QSTR_NORMAL), MP_ROM_INT(0x00) }, - { MP_ROM_QSTR(MP_QSTR_REVERSED), MP_ROM_INT(0x01) }, { MP_ROM_QSTR(MP_QSTR_FAST_DECAY), MP_ROM_INT(0x00) }, { MP_ROM_QSTR(MP_QSTR_SLOW_DECAY), MP_ROM_INT(0x01) }, }; diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 8f2db642..dad3fbba 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -2301,7 +2301,7 @@ extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, 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) { +extern mp_obj_t MotorCluster_all_directions(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 }, @@ -2322,7 +2322,7 @@ extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos if(direction < 0 || direction > 1) { mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } - self->cluster->all_to_direction((Direction)direction); + self->cluster->all_directions((Direction)direction); } return mp_const_none; } @@ -2428,7 +2428,7 @@ extern mp_obj_t MotorCluster_speed_scale(size_t n_args, const mp_obj_t *pos_args 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) { +extern mp_obj_t MotorCluster_all_speed_scales(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 }, @@ -2449,7 +2449,134 @@ extern mp_obj_t MotorCluster_all_to_speed_scale(size_t n_args, const mp_obj_t *p 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); + self->cluster->all_speed_scales(speed_scale); + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_zeropoint(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->zeropoint((uint)motor)); + } + else { + enum { ARG_self, ARG_motors, ARG_zeropoint }; + 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_zeropoint, 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 zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + self->cluster->zeropoint((uint)motor, zeropoint); + } + } + 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 zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + delete[] motors; + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + + self->cluster->zeropoint(motors, length, zeropoint); + delete[] motors; + } + } + } + } + return mp_const_none; +} + +extern mp_obj_t MotorCluster_all_zeropoints(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + enum { ARG_self, ARG_zeropoint }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_zeropoint, 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 zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + self->cluster->all_zeropoints(zeropoint); } return mp_const_none; } @@ -2555,7 +2682,7 @@ extern mp_obj_t MotorCluster_deadzone(size_t n_args, const mp_obj_t *pos_args, m 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) { +extern mp_obj_t MotorCluster_all_deadzones(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 }, @@ -2577,7 +2704,7 @@ enum { ARG_self, ARG_deadzone, ARG_load }; 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); + self->cluster->all_deadzones(deadzone, args[ARG_load].u_bool); } return mp_const_none; } @@ -2683,7 +2810,7 @@ extern mp_obj_t MotorCluster_decay_mode(size_t n_args, const mp_obj_t *pos_args, 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) { +extern mp_obj_t MotorCluster_all_decay_modes(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 }, @@ -2705,7 +2832,7 @@ extern mp_obj_t MotorCluster_all_to_decay_mode(size_t n_args, const mp_obj_t *po 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); + self->cluster->all_decay_modes((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 7ce746a4..d9763f69 100644 --- a/micropython/modules/motor/motor.h +++ b/micropython/modules/motor/motor.h @@ -59,10 +59,12 @@ extern mp_obj_t MotorCluster_to_percent(size_t n_args, const mp_obj_t *pos_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_to_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_to_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_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t MotorCluster_all_zeropoints(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_all_deadzones(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); +extern mp_obj_t MotorCluster_all_decay_modes(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);