From ec1f0a5ddcd4971ce3bc958eae40f6b877d302c2 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Wed, 2 Mar 2022 17:36:00 +0000 Subject: [PATCH] ServoCluster states are now only created for the servos actually assigned --- drivers/pwm/pwm_cluster.cpp | 2 +- drivers/servo/servo_cluster.cpp | 146 +++++++++++++++------------- drivers/servo/servo_cluster.hpp | 6 +- drivers/servo/servo_state.hpp | 3 - micropython/modules/servo/servo.cpp | 71 +++++++------- 5 files changed, 114 insertions(+), 114 deletions(-) diff --git a/drivers/pwm/pwm_cluster.cpp b/drivers/pwm/pwm_cluster.cpp index 7a44f976..9607c10f 100644 --- a/drivers/pwm/pwm_cluster.cpp +++ b/drivers/pwm/pwm_cluster.cpp @@ -95,7 +95,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count) : pio(pi wrap_level = 0; // Initialise all the channels this PWM will control - uint pin_end = MIN(pin_count, NUM_BANK0_GPIOS); + uint pin_end = MIN(pin_count + pin_base, NUM_BANK0_GPIOS); for(uint channel = pin_base; channel < pin_end; channel++) { pin_mask |= (1u << channel); } diff --git a/drivers/servo/servo_cluster.cpp b/drivers/servo/servo_cluster.cpp index f64c9bc8..3b2950ae 100644 --- a/drivers/servo/servo_cluster.cpp +++ b/drivers/servo/servo_cluster.cpp @@ -5,17 +5,42 @@ namespace servo { ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask) : pwms(pio, sm, pin_mask) { + + for(uint i = 0; i < NUM_BANK0_GPIOS; i++) { + if(pimoroni::PWMCluster::bit_in_mask(i, pin_mask)) { + servos[i] = new ServoState(); + } + } } ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count) : pwms(pio, sm, pin_base, pin_count) { + uint pin_mask = pwms.get_pin_mask(); + + for(uint i = 0; i < NUM_BANK0_GPIOS; i++) { + if(pimoroni::PWMCluster::bit_in_mask(i, pin_mask)) { + servos[i] = new ServoState(); + } + } } ServoCluster::ServoCluster(PIO pio, uint sm, std::initializer_list pins) : pwms(pio, sm, pins) { + uint pin_mask = pwms.get_pin_mask(); + + for(uint i = 0; i < NUM_BANK0_GPIOS; i++) { + if(pimoroni::PWMCluster::bit_in_mask(i, pin_mask)) { + servos[i] = new ServoState(); + } + } } ServoCluster::~ServoCluster() { + for(uint i = 0; i < NUM_BANK0_GPIOS; i++) { + if(servos[i] != nullptr) { + delete servos[i]; + } + } } bool ServoCluster::init() { @@ -52,53 +77,47 @@ namespace servo { return pwms.get_pin_mask(); } + bool ServoCluster::is_assigned(uint servo) const { + return (servo < NUM_BANK0_GPIOS) && (servos[servo] != nullptr); + } + void ServoCluster::enable(uint servo, bool load) { - if(servo < NUM_BANK0_GPIOS) { - float new_pulse = servos[servo].enable(); - apply_pulse(servo, new_pulse, load); - } + assert(is_assigned(servo)); + float new_pulse = servos[servo]->enable(); + apply_pulse(servo, new_pulse, load); } void ServoCluster::disable(uint servo, bool load) { - if(servo < NUM_BANK0_GPIOS) { - float new_pulse = servos[servo].disable(); - apply_pulse(servo, new_pulse, load); - } + assert(is_assigned(servo)); + float new_pulse = servos[servo]->disable(); + apply_pulse(servo, new_pulse, load); } bool ServoCluster::is_enabled(uint servo) const { - if(servo < NUM_BANK0_GPIOS) - return servos[servo].is_enabled(); - else - return false; + assert(is_assigned(servo)); + return servos[servo]->is_enabled(); } float ServoCluster::get_value(uint servo) const { - if(servo < NUM_BANK0_GPIOS) - return servos[servo].get_value(); - else - return 0.0f; + assert(is_assigned(servo)); + return servos[servo]->get_value(); } void ServoCluster::set_value(uint servo, float value, bool load) { - if(servo < NUM_BANK0_GPIOS) { - float new_pulse = servos[servo].set_value(value); - apply_pulse(servo, new_pulse, load); - } + assert(is_assigned(servo)); + float new_pulse = servos[servo]->set_value(value); + apply_pulse(servo, new_pulse, load); } float ServoCluster::get_pulse(uint servo) const { - if(servo < NUM_BANK0_GPIOS) - return servos[servo].get_pulse(); - else - return 0.0f; + assert(is_assigned(servo)); + return servos[servo]->get_pulse(); } void ServoCluster::set_pulse(uint servo, float pulse, bool load) { - if(servo < NUM_BANK0_GPIOS) { - float new_pulse = servos[servo].set_pulse(pulse); - apply_pulse(servo, new_pulse, load); - } + assert(is_assigned(servo)); + float new_pulse = servos[servo]->set_pulse(pulse); + apply_pulse(servo, new_pulse, load); } float ServoCluster::get_frequency() const { @@ -118,8 +137,10 @@ namespace servo { // Update the pwm before setting the new wrap for(uint servo = 0; servo < NUM_BANK0_GPIOS; servo++) { - float current_pulse = servos[servo].get_pulse(); - apply_pulse(servo, current_pulse, false); + if(servos[servo] != nullptr) { + float current_pulse = servos[servo]->get_pulse(); + apply_pulse(servo, current_pulse, false); + } } // Set the new wrap (should be 1 less than the period to get full 0 to 100%) @@ -138,73 +159,58 @@ namespace servo { } float ServoCluster::get_min_value(uint servo) const { - if(servo < NUM_BANK0_GPIOS) - return servos[servo].get_min_value(); - else - return 0.0f; + assert(is_assigned(servo)); + return servos[servo]->get_min_value(); } float ServoCluster::get_mid_value(uint servo) const { - if(servo < NUM_BANK0_GPIOS) - return servos[servo].get_mid_value(); - else - return 0.0f; + assert(is_assigned(servo)); + return servos[servo]->get_mid_value(); } float ServoCluster::get_max_value(uint servo) const { - if(servo < NUM_BANK0_GPIOS) - return servos[servo].get_max_value(); - else - return 0.0f; + assert(is_assigned(servo)); + return servos[servo]->get_max_value(); } void ServoCluster::to_min(uint servo, bool load) { - if(servo < NUM_BANK0_GPIOS) { - float new_pulse = servos[servo].to_min(); - apply_pulse(servo, new_pulse, load); - } + assert(is_assigned(servo)); + float new_pulse = servos[servo]->to_min(); + apply_pulse(servo, new_pulse, load); } void ServoCluster::to_mid(uint servo, bool load) { - if(servo < NUM_BANK0_GPIOS) { - float new_pulse = servos[servo].to_mid(); - apply_pulse(servo, new_pulse, load); - } + assert(is_assigned(servo)); + float new_pulse = servos[servo]->to_mid(); + apply_pulse(servo, new_pulse, load); } void ServoCluster::to_max(uint servo, bool load) { - if(servo < NUM_BANK0_GPIOS) { - float new_pulse = servos[servo].to_max(); - apply_pulse(servo, new_pulse, load); - } + assert(is_assigned(servo)); + float new_pulse = servos[servo]->to_max(); + apply_pulse(servo, new_pulse, load); } void ServoCluster::to_percent(uint servo, float in, float in_min, float in_max, bool load) { - if(servo < NUM_BANK0_GPIOS) { - float new_pulse = servos[servo].to_percent(in, in_min, in_max); - apply_pulse(servo, new_pulse, load); - } + assert(is_assigned(servo)); + float new_pulse = servos[servo]->to_percent(in, in_min, in_max); + apply_pulse(servo, new_pulse, load); } void ServoCluster::to_percent(uint servo, float in, float in_min, float in_max, float value_min, float value_max, bool load) { - if(servo < NUM_BANK0_GPIOS) { - float new_pulse = servos[servo].to_percent(in, in_min, in_max, value_min, value_max); - apply_pulse(servo, new_pulse, load); - } + assert(is_assigned(servo)); + float new_pulse = servos[servo]->to_percent(in, in_min, in_max, value_min, value_max); + apply_pulse(servo, new_pulse, load); } Calibration* ServoCluster::calibration(uint servo) { - if(servo < NUM_BANK0_GPIOS) - return &servos[servo].calibration(); - else - return nullptr; + assert(is_assigned(servo)); + return &servos[servo]->calibration(); } const Calibration* ServoCluster::calibration(uint servo) const { - if(servo < NUM_BANK0_GPIOS) - return &servos[servo].calibration(); - else - return nullptr; + assert(is_assigned(servo)); + return &servos[servo]->calibration(); } void ServoCluster::apply_pulse(uint servo, float pulse, bool load) { diff --git a/drivers/servo/servo_cluster.hpp b/drivers/servo/servo_cluster.hpp index fec72eb0..09070e17 100644 --- a/drivers/servo/servo_cluster.hpp +++ b/drivers/servo/servo_cluster.hpp @@ -14,9 +14,7 @@ namespace servo { pimoroni::PWMCluster pwms; uint32_t pwm_period; float pwm_frequency = ServoState::DEFAULT_FREQUENCY; - ServoState servos[NUM_BANK0_GPIOS]; // TODO change this to array of pointers - // so that only the servos actually assigned - // to this cluster have states + ServoState* servos[NUM_BANK0_GPIOS]; //-------------------------------------------------- @@ -37,6 +35,8 @@ namespace servo { // For print access in micropython uint get_pin_mask() const; + bool is_assigned(uint servo) const; + void enable(uint servo, bool load = true); void disable(uint servo, bool load = true); bool is_enabled(uint servo) const; diff --git a/drivers/servo/servo_state.hpp b/drivers/servo/servo_state.hpp index d30f849e..8d0f5514 100644 --- a/drivers/servo/servo_state.hpp +++ b/drivers/servo/servo_state.hpp @@ -37,13 +37,10 @@ namespace servo { public: ServoState(CalibrationType default_type = ANGULAR); - //-------------------------------------------------- // Methods //-------------------------------------------------- public: - bool init(); - float enable(); float disable(); bool is_enabled() const; diff --git a/micropython/modules/servo/servo.cpp b/micropython/modules/servo/servo.cpp index 68916d5d..486eaf0c 100644 --- a/micropython/modules/servo/servo.cpp +++ b/micropython/modules/servo/servo.cpp @@ -4,9 +4,6 @@ #define MP_OBJ_TO_PTR2(o, t) ((t *)(uintptr_t)(o)) -#define IS_SERVO_INVALID(servo) (((servo) < 0) || ((servo) >= (int)NUM_BANK0_GPIOS)) - - using namespace servo; extern "C" { @@ -927,8 +924,8 @@ extern mp_obj_t ServoCluster_enable(size_t n_args, const mp_obj_t *pos_args, mp_ _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else self->cluster->enable((uint)servo); return mp_const_none; @@ -948,8 +945,8 @@ extern mp_obj_t ServoCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else self->cluster->disable((uint)servo); return mp_const_none; @@ -969,8 +966,8 @@ extern mp_obj_t ServoCluster_is_enabled(size_t n_args, const mp_obj_t *pos_args, _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else return self->cluster->is_enabled((uint)servo) ? mp_const_true : mp_const_false; } @@ -990,8 +987,8 @@ extern mp_obj_t ServoCluster_value(size_t n_args, const mp_obj_t *pos_args, mp_m _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else return mp_obj_new_float(self->cluster->get_value((uint)servo)); } @@ -1010,8 +1007,8 @@ extern mp_obj_t ServoCluster_value(size_t n_args, const mp_obj_t *pos_args, mp_m _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else { float value = mp_obj_get_float(args[ARG_value].u_obj); self->cluster->set_value((uint)servo, value); @@ -1035,8 +1032,8 @@ extern mp_obj_t ServoCluster_pulse(size_t n_args, const mp_obj_t *pos_args, mp_m _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else return mp_obj_new_float(self->cluster->get_pulse((uint)servo)); } @@ -1055,8 +1052,8 @@ extern mp_obj_t ServoCluster_pulse(size_t n_args, const mp_obj_t *pos_args, mp_m _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else { float pulse = mp_obj_get_float(args[ARG_pulse].u_obj); self->cluster->set_pulse((uint)servo, pulse); @@ -1116,8 +1113,8 @@ extern mp_obj_t ServoCluster_min_value(size_t n_args, const mp_obj_t *pos_args, _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else return mp_obj_new_float(self->cluster->get_min_value((uint)servo)); } @@ -1136,8 +1133,8 @@ extern mp_obj_t ServoCluster_mid_value(size_t n_args, const mp_obj_t *pos_args, _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else return mp_obj_new_float(self->cluster->get_mid_value((uint)servo)); } @@ -1156,8 +1153,8 @@ extern mp_obj_t ServoCluster_max_value(size_t n_args, const mp_obj_t *pos_args, _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else return mp_obj_new_float(self->cluster->get_max_value((uint)servo)); } @@ -1176,8 +1173,8 @@ extern mp_obj_t ServoCluster_to_min(size_t n_args, const mp_obj_t *pos_args, mp_ _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else self->cluster->to_min((uint)servo); @@ -1198,8 +1195,8 @@ extern mp_obj_t ServoCluster_to_mid(size_t n_args, const mp_obj_t *pos_args, mp_ _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else self->cluster->to_mid((uint)servo); @@ -1220,8 +1217,8 @@ extern mp_obj_t ServoCluster_to_max(size_t n_args, const mp_obj_t *pos_args, mp_ _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else self->cluster->to_max((uint)servo); @@ -1244,8 +1241,8 @@ extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else { float in = mp_obj_get_float(args[ARG_in].u_obj); self->cluster->to_percent((uint)servo, in); @@ -1268,8 +1265,8 @@ extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else { float in = mp_obj_get_float(args[ARG_in].u_obj); float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); @@ -1296,8 +1293,8 @@ extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else { float in = mp_obj_get_float(args[ARG_in].u_obj); float in_min = mp_obj_get_float(args[ARG_in_min].u_obj); @@ -1324,8 +1321,8 @@ extern mp_obj_t ServoCluster_calibration(size_t n_args, const mp_obj_t *pos_args _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t); int servo = args[ARG_servo].u_int; - if(IS_SERVO_INVALID(servo)) - mp_raise_ValueError("servo out of range. Expected 0 to 29"); + if(!self->cluster->is_assigned((uint)servo)) + mp_raise_ValueError("servo not assigned to this cluster"); else { // NOTE This seems to work, in that it give MP access to the calibration object // Could very easily mess up in weird ways once object deletion is considered