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

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

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

@ -84,6 +84,7 @@ namespace pimoroni {
public: public:
PWMCluster(PIO pio, uint sm, uint pin_mask); PWMCluster(PIO pio, uint sm, uint pin_mask);
PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count); 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(PIO pio, uint sm, std::initializer_list<uint8_t> pins);
~PWMCluster(); ~PWMCluster();

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

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

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

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

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

@ -52,14 +52,6 @@ namespace servo {
return state.is_enabled(); 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 { float Servo::get_pulse() const {
return state.get_pulse(); return state.get_pulse();
} }
@ -68,6 +60,14 @@ namespace servo {
apply_pulse(state.set_pulse(pulse)); 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 { float Servo::get_frequency() const {
return pwm_frequency; return pwm_frequency;
} }

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

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

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

@ -4,17 +4,22 @@
namespace servo { namespace servo {
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase) 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); 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) 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); 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) 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); create_servo_states(default_type, auto_phase);
} }
@ -80,17 +85,6 @@ namespace servo {
return servos[servo].is_enabled(); 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 { float ServoCluster::get_pulse(uint servo) const {
assert(servo < pwms.get_chan_count()); assert(servo < pwms.get_chan_count());
return servos[servo].get_pulse(); return servos[servo].get_pulse();
@ -102,6 +96,17 @@ namespace servo {
apply_pulse(servo, new_pulse, load); 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 { float ServoCluster::get_phase(uint servo) const {
assert(servo < pwms.get_chan_count()); assert(servo < pwms.get_chan_count());
return servo_phases[servo]; return servo_phases[servo];

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

@ -24,9 +24,11 @@ namespace servo {
public: 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_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, 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(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true);
~ServoCluster(); ~ServoCluster();
//-------------------------------------------------- //--------------------------------------------------
// Methods // Methods
//-------------------------------------------------- //--------------------------------------------------
@ -40,12 +42,12 @@ namespace servo {
void disable(uint servo, bool load = true); void disable(uint servo, bool load = true);
bool is_enabled(uint servo) const; 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; float get_pulse(uint servo) const;
void set_pulse(uint servo, float pulse, bool load = true); 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; float get_phase(uint servo) const;
void set_phase(uint servo, float phase, bool load = true); 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; 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 { float ServoState::get_pulse() const {
return last_enabled_pulse; return last_enabled_pulse;
} }
@ -58,6 +44,20 @@ namespace servo {
return disable(); 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 ServoState::get_min_value() const {
float value = 0.0f; float value = 0.0f;
Calibration::Point *point = table.first_point(); Calibration::Point *point = table.first_point();

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

@ -47,12 +47,12 @@ namespace servo {
private: private:
float _enable(); // Internal version of enable without convenient initialisation to mid point float _enable(); // Internal version of enable without convenient initialisation to mid point
public: public:
float get_value() const;
float set_value(float value);
float get_pulse() const; float get_pulse() const;
float set_pulse(float pulse); float set_pulse(float pulse);
float get_value() const;
float set_value(float value);
public: public:
float get_min_value() const; float get_min_value() const;
float get_mid_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_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_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_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_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_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); 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(Servo_calibration_obj, Servo_calibration);
MP_DEFINE_CONST_FUN_OBJ_1(ServoCluster___del___obj, ServoCluster___del__); 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_enable_obj, 2, ServoCluster_enable);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_disable_obj, 2, ServoCluster_disable); 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_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_value_obj, 2, ServoCluster_value);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_pulse_obj, 2, ServoCluster_pulse); 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_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_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_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_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_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_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_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_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) }, { 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_enable), MP_ROM_PTR(&Servo_enable_obj) },
{ MP_ROM_QSTR(MP_QSTR_disable), MP_ROM_PTR(&Servo_disable_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_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_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_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_min_value), MP_ROM_PTR(&Servo_min_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_mid_value), MP_ROM_PTR(&Servo_mid_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[] = { 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___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_enable), MP_ROM_PTR(&ServoCluster_enable_obj) },
{ MP_ROM_QSTR(MP_QSTR_disable), MP_ROM_PTR(&ServoCluster_disable_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_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_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_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_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_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, .locals_dict = (mp_obj_dict_t*)&ServoCluster_locals_dict,
}; };
// TODO What is this? Should remove?
typedef struct _mp_obj_float_t { typedef struct _mp_obj_float_t {
mp_obj_base_t base; mp_obj_base_t base;
mp_float_t value; 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) if(i < size - 1)
mp_print_str(print, ", "); 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, "})"); 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 *****/ /***** 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) { 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; _Calibtration_obj_t *self = nullptr;
enum { ARG_type }; 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); 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 *****/ /***** Methods *****/
mp_obj_t Calibration_create_blank_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { 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 }; 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); _Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
int index = args[ARG_index].u_int; int index = args[ARG_index].u_int;
if(index < 0 || index >= (int)self->calibration->size()) int calibration_size = (int)self->calibration->size();
mp_raise_ValueError("index out of range. Expected 0 to size()-1"); 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 { else {
Calibration::Point *point = self->calibration->point_at((uint)index); 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); _Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
int index = args[ARG_index].u_int; int index = args[ARG_index].u_int;
if(index < 0 || index >= (int)self->calibration->size()) int calibration_size = (int)self->calibration->size();
mp_raise_ValueError("index out of range. Expected 0 to size()-1"); 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 { else {
Calibration::Point *point = self->calibration->point_at((uint)index); 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"); 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) { 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_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->first_point(); Calibration::Point *point = self->calibration->first_point();
if(point == nullptr)
mp_obj_t tuple[2]; mp_raise_ValueError("this calibration does not have any points");
tuple[0] = mp_obj_new_float(point->pulse); else {
tuple[1] = mp_obj_new_float(point->value); mp_obj_t tuple[2];
return mp_obj_new_tuple(2, tuple); tuple[0] = mp_obj_new_float(point->pulse);
tuple[1] = mp_obj_new_float(point->value);
return mp_obj_new_tuple(2, tuple);
}
} }
else { else {
enum { ARG_self, ARG_point }; 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_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->first_point(); Calibration::Point *point = self->calibration->first_point();
if(point == nullptr)
const mp_obj_t object = args[ARG_point].u_obj; mp_raise_ValueError("this calibration does not have any points");
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 { 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) { 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_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->last_point(); Calibration::Point *point = self->calibration->last_point();
if(point == nullptr)
mp_obj_t tuple[2]; mp_raise_ValueError("this calibration does not have any points");
tuple[0] = mp_obj_new_float(point->pulse); else {
tuple[1] = mp_obj_new_float(point->value); mp_obj_t tuple[2];
return mp_obj_new_tuple(2, tuple); tuple[0] = mp_obj_new_float(point->pulse);
tuple[1] = mp_obj_new_float(point->value);
return mp_obj_new_tuple(2, tuple);
}
} }
else { else {
enum { ARG_self, ARG_point }; 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_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->last_point(); Calibration::Point *point = self->calibration->last_point();
if(point == nullptr)
const mp_obj_t object = args[ARG_point].u_obj; mp_raise_ValueError("this calibration does not have any points");
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 { 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) { 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); return mp_obj_new_tuple(2, tuple);
} }
else { 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; 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); return mp_obj_new_tuple(2, tuple);
} }
else { 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; 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 { typedef struct _Servo_obj_t {
mp_obj_base_t base; mp_obj_base_t base;
Servo* servo; Servo* servo;
// NOTE should this keep track of all calibration objects released?
} _Servo_obj_t; } _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, ")"); 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 *****/ /***** 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) { 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); 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 *****/ /***** Methods *****/
extern mp_obj_t Servo_pin(mp_obj_t self_in) { extern mp_obj_t Servo_pin(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t); _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; 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) { 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) { if(n_args <= 1) {
enum { ARG_self }; 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) { 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) { if(n_args <= 1) {
enum { ARG_self }; 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); 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"); 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); self->servo->to_percent(in, in_min, in_max, value_min, value_max);
} }
return mp_const_none; 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); _ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
mp_print_str(print, "ServoCluster("); mp_print_str(print, "ServoCluster(");
mp_print_str(print, "pins = {"); mp_print_str(print, "servos = {");
bool first = true; bool first = true;
uint8_t servo_count = self->cluster->get_count(); 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) { if(!first) {
mp_print_str(print, ", "); 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; first = false;
} }
mp_print_str(print, "}, freq = "); 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, ")"); 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 *****/ /***** 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) { 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; _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[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_sm, 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_pins, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_pin_count, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_type, MP_ARG_INT, {.u_int = (uint8_t)servo::CalibrationType::ANGULAR} }, { 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_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} },
{ MP_QSTR_auto_phase, MP_ARG_BOOL, {.u_bool = true} },
}; };
// Parse args. // 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; PIO pio = args[ARG_pio].u_int == 0 ? pio0 : pio1;
int sm = args[ARG_sm].u_int; 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; servo::CalibrationType calibration_type = (servo::CalibrationType)args[ARG_type].u_int;
float freq = servo::ServoState::DEFAULT_FREQUENCY; 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); 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 = m_new_obj_with_finaliser(_ServoCluster_obj_t);
self->base.type = &ServoCluster_type; 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(); self->cluster->init();
// Cleanup the pins array
if(pins != nullptr)
delete[] pins;
return MP_OBJ_FROM_PTR(self); 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 *****/ /***** 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 }; enum { ARG_self, ARG_servo };
static const mp_arg_t allowed_args[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, { 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) 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); mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count);
else 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; 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) { 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[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
}; };
// Parse args. // 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; 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) { 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) { if(n_args <= 2) {
enum { ARG_self, ARG_servo }; 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)); return mp_obj_new_float(self->cluster->get_pulse((uint)servo));
} }
else { else {
enum { ARG_self, ARG_servo, ARG_pulse }; enum { ARG_self, ARG_servo, ARG_pulse, ARG_load };
static const mp_arg_t allowed_args[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_pulse, 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. // 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); mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count);
else { else {
float pulse = mp_obj_get_float(args[ARG_pulse].u_obj); 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; 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) { 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[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
}; };
// Parse args. // 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) 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); mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count);
else else
self->cluster->to_min((uint)servo); self->cluster->to_min((uint)servo, args[ARG_servo].u_bool);
return mp_const_none; 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) { 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[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
}; };
// Parse args. // 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) 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); mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count);
else else
self->cluster->to_mid((uint)servo); self->cluster->to_mid((uint)servo, args[ARG_servo].u_bool);
return mp_const_none; 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) { 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[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_load, MP_ARG_BOOL, { .u_bool = true }},
}; };
// Parse args. // 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) 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); mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count);
else else
self->cluster->to_max((uint)servo); self->cluster->to_max((uint)servo, args[ARG_servo].u_bool);
return mp_const_none; 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) { 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) { if(n_args <= 4) {
enum { ARG_self, ARG_servo, ARG_in }; enum { ARG_self, ARG_servo, ARG_in, ARG_load };
static const mp_arg_t allowed_args[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_in, 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. // 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); mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("servo out of range. Expected 0 to %d"), servo_count);
else { else {
float in = mp_obj_get_float(args[ARG_in].u_obj); 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) { else if(n_args <= 6) {
enum { ARG_self, ARG_servo, ARG_in, ARG_in_min, ARG_in_max }; enum { ARG_self, ARG_servo, ARG_in, ARG_in_min, ARG_in_max, ARG_load };
static const mp_arg_t allowed_args[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_in, 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_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_max, 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. // 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 = mp_obj_get_float(args[ARG_in].u_obj);
float in_min = mp_obj_get_float(args[ARG_in_min].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 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 { 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[] = { static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_servo, MP_ARG_REQUIRED | MP_ARG_INT }, { 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_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_max, 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_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. // 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 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_min = mp_obj_get_float(args[ARG_value_min].u_obj);
float value_max = mp_obj_get_float(args[ARG_value_max].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; 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_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_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_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_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_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); 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_enable(mp_obj_t self_in);
extern mp_obj_t Servo_disable(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_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_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_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_min_value(mp_obj_t self_in);
extern mp_obj_t Servo_mid_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 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_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___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_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_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_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_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_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_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_mid_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);