mirror
/
pimoroni-pico
kopia lustrzana https://github.com/pimoroni/pimoroni-pico
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Expanded list/tuple support to more functions

servo-pio
ZodiusInfuser 2022-03-10 20:34:59 +00:00
rodzic 2ef84ae286
commit f83521d3ff
5 zmienionych plików z 992 dodań i 61 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

208
drivers/servo/servo_cluster.cpp
Wyświetl plik

@ -148,6 +148,32 @@ namespace servo {
apply_pulse(servo, new_pulse, load);
}
void ServoCluster::set_pulse(const uint8_t *servos, uint8_t length, float pulse, bool load) {
assert(servos != nullptr);
for(uint8_t i = 0; i < length; i++) {
set_pulse(servos[i], pulse, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::set_pulse(std::initializer_list<uint8_t> servos, float pulse, bool load) {
for(auto servo : servos) {
set_pulse(servo, pulse, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::set_all_pulses(float pulse, bool load) {
uint8_t servo_count = pwms.get_chan_count();
for(uint8_t servo = 0; servo < servo_count; servo++) {
set_pulse(servo, pulse, false);
}
if(load)
pwms.load_pwm();
}
float ServoCluster::get_value(uint servo) const {
assert(servo < pwms.get_chan_count());
return states[servo].get_value();
@ -159,6 +185,32 @@ namespace servo {
apply_pulse(servo, new_pulse, load);
}
void ServoCluster::set_value(const uint8_t *servos, uint8_t length, float value, bool load) {
assert(servos != nullptr);
for(uint8_t i = 0; i < length; i++) {
set_value(servos[i], value, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::set_value(std::initializer_list<uint8_t> servos, float value, bool load) {
for(auto servo : servos) {
set_value(servo, value, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::set_all_values(float value, bool load) {
uint8_t servo_count = pwms.get_chan_count();
for(uint8_t servo = 0; servo < servo_count; servo++) {
set_value(servo, value, false);
}
if(load)
pwms.load_pwm();
}
float ServoCluster::get_phase(uint servo) const {
assert(servo < pwms.get_chan_count());
return servo_phases[servo];
@ -170,6 +222,32 @@ namespace servo {
pwms.set_chan_offset(servo, (uint32_t)(servo_phases[servo] * (float)pwms.get_wrap()), load);
}
void ServoCluster::set_phase(const uint8_t *servos, uint8_t length, float phase, bool load) {
assert(servos != nullptr);
for(uint8_t i = 0; i < length; i++) {
set_phase(servos[i], phase, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::set_phase(std::initializer_list<uint8_t> servos, float phase, bool load) {
for(auto servo : servos) {
set_phase(servo, phase, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::set_all_phases(float phase, bool load) {
uint8_t servo_count = pwms.get_chan_count();
for(uint8_t servo = 0; servo < servo_count; servo++) {
set_phase(servo, phase, false);
}
if(load)
pwms.load_pwm();
}
float ServoCluster::get_frequency() const {
return pwm_frequency;
}
@ -229,30 +307,160 @@ namespace servo {
apply_pulse(servo, new_pulse, load);
}
void ServoCluster::to_min(const uint8_t *servos, uint8_t length, bool load) {
assert(servos != nullptr);
for(uint8_t i = 0; i < length; i++) {
to_min(servos[i], false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::to_min(std::initializer_list<uint8_t> servos, bool load) {
for(auto servo : servos) {
to_min(servo, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::all_to_min(bool load) {
uint8_t servo_count = pwms.get_chan_count();
for(uint8_t servo = 0; servo < servo_count; servo++) {
to_min(servo, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::to_mid(uint servo, bool load) {
assert(is_assigned(servo));
float new_pulse = states[servo].to_mid();
apply_pulse(servo, new_pulse, load);
}
void ServoCluster::to_mid(const uint8_t *servos, uint8_t length, bool load) {
assert(servos != nullptr);
for(uint8_t i = 0; i < length; i++) {
to_mid(servos[i], false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::to_mid(std::initializer_list<uint8_t> servos, bool load) {
for(auto servo : servos) {
to_mid(servo, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::all_to_mid(bool load) {
uint8_t servo_count = pwms.get_chan_count();
for(uint8_t servo = 0; servo < servo_count; servo++) {
to_mid(servo, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::to_max(uint servo, bool load) {
assert(is_assigned(servo));
float new_pulse = states[servo].to_max();
apply_pulse(servo, new_pulse, load);
}
void ServoCluster::to_max(const uint8_t *servos, uint8_t length, bool load) {
assert(servos != nullptr);
for(uint8_t i = 0; i < length; i++) {
to_max(servos[i], false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::to_max(std::initializer_list<uint8_t> servos, bool load) {
for(auto servo : servos) {
to_max(servo, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::all_to_max(bool load) {
uint8_t servo_count = pwms.get_chan_count();
for(uint8_t servo = 0; servo < servo_count; servo++) {
to_max(servo, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::to_percent(uint servo, float in, float in_min, float in_max, bool load) {
assert(is_assigned(servo));
float new_pulse = states[servo].to_percent(in, in_min, in_max);
apply_pulse(servo, new_pulse, load);
}
void ServoCluster::to_percent(const uint8_t *servos, uint8_t length, float in, float in_min, float in_max, bool load) {
assert(servos != nullptr);
for(uint8_t i = 0; i < length; i++) {
to_percent(servos[i], in, in_min, in_max, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::to_percent(std::initializer_list<uint8_t> servos, float in, float in_min, float in_max, bool load) {
for(auto servo : servos) {
to_percent(servo, in, in_min, in_max, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::all_to_percent(float in, float in_min, float in_max, bool load) {
uint8_t servo_count = pwms.get_chan_count();
for(uint8_t servo = 0; servo < servo_count; servo++) {
to_percent(servo, in, in_min, in_max, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::to_percent(uint servo, float in, float in_min, float in_max, float value_min, float value_max, bool load) {
assert(is_assigned(servo));
float new_pulse = states[servo].to_percent(in, in_min, in_max, value_min, value_max);
apply_pulse(servo, new_pulse, load);
}
void ServoCluster::to_percent(const uint8_t *servos, uint8_t length, float in, float in_min, float in_max, float value_min, float value_max, bool load) {
assert(servos != nullptr);
for(uint8_t i = 0; i < length; i++) {
to_percent(servos[i], in, in_min, in_max, value_min, value_max, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::to_percent(std::initializer_list<uint8_t> servos, float in, float in_min, float in_max, float value_min, float value_max, bool load) {
for(auto servo : servos) {
to_percent(servo, in, in_min, in_max, value_min, value_max, false);
}
if(load)
pwms.load_pwm();
}
void ServoCluster::all_to_percent(float in, float in_min, float in_max, float value_min, float value_max, bool load) {
uint8_t servo_count = pwms.get_chan_count();
for(uint8_t servo = 0; servo < servo_count; servo++) {
to_percent(servo, in, in_min, in_max, value_min, value_max, false);
}
if(load)
pwms.load_pwm();
}
Calibration& ServoCluster::calibration(uint servo) {
assert(is_assigned(servo));
return states[servo].calibration();

29
drivers/servo/servo_cluster.hpp
Wyświetl plik

@ -47,16 +47,26 @@ namespace servo {
void disable(const uint8_t *servos, uint8_t length, bool load = true);
void disable(std::initializer_list<uint8_t> servos, bool load = true);
void disable_all(bool load = true);
bool is_enabled(uint servo) const;
float get_pulse(uint servo) const;
void set_pulse(uint servo, float pulse, bool load = true);
void set_pulse(const uint8_t *servos, uint8_t length, float pulse, bool load = true);
void set_pulse(std::initializer_list<uint8_t> servos, float pulse, bool load = true);
void set_all_pulses(float pulse, bool load = true);
float get_value(uint servo) const;
void set_value(uint servo, float value, bool load = true);
void set_value(const uint8_t *servos, uint8_t length, float value, bool load = true);
void set_value(std::initializer_list<uint8_t> servos, float value, bool load = true);
void set_all_values(float value, bool load = true);
float get_phase(uint servo) const;
void set_phase(uint servo, float phase, bool load = true);
void set_phase(const uint8_t *servos, uint8_t length, float phase, bool load = true);
void set_phase(std::initializer_list<uint8_t> servos, float phase, bool load = true);
void set_all_phases(float phase, bool load = true);
float get_frequency() const;
bool set_frequency(float freq);
@ -67,10 +77,29 @@ namespace servo {
float get_max_value(uint servo) const;
void to_min(uint servo, bool load = true);
void to_min(const uint8_t *servos, uint8_t length, bool load = true);
void to_min(std::initializer_list<uint8_t> servos, bool load = true);
void all_to_min(bool load = true);
void to_mid(uint servo, bool load = true);
void to_mid(const uint8_t *servos, uint8_t length, bool load = true);
void to_mid(std::initializer_list<uint8_t> servos, bool load = true);
void all_to_mid(bool load = true);
void to_max(uint servo, bool load = true);
void to_max(const uint8_t *servos, uint8_t length, bool load = true);
void to_max(std::initializer_list<uint8_t> servos, bool load = true);
void all_to_max(bool load = true);
void to_percent(uint servo, float in, float in_min = ServoState::ZERO_PERCENT, float in_max = ServoState::ONEHUNDRED_PERCENT, bool load = true);
void to_percent(const uint8_t *servos, uint8_t length, float in, float in_min = ServoState::ZERO_PERCENT, float in_max = ServoState::ONEHUNDRED_PERCENT, bool load = true);
void to_percent(std::initializer_list<uint8_t> servos, float in, float in_min = ServoState::ZERO_PERCENT, float in_max = ServoState::ONEHUNDRED_PERCENT, bool load = true);
void all_to_percent(float in, float in_min = ServoState::ZERO_PERCENT, float in_max = ServoState::ONEHUNDRED_PERCENT, bool load = true);
void to_percent(uint servo, float in, float in_min, float in_max, float value_min, float value_max, bool load = true);
void to_percent(const uint8_t *servos, uint8_t length, float in, float in_min, float in_max, float value_min, float value_max, bool load = true);
void to_percent(std::initializer_list<uint8_t> servos, float in, float in_min, float in_max, float value_min, float value_max, bool load = true);
void all_to_percent(float in, float in_min, float in_max, float value_min, float value_max, bool load = true);
Calibration& calibration(uint servo);
const Calibration& calibration(uint servo) const;

14
micropython/modules/servo/servo.c
Wyświetl plik

@ -44,16 +44,23 @@ MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_disable_obj, 2, ServoCluster_disable);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_disable_all_obj, 1, ServoCluster_disable_all);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_is_enabled_obj, 2, ServoCluster_is_enabled);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_value_obj, 2, ServoCluster_value);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_all_values_obj, 1, ServoCluster_all_values);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_pulse_obj, 2, ServoCluster_pulse);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_all_pulses_obj, 1, ServoCluster_all_pulses);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_phase_obj, 2, ServoCluster_phase);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_all_phases_obj, 1, ServoCluster_all_phases);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_frequency_obj, 1, ServoCluster_frequency);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_min_value_obj, 2, ServoCluster_min_value);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_mid_value_obj, 2, ServoCluster_mid_value);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_max_value_obj, 2, ServoCluster_max_value);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_to_min_obj, 2, ServoCluster_to_min);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_all_to_min_obj, 1, ServoCluster_all_to_min);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_to_mid_obj, 2, ServoCluster_to_mid);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_all_to_mid_obj, 1, ServoCluster_all_to_mid);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_to_max_obj, 2, ServoCluster_to_max);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_all_to_max_obj, 1, ServoCluster_all_to_max);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_to_percent_obj, 3, ServoCluster_to_percent);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_all_to_percent_obj, 2, ServoCluster_all_to_percent);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_calibration_obj, 2, ServoCluster_calibration);
MP_DEFINE_CONST_FUN_OBJ_1(ServoCluster_load_obj, ServoCluster_load);
@ -105,16 +112,23 @@ STATIC const mp_rom_map_elem_t ServoCluster_locals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR_disable_all), MP_ROM_PTR(&ServoCluster_disable_all_obj) },
{ MP_ROM_QSTR(MP_QSTR_is_enabled), MP_ROM_PTR(&ServoCluster_is_enabled_obj) },
{ MP_ROM_QSTR(MP_QSTR_pulse), MP_ROM_PTR(&ServoCluster_pulse_obj) },
{ MP_ROM_QSTR(MP_QSTR_all_pulses), MP_ROM_PTR(&ServoCluster_all_pulses_obj) },
{ MP_ROM_QSTR(MP_QSTR_value), MP_ROM_PTR(&ServoCluster_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_all_values), MP_ROM_PTR(&ServoCluster_all_values_obj) },
{ MP_ROM_QSTR(MP_QSTR_phase), MP_ROM_PTR(&ServoCluster_phase_obj) },
{ MP_ROM_QSTR(MP_QSTR_all_phases), MP_ROM_PTR(&ServoCluster_all_phases_obj) },
{ MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&ServoCluster_frequency_obj) },
{ MP_ROM_QSTR(MP_QSTR_min_value), MP_ROM_PTR(&ServoCluster_min_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_mid_value), MP_ROM_PTR(&ServoCluster_mid_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_max_value), MP_ROM_PTR(&ServoCluster_max_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_to_min), MP_ROM_PTR(&ServoCluster_to_min_obj) },
{ MP_ROM_QSTR(MP_QSTR_all_to_min), MP_ROM_PTR(&ServoCluster_all_to_min_obj) },
{ MP_ROM_QSTR(MP_QSTR_to_mid), MP_ROM_PTR(&ServoCluster_to_mid_obj) },
{ MP_ROM_QSTR(MP_QSTR_all_to_mid), MP_ROM_PTR(&ServoCluster_all_to_mid_obj) },
{ MP_ROM_QSTR(MP_QSTR_to_max), MP_ROM_PTR(&ServoCluster_to_max_obj) },
{ MP_ROM_QSTR(MP_QSTR_all_to_max), MP_ROM_PTR(&ServoCluster_all_to_max_obj) },
{ MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&ServoCluster_to_percent_obj) },
{ MP_ROM_QSTR(MP_QSTR_all_to_percent), MP_ROM_PTR(&ServoCluster_all_to_percent_obj) },
{ MP_ROM_QSTR(MP_QSTR_calibration), MP_ROM_PTR(&ServoCluster_calibration_obj) },
{ MP_ROM_QSTR(MP_QSTR_load), MP_ROM_PTR(&ServoCluster_load_obj) },
};

795
micropython/modules/servo/servo.cpp
Wyświetl plik

@ -1264,10 +1264,104 @@ extern mp_obj_t ServoCluster_pulse(size_t n_args, const mp_obj_t *pos_args, mp_m
return mp_obj_new_float(self->cluster->get_pulse((uint)servo));
}
else {
enum { ARG_self, ARG_servo, ARG_pulse, ARG_load };
enum { ARG_self, ARG_servos, ARG_pulse, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servos, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_pulse, 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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
// Determine what servo(s) to enable
const mp_obj_t object = args[ARG_servos].u_obj;
if(mp_obj_is_int(object)) {
int servo = mp_obj_get_int(object);
if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else {
float pulse = mp_obj_get_float(args[ARG_pulse].u_obj);
self->cluster->set_pulse((uint)servo, pulse, 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 servo indices
uint8_t *servos = new uint8_t[length];
for(size_t i = 0; i < length; i++) {
int servo = mp_obj_get_int(items[i]);
if(servo < 0 || servo >= servo_count) {
delete[] servos;
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a servo in the list or tuple is out of range. Expected 0 to %d"), servo_count - 1);
}
else {
servos[i] = (uint8_t)servo;
}
}
float pulse = mp_obj_get_float(args[ARG_pulse].u_obj);
self->cluster->set_pulse(servos, length, pulse, args[ARG_load].u_bool);
delete[] servos;
}
}
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_all_pulses(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[] = {
{ MP_QSTR_, 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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
uint servo_count = self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(mp_obj_new_tuple(servo_count, NULL), mp_obj_tuple_t);
for(uint servo = 0; servo < servo_count; servo++) {
tuple->items[servo] = mp_obj_new_float(self->cluster->get_pulse(servo));
}
return MP_OBJ_FROM_PTR(tuple);
}
}
else {
enum { ARG_self, ARG_pulse, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_pulse, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
};
@ -1278,15 +1372,12 @@ 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;
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else {
float pulse = mp_obj_get_float(args[ARG_pulse].u_obj);
self->cluster->set_pulse((uint)servo, pulse, args[ARG_load].u_bool);
self->cluster->set_all_pulses(pulse, args[ARG_load].u_bool);
}
}
return mp_const_none;
@ -1316,10 +1407,104 @@ extern mp_obj_t ServoCluster_value(size_t n_args, const mp_obj_t *pos_args, mp_m
return mp_obj_new_float(self->cluster->get_value((uint)servo));
}
else {
enum { ARG_self, ARG_servo, ARG_value, ARG_load };
enum { ARG_self, ARG_servos, ARG_value, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servos, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_value, 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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
// Determine what servo(s) to enable
const mp_obj_t object = args[ARG_servos].u_obj;
if(mp_obj_is_int(object)) {
int servo = mp_obj_get_int(object);
if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else {
float value = mp_obj_get_float(args[ARG_value].u_obj);
self->cluster->set_value((uint)servo, value, 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 servo indices
uint8_t *servos = new uint8_t[length];
for(size_t i = 0; i < length; i++) {
int servo = mp_obj_get_int(items[i]);
if(servo < 0 || servo >= servo_count) {
delete[] servos;
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a servo in the list or tuple is out of range. Expected 0 to %d"), servo_count - 1);
}
else {
servos[i] = (uint8_t)servo;
}
}
float value = mp_obj_get_float(args[ARG_value].u_obj);
self->cluster->set_value(servos, length, value, args[ARG_load].u_bool);
delete[] servos;
}
}
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_all_values(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 1) {
enum { ARG_self, ARG_servo };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, 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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
uint servo_count = self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(mp_obj_new_tuple(servo_count, NULL), mp_obj_tuple_t);
for(uint servo = 0; servo < servo_count; servo++) {
tuple->items[servo] = mp_obj_new_float(self->cluster->get_value(servo));
}
return MP_OBJ_FROM_PTR(tuple);
}
}
else {
enum { ARG_self, ARG_value, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_value, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
};
@ -1330,15 +1515,12 @@ 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;
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else {
float value = mp_obj_get_float(args[ARG_value].u_obj);
self->cluster->set_value((uint)servo, value, args[ARG_load].u_bool);
self->cluster->set_all_values(value, args[ARG_load].u_bool);
}
}
return mp_const_none;
@ -1368,10 +1550,104 @@ extern mp_obj_t ServoCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_m
return mp_obj_new_float(self->cluster->get_phase((uint)servo));
}
else {
enum { ARG_self, ARG_servo, ARG_phase, ARG_load };
enum { ARG_self, ARG_servos, ARG_phase, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servos, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_phase, 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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
// Determine what servo(s) to enable
const mp_obj_t object = args[ARG_servos].u_obj;
if(mp_obj_is_int(object)) {
int servo = mp_obj_get_int(object);
if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else {
float phase = mp_obj_get_float(args[ARG_phase].u_obj);
self->cluster->set_phase((uint)servo, phase, 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 servo indices
uint8_t *servos = new uint8_t[length];
for(size_t i = 0; i < length; i++) {
int servo = mp_obj_get_int(items[i]);
if(servo < 0 || servo >= servo_count) {
delete[] servos;
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a servo in the list or tuple is out of range. Expected 0 to %d"), servo_count - 1);
}
else {
servos[i] = (uint8_t)servo;
}
}
float phase = mp_obj_get_float(args[ARG_phase].u_obj);
self->cluster->set_phase(servos, length, phase, args[ARG_load].u_bool);
delete[] servos;
}
}
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_all_phases(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[] = {
{ MP_QSTR_, 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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
uint servo_count = self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(mp_obj_new_tuple(servo_count, NULL), mp_obj_tuple_t);
for(uint servo = 0; servo < servo_count; servo++) {
tuple->items[servo] = mp_obj_new_float(self->cluster->get_phase(servo));
}
return MP_OBJ_FROM_PTR(tuple);
}
}
else {
enum { ARG_self, ARG_phase, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_phase, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
};
@ -1382,15 +1658,12 @@ extern mp_obj_t ServoCluster_phase(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;
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else {
float phase = mp_obj_get_float(args[ARG_phase].u_obj);
self->cluster->set_phase((uint)servo, phase, args[ARG_load].u_bool);
self->cluster->set_all_phases(phase, args[ARG_load].u_bool);
}
}
return mp_const_none;
@ -1509,10 +1782,10 @@ extern mp_obj_t ServoCluster_max_value(size_t n_args, const mp_obj_t *pos_args,
}
extern mp_obj_t ServoCluster_to_min(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_servo, ARG_load };
enum { ARG_self, ARG_servos, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_servos, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
};
@ -1522,23 +1795,85 @@ 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;
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else
self->cluster->to_min((uint)servo, args[ARG_load].u_bool);
else {
// Determine what servo(s) to enable
const mp_obj_t object = args[ARG_servos].u_obj;
if(mp_obj_is_int(object)) {
int servo = mp_obj_get_int(object);
if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else
self->cluster->to_min((uint)servo, 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 servo indices
uint8_t *servos = new uint8_t[length];
for(size_t i = 0; i < length; i++) {
int servo = mp_obj_get_int(items[i]);
if(servo < 0 || servo >= servo_count) {
delete[] servos;
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a servo in the list or tuple is out of range. Expected 0 to %d"), servo_count - 1);
}
else {
servos[i] = (uint8_t)servo;
}
}
self->cluster->to_min(servos, length, args[ARG_load].u_bool);
delete[] servos;
}
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_all_to_min(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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
self->cluster->all_to_min(args[ARG_load].u_bool);
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_to_mid(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_servo, ARG_load };
enum { ARG_self, ARG_servos, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_servos, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
};
@ -1548,23 +1883,85 @@ 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;
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else
self->cluster->to_mid((uint)servo, args[ARG_load].u_bool);
else {
// Determine what servo(s) to enable
const mp_obj_t object = args[ARG_servos].u_obj;
if(mp_obj_is_int(object)) {
int servo = mp_obj_get_int(object);
if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else
self->cluster->to_mid((uint)servo, 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 servo indices
uint8_t *servos = new uint8_t[length];
for(size_t i = 0; i < length; i++) {
int servo = mp_obj_get_int(items[i]);
if(servo < 0 || servo >= servo_count) {
delete[] servos;
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a servo in the list or tuple is out of range. Expected 0 to %d"), servo_count - 1);
}
else {
servos[i] = (uint8_t)servo;
}
}
self->cluster->to_mid(servos, length, args[ARG_load].u_bool);
delete[] servos;
}
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_all_to_mid(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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
self->cluster->all_to_mid(args[ARG_load].u_bool);
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_to_max(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_servo, ARG_load };
enum { ARG_self, ARG_servos, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_servos, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
};
@ -1574,24 +1971,86 @@ 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;
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else
self->cluster->to_max((uint)servo, args[ARG_load].u_bool);
else {
// Determine what servo(s) to enable
const mp_obj_t object = args[ARG_servos].u_obj;
if(mp_obj_is_int(object)) {
int servo = mp_obj_get_int(object);
if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else
self->cluster->to_max((uint)servo, 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 servo indices
uint8_t *servos = new uint8_t[length];
for(size_t i = 0; i < length; i++) {
int servo = mp_obj_get_int(items[i]);
if(servo < 0 || servo >= servo_count) {
delete[] servos;
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a servo in the list or tuple is out of range. Expected 0 to %d"), servo_count - 1);
}
else {
servos[i] = (uint8_t)servo;
}
}
self->cluster->to_max(servos, length, args[ARG_load].u_bool);
delete[] servos;
}
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_all_to_max(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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
self->cluster->all_to_max(args[ARG_load].u_bool);
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 4) {
enum { ARG_self, ARG_servo, ARG_in, ARG_load };
enum { ARG_self, ARG_servos, ARG_in, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_servos, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
};
@ -1602,22 +2061,64 @@ 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;
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else {
float in = mp_obj_get_float(args[ARG_in].u_obj);
self->cluster->to_percent((uint)servo, in, args[ARG_load].u_bool);
// Determine what servo(s) to enable
const mp_obj_t object = args[ARG_servos].u_obj;
if(mp_obj_is_int(object)) {
int servo = mp_obj_get_int(object);
if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
else {
float in = mp_obj_get_float(args[ARG_in].u_obj);
self->cluster->to_percent((uint)servo, in, 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 servo indices
uint8_t *servos = new uint8_t[length];
for(size_t i = 0; i < length; i++) {
int servo = mp_obj_get_int(items[i]);
if(servo < 0 || servo >= servo_count) {
delete[] servos;
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a servo in the list or tuple is out of range. Expected 0 to %d"), servo_count - 1);
}
else {
servos[i] = (uint8_t)servo;
}
}
float in = mp_obj_get_float(args[ARG_in].u_obj);
self->cluster->to_percent(servos, length, in, args[ARG_load].u_bool);
delete[] servos;
}
}
}
}
else if(n_args <= 6) {
enum { ARG_self, ARG_servo, ARG_in, ARG_in_min, ARG_in_max, ARG_load };
enum { ARG_self, ARG_servos, ARG_in, ARG_in_min, ARG_in_max, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_servos, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ },
@ -1630,24 +2131,199 @@ 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;
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
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);
float in_max = mp_obj_get_float(args[ARG_in_max].u_obj);
self->cluster->to_percent((uint)servo, in, in_min, in_max, args[ARG_load].u_bool);
// Determine what servo(s) to enable
const mp_obj_t object = args[ARG_servos].u_obj;
if(mp_obj_is_int(object)) {
int servo = mp_obj_get_int(object);
if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
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);
float in_max = mp_obj_get_float(args[ARG_in_max].u_obj);
self->cluster->to_percent((uint)servo, in, in_min, in_max, 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 servo indices
uint8_t *servos = new uint8_t[length];
for(size_t i = 0; i < length; i++) {
int servo = mp_obj_get_int(items[i]);
if(servo < 0 || servo >= servo_count) {
delete[] servos;
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a servo in the list or tuple is out of range. Expected 0 to %d"), servo_count - 1);
}
else {
servos[i] = (uint8_t)servo;
}
}
float in = mp_obj_get_float(args[ARG_in].u_obj);
float in_min = mp_obj_get_float(args[ARG_in_min].u_obj);
float in_max = mp_obj_get_float(args[ARG_in_max].u_obj);
self->cluster->to_percent(servos, length, in, in_min, in_max, args[ARG_load].u_bool);
delete[] servos;
}
}
}
}
else {
enum { ARG_self, ARG_servo, ARG_in, ARG_in_min, ARG_in_max, ARG_value_min, ARG_value_max, ARG_load };
enum { ARG_self, ARG_servos, ARG_in, ARG_in_min, ARG_in_max, ARG_value_min, ARG_value_max, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servos, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_value_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_value_max, 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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
// Determine what servo(s) to enable
const mp_obj_t object = args[ARG_servos].u_obj;
if(mp_obj_is_int(object)) {
int servo = mp_obj_get_int(object);
if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
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);
float in_max = mp_obj_get_float(args[ARG_in_max].u_obj);
float value_min = mp_obj_get_float(args[ARG_value_min].u_obj);
float value_max = mp_obj_get_float(args[ARG_value_max].u_obj);
self->cluster->to_percent((uint)servo, in, in_min, in_max, value_min, value_max, 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 servo indices
uint8_t *servos = new uint8_t[length];
for(size_t i = 0; i < length; i++) {
int servo = mp_obj_get_int(items[i]);
if(servo < 0 || servo >= servo_count) {
delete[] servos;
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("a servo in the list or tuple is out of range. Expected 0 to %d"), servo_count - 1);
}
else {
servos[i] = (uint8_t)servo;
}
}
float in = mp_obj_get_float(args[ARG_in].u_obj);
float in_min = mp_obj_get_float(args[ARG_in_min].u_obj);
float in_max = mp_obj_get_float(args[ARG_in_max].u_obj);
float value_min = mp_obj_get_float(args[ARG_value_min].u_obj);
float value_max = mp_obj_get_float(args[ARG_value_max].u_obj);
self->cluster->to_percent(servos, length, in, in_min, in_max, value_min, value_max, args[ARG_load].u_bool);
delete[] servos;
}
}
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_all_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 3) {
enum { ARG_self, ARG_in, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in, 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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else {
float in = mp_obj_get_float(args[ARG_in].u_obj);
self->cluster->all_to_percent(in, args[ARG_load].u_bool);
}
}
else if(n_args <= 5) {
enum { ARG_self, ARG_in, ARG_in_min, ARG_in_max, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_max, 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);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
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);
float in_max = mp_obj_get_float(args[ARG_in_max].u_obj);
self->cluster->all_to_percent(in, in_min, in_max, args[ARG_load].u_bool);
}
}
else {
enum { ARG_self, ARG_in, ARG_in_min, ARG_in_max, ARG_value_min, ARG_value_max, ARG_load };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ },
@ -1662,19 +2338,16 @@ 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;
int servo_count = (int)self->cluster->get_count();
if(servo_count == 0)
mp_raise_ValueError("this cluster does not have any servos");
else if(servo < 0 || servo >= servo_count)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count - 1);
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);
float in_max = mp_obj_get_float(args[ARG_in_max].u_obj);
float value_min = mp_obj_get_float(args[ARG_value_min].u_obj);
float value_max = mp_obj_get_float(args[ARG_value_max].u_obj);
self->cluster->to_percent((uint)servo, in, in_min, in_max, value_min, value_max, args[ARG_load].u_bool);
self->cluster->all_to_percent(in, in_min, in_max, value_min, value_max, args[ARG_load].u_bool);
}
}
return mp_const_none;

7
micropython/modules/servo/servo.h
Wyświetl plik

@ -55,15 +55,22 @@ extern mp_obj_t ServoCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp
extern mp_obj_t ServoCluster_disable_all(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_is_enabled(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_all_pulses(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_all_values(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_all_phases(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_min_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_mid_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_max_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_to_min(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_all_to_min(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_to_mid(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_all_to_mid(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_to_max(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_all_to_max(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_all_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_load(mp_obj_t self_in);