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

Updated MP bindings to support latest servo features

pull/259/head
ZodiusInfuser 2022-03-08 14:26:57 +00:00
rodzic e07e248d9e
commit 176362a49b
13 zmienionych plików z 527 dodań i 262 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

33
drivers/pwm/pwm_cluster.cpp
Wyświetl plik

@ -90,7 +90,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask)
, channels(nullptr)
, wrap_level(0) {
// Initialise all the channels this PWM will control
// Create the channel mapping
for(uint pin = 0; pin < NUM_BANK0_GPIOS; pin++) {
if(bit_in_mask(pin, pin_mask)) {
channel_to_pin_map[channel_count] = pin;
@ -98,6 +98,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_mask)
}
}
// Initialise all the channels this PWM will control
if(channel_count > 0) {
channels = new ChannelState[channel_count];
}
@ -112,7 +113,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count)
, channels(nullptr)
, wrap_level(0) {
// Initialise all the channels this PWM will control
// Create the pin mask and channel mapping
uint pin_end = MIN(pin_count + pin_base, NUM_BANK0_GPIOS);
for(uint pin = pin_base; pin < pin_end; pin++) {
pin_mask |= (1u << pin);
@ -120,6 +121,31 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count)
channel_count++;
}
// Initialise all the channels this PWM will control
if(channel_count > 0) {
channels = new ChannelState[channel_count];
}
}
PWMCluster::PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length)
: pio(pio)
, sm(sm)
, pin_mask(0x00000000)
, channel_count(0)
, channels(nullptr)
, wrap_level(0) {
// Create the pin mask and channel mapping
for(uint i = 0; i < length; i++) {
uint8_t pin = pins[i];
if(pin < NUM_BANK0_GPIOS) {
pin_mask |= (1u << pin);
channel_to_pin_map[channel_count] = pin;
channel_count++;
}
}
// Initialise all the channels this PWM will control
if(channel_count > 0) {
channels = new ChannelState[channel_count];
}
@ -133,7 +159,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins)
, channels(nullptr)
, wrap_level(0) {
// Populate the pin mask
// Create the pin mask and channel mapping
for(auto pin : pins) {
if(pin < NUM_BANK0_GPIOS) {
pin_mask |= (1u << pin);
@ -142,6 +168,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins)
}
}
// Initialise all the channels this PWM will control
if(channel_count > 0) {
channels = new ChannelState[channel_count];
}

1
drivers/pwm/pwm_cluster.hpp
Wyświetl plik

@ -84,6 +84,7 @@ namespace pimoroni {
public:
PWMCluster(PIO pio, uint sm, uint pin_mask);
PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count);
PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length);
PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins);
~PWMCluster();

11
drivers/servo/calibration.cpp
Wyświetl plik

@ -5,7 +5,7 @@ namespace servo {
: pulse(0.0f), value(0.0f) {
}
Calibration::Point::Point(uint16_t pulse, float value)
Calibration::Point::Point(float pulse, float value)
: pulse(pulse), value(value) {
}
@ -111,12 +111,19 @@ namespace servo {
return nullptr;
}
bool Calibration::has_lower_limit() const {
return limit_lower;
}
bool Calibration::has_upper_limit() const {
return limit_upper;
}
void Calibration::limit_to_calibration(bool lower, bool upper) {
limit_lower = lower;
limit_upper = upper;
}
bool Calibration::value_to_pulse(float value, float &pulse_out, float &value_out) const {
bool success = false;
if(calibration_size >= 2) {

6
drivers/servo/calibration.hpp
Wyświetl plik

@ -34,7 +34,7 @@ namespace servo {
// Constructors/Destructor
//--------------------------------------------------
Point();
Point(uint16_t pulse, float value);
Point(float pulse, float value);
//--------------------------------------------------
@ -69,6 +69,8 @@ namespace servo {
Point* first_point() const;
Point* last_point() const;
bool has_lower_limit() const;
bool has_upper_limit() const;
void limit_to_calibration(bool lower, bool upper);
bool value_to_pulse(float value, float &pulse_out, float &value_out) const;
@ -80,7 +82,7 @@ namespace servo {
//--------------------------------------------------
// Variables
//--------------------------------------------------
protected:
private:
Point* calibration;
uint calibration_size;
bool limit_lower;

16
drivers/servo/servo.cpp
Wyświetl plik

@ -52,14 +52,6 @@ namespace servo {
return state.is_enabled();
}
float Servo::get_value() const {
return state.get_value();
}
void Servo::set_value(float value) {
apply_pulse(state.set_value(value));
}
float Servo::get_pulse() const {
return state.get_pulse();
}
@ -68,6 +60,14 @@ namespace servo {
apply_pulse(state.set_pulse(pulse));
}
float Servo::get_value() const {
return state.get_value();
}
void Servo::set_value(float value) {
apply_pulse(state.set_value(value));
}
float Servo::get_frequency() const {
return pwm_frequency;
}

6
drivers/servo/servo.hpp
Wyświetl plik

@ -38,12 +38,12 @@ namespace servo {
void disable();
bool is_enabled() const;
float get_value() const;
void set_value(float value);
float get_pulse() const;
void set_pulse(float pulse);
float get_value() const;
void set_value(float value);
float get_frequency() const;
bool set_frequency(float freq);

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

@ -4,17 +4,22 @@
namespace servo {
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase)
: pwms(pio, sm, pin_mask), pwm_frequency(freq) {
: pwms(pio, sm, pin_mask), pwm_frequency(freq) {
create_servo_states(default_type, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq, bool auto_phase)
: pwms(pio, sm, pin_base, pin_count), pwm_frequency(freq) {
: pwms(pio, sm, pin_base, pin_count), pwm_frequency(freq) {
create_servo_states(default_type, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type, float freq, bool auto_phase)
: pwms(pio, sm, pins, length), pwm_frequency(freq) {
create_servo_states(default_type, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type, float freq, bool auto_phase)
: pwms(pio, sm, pins), pwm_frequency(freq) {
: pwms(pio, sm, pins), pwm_frequency(freq) {
create_servo_states(default_type, auto_phase);
}
@ -80,17 +85,6 @@ namespace servo {
return servos[servo].is_enabled();
}
float ServoCluster::get_value(uint servo) const {
assert(servo < pwms.get_chan_count());
return servos[servo].get_value();
}
void ServoCluster::set_value(uint servo, float value, bool load) {
assert(servo < pwms.get_chan_count());
float new_pulse = servos[servo].set_value(value);
apply_pulse(servo, new_pulse, load);
}
float ServoCluster::get_pulse(uint servo) const {
assert(servo < pwms.get_chan_count());
return servos[servo].get_pulse();
@ -102,6 +96,17 @@ namespace servo {
apply_pulse(servo, new_pulse, load);
}
float ServoCluster::get_value(uint servo) const {
assert(servo < pwms.get_chan_count());
return servos[servo].get_value();
}
void ServoCluster::set_value(uint servo, float value, bool load) {
assert(servo < pwms.get_chan_count());
float new_pulse = servos[servo].set_value(value);
apply_pulse(servo, new_pulse, load);
}
float ServoCluster::get_phase(uint servo) const {
assert(servo < pwms.get_chan_count());
return servo_phases[servo];

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

@ -24,9 +24,11 @@ namespace servo {
public:
ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
ServoCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
~ServoCluster();
//--------------------------------------------------
// Methods
//--------------------------------------------------
@ -40,12 +42,12 @@ namespace servo {
void disable(uint servo, bool load = true);
bool is_enabled(uint servo) const;
float get_value(uint servo) const;
void set_value(uint servo, float value, bool load = true);
float get_pulse(uint servo) const;
void set_pulse(uint servo, float pulse, bool load = true);
float get_value(uint servo) const;
void set_value(uint servo, float value, bool load = true);
float get_phase(uint servo) const;
void set_phase(uint servo, float phase, bool load = true);

28
drivers/servo/servo_state.cpp
Wyświetl plik

@ -28,20 +28,6 @@ namespace servo {
return enabled;
}
float ServoState::get_value() const {
return servo_value;
}
float ServoState::set_value(float value) {
float pulse_out, value_out;
if(table.value_to_pulse(value, pulse_out, value_out)) {
last_enabled_pulse = pulse_out;
servo_value = value_out;
return _enable();
}
return disable();
}
float ServoState::get_pulse() const {
return last_enabled_pulse;
}
@ -58,6 +44,20 @@ namespace servo {
return disable();
}
float ServoState::get_value() const {
return servo_value;
}
float ServoState::set_value(float value) {
float pulse_out, value_out;
if(table.value_to_pulse(value, pulse_out, value_out)) {
last_enabled_pulse = pulse_out;
servo_value = value_out;
return _enable();
}
return disable();
}
float ServoState::get_min_value() const {
float value = 0.0f;
Calibration::Point *point = table.first_point();

6
drivers/servo/servo_state.hpp
Wyświetl plik

@ -47,12 +47,12 @@ namespace servo {
private:
float _enable(); // Internal version of enable without convenient initialisation to mid point
public:
float get_value() const;
float set_value(float value);
float get_pulse() const;
float set_pulse(float pulse);
float get_value() const;
float set_value(float value);
public:
float get_min_value() const;
float get_mid_value() const;

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

@ -12,6 +12,8 @@ MP_DEFINE_CONST_FUN_OBJ_1(Calibration_size_obj, Calibration_size);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_point_at_obj, 2, Calibration_point_at);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_first_point_obj, 1, Calibration_first_point);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_last_point_obj, 1, Calibration_last_point);
MP_DEFINE_CONST_FUN_OBJ_1(Calibration_has_lower_limit_obj, Calibration_has_lower_limit);
MP_DEFINE_CONST_FUN_OBJ_1(Calibration_has_upper_limit_obj, Calibration_has_upper_limit);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_limit_to_calibration_obj, 3, Calibration_limit_to_calibration);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_value_to_pulse_obj, 2, Calibration_value_to_pulse);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_pulse_to_value_obj, 2, Calibration_pulse_to_value);
@ -34,11 +36,14 @@ MP_DEFINE_CONST_FUN_OBJ_KW(Servo_to_percent_obj, 2, Servo_to_percent);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_calibration_obj, Servo_calibration);
MP_DEFINE_CONST_FUN_OBJ_1(ServoCluster___del___obj, ServoCluster___del__);
MP_DEFINE_CONST_FUN_OBJ_1(ServoCluster_count_obj, ServoCluster_count);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_pin_obj, 2, ServoCluster_pin);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_enable_obj, 2, ServoCluster_enable);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_disable_obj, 2, ServoCluster_disable);
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_pulse_obj, 2, ServoCluster_pulse);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_phase_obj, 2, ServoCluster_phase);
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);
@ -61,6 +66,8 @@ STATIC const mp_rom_map_elem_t Calibration_locals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR_point_at), MP_ROM_PTR(&Calibration_point_at_obj) },
{ MP_ROM_QSTR(MP_QSTR_first_point), MP_ROM_PTR(&Calibration_first_point_obj) },
{ MP_ROM_QSTR(MP_QSTR_last_point), MP_ROM_PTR(&Calibration_last_point_obj) },
{ MP_ROM_QSTR(MP_QSTR_has_lower_limit), MP_ROM_PTR(&Calibration_has_lower_limit_obj) },
{ MP_ROM_QSTR(MP_QSTR_has_upper_limit), MP_ROM_PTR(&Calibration_has_upper_limit_obj) },
{ MP_ROM_QSTR(MP_QSTR_limit_to_calibration), MP_ROM_PTR(&Calibration_limit_to_calibration_obj) },
{ MP_ROM_QSTR(MP_QSTR_value_to_pulse), MP_ROM_PTR(&Calibration_value_to_pulse_obj) },
{ MP_ROM_QSTR(MP_QSTR_pulse_to_value), MP_ROM_PTR(&Calibration_pulse_to_value_obj) },
@ -72,8 +79,8 @@ STATIC const mp_rom_map_elem_t Servo_locals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR_enable), MP_ROM_PTR(&Servo_enable_obj) },
{ MP_ROM_QSTR(MP_QSTR_disable), MP_ROM_PTR(&Servo_disable_obj) },
{ MP_ROM_QSTR(MP_QSTR_is_enabled), MP_ROM_PTR(&Servo_is_enabled_obj) },
{ MP_ROM_QSTR(MP_QSTR_value), MP_ROM_PTR(&Servo_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_pulse), MP_ROM_PTR(&Servo_pulse_obj) },
{ MP_ROM_QSTR(MP_QSTR_value), MP_ROM_PTR(&Servo_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&Servo_frequency_obj) },
{ MP_ROM_QSTR(MP_QSTR_min_value), MP_ROM_PTR(&Servo_min_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_mid_value), MP_ROM_PTR(&Servo_mid_value_obj) },
@ -87,11 +94,14 @@ STATIC const mp_rom_map_elem_t Servo_locals_dict_table[] = {
STATIC const mp_rom_map_elem_t ServoCluster_locals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&ServoCluster___del___obj) },
{ MP_ROM_QSTR(MP_QSTR_count), MP_ROM_PTR(&ServoCluster_count_obj) },
{ MP_ROM_QSTR(MP_QSTR_pin), MP_ROM_PTR(&ServoCluster_pin_obj) },
{ MP_ROM_QSTR(MP_QSTR_enable), MP_ROM_PTR(&ServoCluster_enable_obj) },
{ MP_ROM_QSTR(MP_QSTR_disable), MP_ROM_PTR(&ServoCluster_disable_obj) },
{ MP_ROM_QSTR(MP_QSTR_is_enabled), MP_ROM_PTR(&ServoCluster_is_enabled_obj) },
{ MP_ROM_QSTR(MP_QSTR_value), MP_ROM_PTR(&ServoCluster_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_pulse), MP_ROM_PTR(&ServoCluster_pulse_obj) },
{ MP_ROM_QSTR(MP_QSTR_value), MP_ROM_PTR(&ServoCluster_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_phase), MP_ROM_PTR(&ServoCluster_phase_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) },
@ -132,7 +142,6 @@ const mp_obj_type_t ServoCluster_type = {
.locals_dict = (mp_obj_dict_t*)&ServoCluster_locals_dict,
};
// TODO What is this? Should remove?
typedef struct _mp_obj_float_t {
mp_obj_base_t base;
mp_float_t value;

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

@ -49,19 +49,18 @@ void Calibration_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_
if(i < size - 1)
mp_print_str(print, ", ");
}
mp_print_str(print, "}, limits = {");
mp_obj_print_helper(print, calib->has_lower_limit() ? mp_const_true : mp_const_false, PRINT_REPR);
mp_print_str(print, ", ");
mp_obj_print_helper(print, calib->has_upper_limit() ? mp_const_true : mp_const_false, PRINT_REPR);
mp_print_str(print, "})");
}
/***** Destructor ******/
mp_obj_t Calibration___del__(mp_obj_t self_in) {
_Calibtration_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Calibtration_obj_t);
if(self->owner)
delete self->calibration;
return mp_const_none;
}
/***** Constructor *****/
mp_obj_t Calibration_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
//NOTE Wonder if I can make it so calibration objects cannot be created in MP?
_Calibtration_obj_t *self = nullptr;
enum { ARG_type };
@ -83,6 +82,16 @@ mp_obj_t Calibration_make_new(const mp_obj_type_t *type, size_t n_args, size_t n
return MP_OBJ_FROM_PTR(self);
}
/***** Destructor ******/
mp_obj_t Calibration___del__(mp_obj_t self_in) {
_Calibtration_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Calibtration_obj_t);
if(self->owner)
delete self->calibration;
return mp_const_none;
}
/***** Methods *****/
mp_obj_t Calibration_create_blank_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_size };
@ -230,8 +239,11 @@ mp_obj_t Calibration_point_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
int index = args[ARG_index].u_int;
if(index < 0 || index >= (int)self->calibration->size())
mp_raise_ValueError("index out of range. Expected 0 to size()-1");
int calibration_size = (int)self->calibration->size();
if(calibration_size == 0)
mp_raise_ValueError("this calibration does not have any points");
if(index < 0 || index >= calibration_size)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("index out of range. Expected 0 to %d"), calibration_size);
else {
Calibration::Point *point = self->calibration->point_at((uint)index);
@ -256,8 +268,11 @@ mp_obj_t Calibration_point_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
int index = args[ARG_index].u_int;
if(index < 0 || index >= (int)self->calibration->size())
mp_raise_ValueError("index out of range. Expected 0 to size()-1");
int calibration_size = (int)self->calibration->size();
if(calibration_size == 0)
mp_raise_ValueError("this calibration does not have any points");
if(index < 0 || index >= calibration_size)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("index out of range. Expected 0 to %d"), calibration_size);
else {
Calibration::Point *point = self->calibration->point_at((uint)index);
@ -286,9 +301,9 @@ mp_obj_t Calibration_point_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t
mp_raise_TypeError("can't convert object to list or tuple");
}
}
return mp_const_none;
}
return mp_const_none;
}
mp_obj_t Calibration_first_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
@ -305,11 +320,14 @@ mp_obj_t Calibration_first_point(size_t n_args, const mp_obj_t *pos_args, mp_map
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->first_point();
mp_obj_t tuple[2];
tuple[0] = mp_obj_new_float(point->pulse);
tuple[1] = mp_obj_new_float(point->value);
return mp_obj_new_tuple(2, tuple);
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
mp_obj_t tuple[2];
tuple[0] = mp_obj_new_float(point->pulse);
tuple[1] = mp_obj_new_float(point->value);
return mp_obj_new_tuple(2, tuple);
}
}
else {
enum { ARG_self, ARG_point };
@ -325,34 +343,37 @@ mp_obj_t Calibration_first_point(size_t n_args, const mp_obj_t *pos_args, mp_map
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->first_point();
const mp_obj_t object = args[ARG_point].u_obj;
if(mp_obj_is_type(object, &mp_type_list)) {
mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t);
if(list->len == 2) {
point->pulse = mp_obj_get_float(list->items[0]);
point->value = mp_obj_get_float(list->items[1]);
}
else {
mp_raise_ValueError("list must contain two numbers");
}
}
else if(!mp_obj_is_type(object, &mp_type_tuple)) {
mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t);
if(tuple->len == 2) {
point->pulse = mp_obj_get_float(tuple->items[0]);
point->value = mp_obj_get_float(tuple->items[1]);
}
else {
mp_raise_ValueError("tuple must contain two numbers");
}
}
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
mp_raise_TypeError("can't convert object to list or tuple");
const mp_obj_t object = args[ARG_point].u_obj;
if(mp_obj_is_type(object, &mp_type_list)) {
mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t);
if(list->len == 2) {
point->pulse = mp_obj_get_float(list->items[0]);
point->value = mp_obj_get_float(list->items[1]);
}
else {
mp_raise_ValueError("list must contain two numbers");
}
}
else if(!mp_obj_is_type(object, &mp_type_tuple)) {
mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t);
if(tuple->len == 2) {
point->pulse = mp_obj_get_float(tuple->items[0]);
point->value = mp_obj_get_float(tuple->items[1]);
}
else {
mp_raise_ValueError("tuple must contain two numbers");
}
}
else {
mp_raise_TypeError("can't convert object to list or tuple");
}
}
return mp_const_none;
}
return mp_const_none;
}
mp_obj_t Calibration_last_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
@ -369,11 +390,14 @@ mp_obj_t Calibration_last_point(size_t n_args, const mp_obj_t *pos_args, mp_map_
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->last_point();
mp_obj_t tuple[2];
tuple[0] = mp_obj_new_float(point->pulse);
tuple[1] = mp_obj_new_float(point->value);
return mp_obj_new_tuple(2, tuple);
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
mp_obj_t tuple[2];
tuple[0] = mp_obj_new_float(point->pulse);
tuple[1] = mp_obj_new_float(point->value);
return mp_obj_new_tuple(2, tuple);
}
}
else {
enum { ARG_self, ARG_point };
@ -389,34 +413,47 @@ mp_obj_t Calibration_last_point(size_t n_args, const mp_obj_t *pos_args, mp_map_
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->last_point();
const mp_obj_t object = args[ARG_point].u_obj;
if(mp_obj_is_type(object, &mp_type_list)) {
mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t);
if(list->len == 2) {
point->pulse = mp_obj_get_float(list->items[0]);
point->value = mp_obj_get_float(list->items[1]);
}
else {
mp_raise_ValueError("list must contain two numbers");
}
}
else if(!mp_obj_is_type(object, &mp_type_tuple)) {
mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t);
if(tuple->len == 2) {
point->pulse = mp_obj_get_float(tuple->items[0]);
point->value = mp_obj_get_float(tuple->items[1]);
}
else {
mp_raise_ValueError("tuple must contain two numbers");
}
}
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
mp_raise_TypeError("can't convert object to list or tuple");
const mp_obj_t object = args[ARG_point].u_obj;
if(mp_obj_is_type(object, &mp_type_list)) {
mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t);
if(list->len == 2) {
point->pulse = mp_obj_get_float(list->items[0]);
point->value = mp_obj_get_float(list->items[1]);
}
else {
mp_raise_ValueError("list must contain two numbers");
}
}
else if(!mp_obj_is_type(object, &mp_type_tuple)) {
mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t);
if(tuple->len == 2) {
point->pulse = mp_obj_get_float(tuple->items[0]);
point->value = mp_obj_get_float(tuple->items[1]);
}
else {
mp_raise_ValueError("tuple must contain two numbers");
}
}
else {
mp_raise_TypeError("can't convert object to list or tuple");
}
}
return mp_const_none;
}
return mp_const_none;
}
mp_obj_t Calibration_has_lower_limit(mp_obj_t self_in) {
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Calibration_obj_t);
return self->calibration->has_lower_limit() ? mp_const_true : mp_const_false;
}
mp_obj_t Calibration_has_upper_limit(mp_obj_t self_in) {
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Calibration_obj_t);
return self->calibration->has_upper_limit() ? mp_const_true : mp_const_false;
}
mp_obj_t Calibration_limit_to_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
@ -463,7 +500,7 @@ mp_obj_t Calibration_value_to_pulse(size_t n_args, const mp_obj_t *pos_args, mp_
return mp_obj_new_tuple(2, tuple);
}
else {
mp_raise_msg(&mp_type_RuntimeError, "Unable to convert value to pulse. Calibration invalid");
mp_raise_msg(&mp_type_RuntimeError, "Unable to convert value to pulse. Calibration needs at least 2 points");
}
return mp_const_none;
}
@ -491,7 +528,7 @@ mp_obj_t Calibration_pulse_to_value(size_t n_args, const mp_obj_t *pos_args, mp_
return mp_obj_new_tuple(2, tuple);
}
else {
mp_raise_msg(&mp_type_RuntimeError, "Unable to convert pulse to value. Calibration invalid");
mp_raise_msg(&mp_type_RuntimeError, "Unable to convert pulse to value. Calibration needs at least 2 points");
}
return mp_const_none;
}
@ -503,6 +540,7 @@ mp_obj_t Calibration_pulse_to_value(size_t n_args, const mp_obj_t *pos_args, mp_
typedef struct _Servo_obj_t {
mp_obj_base_t base;
Servo* servo;
// NOTE should this keep track of all calibration objects released?
} _Servo_obj_t;
@ -526,12 +564,6 @@ void Servo_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind
mp_print_str(print, ")");
}
/***** Destructor ******/
mp_obj_t Servo___del__(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
delete self->servo;
return mp_const_none;
}
/***** Constructor *****/
mp_obj_t Servo_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
@ -565,6 +597,15 @@ mp_obj_t Servo_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c
return MP_OBJ_FROM_PTR(self);
}
/***** Destructor ******/
mp_obj_t Servo___del__(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
delete self->servo;
return mp_const_none;
}
/***** Methods *****/
extern mp_obj_t Servo_pin(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
@ -588,41 +629,6 @@ extern mp_obj_t Servo_is_enabled(mp_obj_t self_in) {
return self->servo->is_enabled() ? mp_const_true : mp_const_false;
}
extern mp_obj_t Servo_value(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);
_Servo_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Servo_obj_t);
return mp_obj_new_float(self->servo->get_value());
}
else {
enum { ARG_self, ARG_value };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_value, 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);
_Servo_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Servo_obj_t);
float value = mp_obj_get_float(args[ARG_value].u_obj);
self->servo->set_value(value);
return mp_const_none;
}
}
extern mp_obj_t Servo_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 1) {
enum { ARG_self };
@ -658,6 +664,41 @@ extern mp_obj_t Servo_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *k
}
}
extern mp_obj_t Servo_value(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);
_Servo_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Servo_obj_t);
return mp_obj_new_float(self->servo->get_value());
}
else {
enum { ARG_self, ARG_value };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_value, 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);
_Servo_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Servo_obj_t);
float value = mp_obj_get_float(args[ARG_value].u_obj);
self->servo->set_value(value);
return mp_const_none;
}
}
extern mp_obj_t Servo_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 1) {
enum { ARG_self };
@ -688,10 +729,10 @@ extern mp_obj_t Servo_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_
float freq = mp_obj_get_float(args[ARG_freq].u_obj);
if(!self->servo->set_frequency(freq))
if(!self->servo->set_frequency(freq)) {
mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz");
else
return mp_const_none;
}
return mp_const_none;
}
}
@ -792,6 +833,7 @@ extern mp_obj_t Servo_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map
self->servo->to_percent(in, in_min, in_max, value_min, value_max);
}
return mp_const_none;
}
@ -825,7 +867,7 @@ void ServoCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
mp_print_str(print, "ServoCluster(");
mp_print_str(print, "pins = {");
mp_print_str(print, "servos = {");
bool first = true;
uint8_t servo_count = self->cluster->get_count();
@ -833,8 +875,18 @@ void ServoCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind
if(!first) {
mp_print_str(print, ", ");
}
uint8_t pin = self->cluster->get_pin(servo);
mp_obj_print_helper(print, mp_obj_new_int(pin), PRINT_REPR);
mp_print_str(print, "{ pin = ");
mp_obj_print_helper(print, mp_obj_new_int(self->cluster->get_pin(servo)), PRINT_REPR);
mp_print_str(print, ", enabled = ");
mp_obj_print_helper(print, self->cluster->is_enabled(servo) ? mp_const_true : mp_const_false, PRINT_REPR);
mp_print_str(print, ", pulse = ");
mp_obj_print_helper(print, mp_obj_new_float(self->cluster->get_pulse(servo)), PRINT_REPR);
mp_print_str(print, ", value = ");
mp_obj_print_helper(print, mp_obj_new_float(self->cluster->get_value(servo)), PRINT_REPR);
mp_print_str(print, ", phase = ");
mp_obj_print_helper(print, mp_obj_new_float(self->cluster->get_phase(servo)), PRINT_REPR);
mp_print_str(print, "}");
first = false;
}
mp_print_str(print, "}, freq = ");
@ -843,25 +895,19 @@ void ServoCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind
mp_print_str(print, ")");
}
/***** Destructor ******/
mp_obj_t ServoCluster___del__(mp_obj_t self_in) {
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
delete self->cluster;
return mp_const_none;
}
/***** Constructor *****/
mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
_ServoCluster_obj_t *self = nullptr;
enum { ARG_pio, ARG_sm, ARG_pin_base, ARG_pin_count, ARG_type, ARG_freq };
enum { ARG_pio, ARG_sm, ARG_pins, ARG_type, ARG_freq, ARG_auto_phase };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_pin_base, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_pin_count, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_type, MP_ARG_INT, {.u_int = (uint8_t)servo::CalibrationType::ANGULAR} },
{ MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} },
{ MP_QSTR_auto_phase, MP_ARG_BOOL, {.u_bool = true} },
};
// Parse args.
@ -870,8 +916,59 @@ mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t
PIO pio = args[ARG_pio].u_int == 0 ? pio0 : pio1;
int sm = args[ARG_sm].u_int;
int pin_base = args[ARG_pin_base].u_int;
int pin_count = args[ARG_pin_count].u_int;
uint pin_mask = 0;
bool mask_provided = true;
uint32_t pin_count = 0;
uint8_t* pins = nullptr;
// Determine what pins this cluster will use
const mp_obj_t object = args[ARG_pins].u_obj;
if(mp_obj_is_type(object, &mp_type_int)) {
pin_mask = (uint)mp_obj_get_int(object);
}
else if(mp_obj_is_type(object, &mp_type_list)) {
mp_obj_list_t *list = MP_OBJ_TO_PTR2(object, mp_obj_list_t);
pin_count = list->len;
if(pin_count > 0) {
// Create and populate a local array of pins
pins = new uint8_t[pin_count];
for(uint32_t i = 0; i < pin_count; i++) {
int pin = mp_obj_get_int(list->items[i]);
if(pin >= 0 && pin < (int)NUM_BANK0_GPIOS) {
pins[i] = (uint8_t)pin;
}
else {
delete[] pins;
mp_raise_ValueError("a pin in the list is out of range. Expected 0 to 29");
}
}
mask_provided = false;
}
}
else if(mp_obj_is_type(object, &mp_type_tuple)) {
mp_obj_tuple_t *tuple = MP_OBJ_TO_PTR2(object, mp_obj_tuple_t);
pin_count = tuple->len;
if(pin_count > 0) {
// Create and populate a local array of pins
pins = new uint8_t[pin_count];
for(uint i = 0; i < pin_count; i++) {
int pin = mp_obj_get_int(tuple->items[i]);
if(pin >= 0 && pin < (int)NUM_BANK0_GPIOS) {
pins[i] = (uint8_t)pin;
}
else {
delete[] pins;
mp_raise_ValueError("a pin in the tuple is out of range. Expected 0 to 29");
}
}
mask_provided = false;
}
}
else {
mp_raise_TypeError("cannot convert object to a list or tuple of pins, or a pin mask integer");
}
servo::CalibrationType calibration_type = (servo::CalibrationType)args[ARG_type].u_int;
float freq = servo::ServoState::DEFAULT_FREQUENCY;
@ -879,17 +976,40 @@ mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t
freq = mp_obj_get_float(args[ARG_freq].u_obj);
}
bool auto_phase = args[ARG_auto_phase].u_bool;
self = m_new_obj_with_finaliser(_ServoCluster_obj_t);
self->base.type = &ServoCluster_type;
self->cluster = new ServoCluster(pio, sm, (uint)pin_base, (uint)pin_count, calibration_type, freq);
if(mask_provided)
self->cluster = new ServoCluster(pio, sm, pin_mask, calibration_type, freq, auto_phase);
else
self->cluster = new ServoCluster(pio, sm, pins, pin_count, calibration_type, freq, auto_phase);
self->cluster->init();
// Cleanup the pins array
if(pins != nullptr)
delete[] pins;
return MP_OBJ_FROM_PTR(self);
}
/***** Destructor ******/
mp_obj_t ServoCluster___del__(mp_obj_t self_in) {
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
delete self->cluster;
return mp_const_none;
}
/***** Methods *****/
extern mp_obj_t ServoCluster_enable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
extern mp_obj_t ServoCluster_count(mp_obj_t self_in) {
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
return mp_obj_new_int(self->cluster->get_count());
}
extern mp_obj_t ServoCluster_pin(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_servo };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
@ -909,16 +1029,43 @@ extern mp_obj_t ServoCluster_enable(size_t n_args, const mp_obj_t *pos_args, mp_
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);
else
self->cluster->enable((uint)servo);
return mp_obj_new_int(self->cluster->get_pin((uint)servo));
return mp_const_none;
}
extern mp_obj_t ServoCluster_enable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_servo, 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_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 = 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);
else
self->cluster->enable((uint)servo, args[ARG_servo].u_bool);
return mp_const_none;
}
extern mp_obj_t ServoCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_servo };
enum { ARG_self, ARG_servo, 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_load, MP_ARG_BOOL, { .u_bool = true }},
};
// Parse args.
@ -964,57 +1111,6 @@ extern mp_obj_t ServoCluster_is_enabled(size_t n_args, const mp_obj_t *pos_args,
return mp_const_none;
}
extern mp_obj_t ServoCluster_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 2) {
enum { ARG_self, ARG_servo };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, 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);
_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);
else
return mp_obj_new_float(self->cluster->get_value((uint)servo));
}
else {
enum { ARG_self, ARG_servo, ARG_value };
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 },
};
// 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 = 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);
else {
float value = mp_obj_get_float(args[ARG_value].u_obj);
self->cluster->set_value((uint)servo, value);
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 2) {
enum { ARG_self, ARG_servo };
@ -1039,11 +1135,12 @@ 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 };
enum { ARG_self, ARG_servo, 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 }},
};
// Parse args.
@ -1060,7 +1157,111 @@ extern mp_obj_t ServoCluster_pulse(size_t n_args, const mp_obj_t *pos_args, mp_m
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count);
else {
float pulse = mp_obj_get_float(args[ARG_pulse].u_obj);
self->cluster->set_pulse((uint)servo, pulse);
self->cluster->set_pulse((uint)servo, pulse, args[ARG_servo].u_bool);
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 2) {
enum { ARG_self, ARG_servo };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, 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);
_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);
else
return mp_obj_new_float(self->cluster->get_value((uint)servo));
}
else {
enum { ARG_self, ARG_servo, 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 }},
};
// 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 = 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);
else {
float value = mp_obj_get_float(args[ARG_value].u_obj);
self->cluster->set_value((uint)servo, value, args[ARG_servo].u_bool);
}
}
return mp_const_none;
}
extern mp_obj_t ServoCluster_phase(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 2) {
enum { ARG_self, ARG_servo };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, 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);
_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);
else
return mp_obj_new_float(self->cluster->get_phase((uint)servo));
}
else {
enum { ARG_self, ARG_servo, 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 }},
};
// 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 = 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);
else {
float phase = mp_obj_get_float(args[ARG_phase].u_obj);
self->cluster->set_phase((uint)servo, phase, args[ARG_servo].u_bool);
}
}
return mp_const_none;
@ -1179,10 +1380,11 @@ 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 };
enum { ARG_self, ARG_servo, 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_load, MP_ARG_BOOL, { .u_bool = true }},
};
// Parse args.
@ -1198,16 +1400,17 @@ extern mp_obj_t ServoCluster_to_min(size_t n_args, const mp_obj_t *pos_args, mp_
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);
else
self->cluster->to_min((uint)servo);
self->cluster->to_min((uint)servo, args[ARG_servo].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 };
enum { ARG_self, ARG_servo, 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_load, MP_ARG_BOOL, { .u_bool = true }},
};
// Parse args.
@ -1223,16 +1426,17 @@ extern mp_obj_t ServoCluster_to_mid(size_t n_args, const mp_obj_t *pos_args, mp_
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);
else
self->cluster->to_mid((uint)servo);
self->cluster->to_mid((uint)servo, args[ARG_servo].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 };
enum { ARG_self, ARG_servo, 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_load, MP_ARG_BOOL, { .u_bool = true }},
};
// Parse args.
@ -1248,18 +1452,19 @@ extern mp_obj_t ServoCluster_to_max(size_t n_args, const mp_obj_t *pos_args, mp_
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);
else
self->cluster->to_max((uint)servo);
self->cluster->to_max((uint)servo, args[ARG_servo].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 <= 2) {
enum { ARG_self, ARG_servo, ARG_in };
if(n_args <= 4) {
enum { ARG_self, ARG_servo, 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_in, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
};
// Parse args.
@ -1276,17 +1481,18 @@ extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args,
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count);
else {
float in = mp_obj_get_float(args[ARG_in].u_obj);
self->cluster->to_percent((uint)servo, in);
self->cluster->to_percent((uint)servo, in, args[ARG_servo].u_bool);
}
}
else if(n_args <= 4) {
enum { ARG_self, ARG_servo, ARG_in, ARG_in_min, ARG_in_max };
else if(n_args <= 6) {
enum { ARG_self, ARG_servo, 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_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.
@ -1305,11 +1511,11 @@ extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args,
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);
self->cluster->to_percent((uint)servo, in, in_min, in_max, args[ARG_servo].u_bool);
}
}
else {
enum { ARG_self, ARG_servo, ARG_in, ARG_in_min, ARG_in_max, ARG_value_min, ARG_value_max };
enum { ARG_self, ARG_servo, 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 },
@ -1317,7 +1523,8 @@ extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args,
{ 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_value_max, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
};
// Parse args.
@ -1338,7 +1545,7 @@ extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args,
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);
self->cluster->to_percent((uint)servo, in, in_min, in_max, value_min, value_max, args[ARG_servo].u_bool);
}
}
return mp_const_none;

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

@ -19,6 +19,8 @@ extern mp_obj_t Calibration_size(mp_obj_t self_in);
extern mp_obj_t Calibration_point_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_first_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_last_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_has_lower_limit(mp_obj_t self_in);
extern mp_obj_t Calibration_has_upper_limit(mp_obj_t self_in);
extern mp_obj_t Calibration_limit_to_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_value_to_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_pulse_to_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
@ -30,8 +32,8 @@ extern mp_obj_t Servo_pin(mp_obj_t self_in);
extern mp_obj_t Servo_enable(mp_obj_t self_in);
extern mp_obj_t Servo_disable(mp_obj_t self_in);
extern mp_obj_t Servo_is_enabled(mp_obj_t self_in);
extern mp_obj_t Servo_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Servo_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Servo_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Servo_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Servo_min_value(mp_obj_t self_in);
extern mp_obj_t Servo_mid_value(mp_obj_t self_in);
@ -45,11 +47,14 @@ extern mp_obj_t Servo_calibration(mp_obj_t self_in);
extern void ServoCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind);
extern mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args);
extern mp_obj_t ServoCluster___del__(mp_obj_t self_in);
extern mp_obj_t ServoCluster_count(mp_obj_t self_in);
extern mp_obj_t ServoCluster_pin(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_enable(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_disable(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_value(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_value(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_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);