mirrors
/
pimoroni-pico
mirror of https://github.com/pimoroni/pimoroni-pico.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ServoCluster states are now only created for the servos actually assigned

This commit is contained in:
ZodiusInfuser 2022-03-02 17:36:00 +00:00
parent 8fc1270404
commit ec1f0a5ddc
5 changed files with 114 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
drivers/pwm/pwm_cluster.cpp
View File

@ -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);
}

146
drivers/servo/servo_cluster.cpp
View File

@ -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<uint8_t> 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) {

6
drivers/servo/servo_cluster.hpp
View File

@ -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;

3
drivers/servo/servo_state.hpp
View File

@ -37,13 +37,10 @@ namespace servo {
public:
ServoState(CalibrationType default_type = ANGULAR);
//--------------------------------------------------
// Methods
//--------------------------------------------------
public:
bool init();
float enable();
float disable();
bool is_enabled() const;

71
micropython/modules/servo/servo.cpp
View File

@ -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