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

Partial hardware servo support added to MP

This commit is contained in:
ZodiusInfuser 2022-02-17 17:59:09 +00:00
parent f5836e56df
commit 413ef891ce
13 changed files with 720 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
drivers/servo/calibration.cpp
View File

@ -109,7 +109,7 @@ namespace servo {
limit_upper = upper; limit_upper = upper;
} }
float Converter::min_value() { float Converter::get_min_value() {
float value = 0.0f; float value = 0.0f;
if(calibration_points >= 2) { if(calibration_points >= 2) {
value = calibration[0].value; value = calibration[0].value;
@ -117,7 +117,7 @@ namespace servo {
return value; return value;
} }
float Converter::mid_value() { float Converter::get_mid_value() {
float value = 0.0f; float value = 0.0f;
if(calibration_points >= 2) { if(calibration_points >= 2) {
value = (calibration[0].value + calibration[calibration_points - 1].value) / 2.0f; value = (calibration[0].value + calibration[calibration_points - 1].value) / 2.0f;
@ -125,7 +125,7 @@ namespace servo {
return value; return value;
} }
float Converter::max_value() { float Converter::get_max_value() {
float value = 0.0f; float value = 0.0f;
if(calibration_points >= 2) { if(calibration_points >= 2) {
value = calibration[calibration_points - 1].value; value = calibration[calibration_points - 1].value;

6
drivers/servo/calibration.hpp
View File

@ -108,9 +108,9 @@ namespace servo {
// Methods // Methods
//-------------------------------------------------- //--------------------------------------------------
public: public:
float min_value(); float get_min_value();
float mid_value(); float get_mid_value();
float max_value(); float get_max_value();
float value_to_pulse(float value); float value_to_pulse(float value);
float value_from_pulse(float pulse); float value_from_pulse(float pulse);

25
drivers/servo/servo.cpp
View File

@ -24,8 +24,8 @@ namespace servo {
return true; return true;
} }
bool Servo::is_enabled() { uint Servo::get_pin() {
return state.is_enabled(); return pin;
} }
void Servo::enable() { void Servo::enable() {
@ -38,6 +38,10 @@ namespace servo {
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000)); pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
} }
bool Servo::is_enabled() {
return state.is_enabled();
}
float Servo::get_value() { float Servo::get_value() {
return state.get_value(); return state.get_value();
} }
@ -71,6 +75,11 @@ namespace servo {
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000)); pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
} }
void Servo::to_percent(float in) {
float new_pulse = state.to_percent(in, 0.0f, 1.0f);
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
void Servo::to_percent(float in, float in_min, float in_max) { void Servo::to_percent(float in, float in_min, float in_max) {
float new_pulse = state.to_percent(in, in_min, in_max); float new_pulse = state.to_percent(in, in_min, in_max);
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000)); pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
@ -81,6 +90,18 @@ namespace servo {
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000)); pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
} }
float Servo::get_min_value() {
return state.get_min_value();
}
float Servo::get_mid_value() {
return state.get_mid_value();
}
float Servo::get_max_value() {
return state.get_max_value();
}
Calibration& Servo::calibration() { Calibration& Servo::calibration() {
return state.calibration(); return state.calibration();
} }

12
drivers/servo/servo.hpp
View File

@ -51,9 +51,12 @@ namespace servo {
public: public:
bool init(); bool init();
bool is_enabled(); // For print access in micropython
uint get_pin();
void enable(); void enable();
void disable(); void disable();
bool is_enabled();
float get_value(); float get_value();
void set_value(float value); void set_value(float value);
@ -64,9 +67,14 @@ namespace servo {
void to_min(); void to_min();
void to_mid(); void to_mid();
void to_max(); void to_max();
void to_percent(float in, float in_min = 0.0f, float in_max = 1.0f); void to_percent(float in);
void to_percent(float in, float in_min, float in_max);
void to_percent(float in, float in_min, float in_max, float value_min, float value_max); void to_percent(float in, float in_min, float in_max, float value_min, float value_max);
float get_min_value();
float get_mid_value();
float get_max_value();
Calibration& calibration(); Calibration& calibration();
}; };

14
drivers/servo/servo_cluster.cpp
View File

@ -24,13 +24,6 @@ namespace servo {
return true; return true;
} }
bool ServoCluster::is_enabled(uint servo) {
if(servo < NUM_BANK0_GPIOS)
return servos[servo].is_enabled();
else
return false;
}
void ServoCluster::enable(uint servo, bool load) { void ServoCluster::enable(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) { if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].enable(); float new_pulse = servos[servo].enable();
@ -45,6 +38,13 @@ namespace servo {
} }
} }
bool ServoCluster::is_enabled(uint servo) {
if(servo < NUM_BANK0_GPIOS)
return servos[servo].is_enabled();
else
return false;
}
float ServoCluster::get_value(uint servo) { float ServoCluster::get_value(uint servo) {
if(servo < NUM_BANK0_GPIOS) if(servo < NUM_BANK0_GPIOS)
return servos[servo].get_value(); return servos[servo].get_value();

2
drivers/servo/servo_cluster.hpp
View File

@ -47,9 +47,9 @@ namespace servo {
public: public:
bool init(); bool init();
bool is_enabled(uint servo);
void enable(uint servo, bool load = true); void enable(uint servo, bool load = true);
void disable(uint servo, bool load = true); void disable(uint servo, bool load = true);
bool is_enabled(uint servo);
float get_value(uint servo); float get_value(uint servo);
void set_value(uint servo, float value, bool load = true); void set_value(uint servo, float value, bool load = true);

30
drivers/servo/servo_state.cpp
View File

@ -5,13 +5,9 @@ namespace servo {
: /*pin(pin), */converter(type) { : /*pin(pin), */converter(type) {
} }
bool ServoState::is_enabled() {
return enabled;
}
float ServoState::enable() { float ServoState::enable() {
if(last_enabled_pulse < Converter::MIN_VALID_PULSE) { if(last_enabled_pulse < Converter::MIN_VALID_PULSE) {
servo_value = converter.mid_value(); servo_value = converter.get_mid_value();
last_enabled_pulse = converter.value_to_pulse(servo_value); last_enabled_pulse = converter.value_to_pulse(servo_value);
} }
enabled = true; enabled = true;
@ -23,6 +19,10 @@ namespace servo {
return 0.0f; // A zero pulse return 0.0f; // A zero pulse
} }
bool ServoState::is_enabled() {
return enabled;
}
float ServoState::get_value() { float ServoState::get_value() {
return servo_value; return servo_value;
} }
@ -57,19 +57,19 @@ namespace servo {
} }
float ServoState::to_min() { float ServoState::to_min() {
return set_value(converter.min_value()); return set_value(converter.get_min_value());
} }
float ServoState::to_mid() { float ServoState::to_mid() {
return set_value(converter.mid_value()); return set_value(converter.get_mid_value());
} }
float ServoState::to_max() { float ServoState::to_max() {
return set_value(converter.max_value()); return set_value(converter.get_max_value());
} }
float ServoState::to_percent(float in, float in_min, float in_max) { float ServoState::to_percent(float in, float in_min, float in_max) {
float value = Converter::map_float(in, in_min, in_max, converter.min_value(), converter.max_value()); float value = Converter::map_float(in, in_min, in_max, converter.get_min_value(), converter.get_max_value());
return set_value(value); return set_value(value);
} }
@ -78,6 +78,18 @@ namespace servo {
return set_value(value); return set_value(value);
} }
float ServoState::get_min_value() {
return converter.get_min_value();
}
float ServoState::get_mid_value() {
return converter.get_mid_value();
}
float ServoState::get_max_value() {
return converter.get_max_value();
}
Calibration& ServoState::calibration() { Calibration& ServoState::calibration() {
return converter; return converter;
} }

6
drivers/servo/servo_state.hpp
View File

@ -35,9 +35,9 @@ namespace servo {
public: public:
bool init(); bool init();
bool is_enabled();
float enable(); float enable();
float disable(); float disable();
bool is_enabled();
float get_value(); float get_value();
float set_value(float value); float set_value(float value);
@ -51,6 +51,10 @@ namespace servo {
float to_percent(float in, float in_min, float in_max); float to_percent(float in, float in_min, float in_max);
float to_percent(float in, float in_min, float in_max, float value_min, float value_max); float to_percent(float in, float in_min, float in_max, float value_min, float value_max);
float get_min_value();
float get_mid_value();
float get_max_value();
Calibration& calibration(); Calibration& calibration();
}; };

1
micropython/modules/micropython.cmake
View File

@ -40,6 +40,7 @@ include(pico_wireless/micropython)
include(plasma/micropython) include(plasma/micropython)
include(hub75/micropython) include(hub75/micropython)
include(servo/micropython)
include(ulab/code/micropython) include(ulab/code/micropython)
include(qrcode/micropython/micropython) include(qrcode/micropython/micropython)

25
micropython/modules/servo/micropython.cmake Normal file
View File

@ -0,0 +1,25 @@
set(MOD_NAME servo)
string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER)
add_library(usermod_${MOD_NAME} INTERFACE)
target_sources(usermod_${MOD_NAME} INTERFACE
${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.c
${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/servo.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/servo_cluster.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/servo_state.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/calibration.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/multi_pwm.cpp
)
pico_generate_pio_header(usermod_${MOD_NAME} ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/multi_pwm.pio)
target_include_directories(usermod_${MOD_NAME} INTERFACE
${CMAKE_CURRENT_LIST_DIR}
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/servo/
)
target_compile_definitions(usermod_${MOD_NAME} INTERFACE
MODULE_SERVO_ENABLED=1
)
target_link_libraries(usermod INTERFACE usermod_${MOD_NAME})

118
micropython/modules/servo/servo.c Normal file
View File

@ -0,0 +1,118 @@
#include "servo.h"
/***** Methods *****/
MP_DEFINE_CONST_FUN_OBJ_1(Servo___del___obj, Servo___del__);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_enable_obj, Servo_enable);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_disable_obj, Servo_disable);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_is_enabled_obj, Servo_is_enabled);
MP_DEFINE_CONST_FUN_OBJ_KW(Servo_value_obj, 1, Servo_value);
MP_DEFINE_CONST_FUN_OBJ_KW(Servo_pulse_obj, 1, Servo_pulse);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_to_min_obj, Servo_to_min);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_to_mid_obj, Servo_to_mid);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_to_max_obj, Servo_to_max);
MP_DEFINE_CONST_FUN_OBJ_KW(Servo_to_percent_obj, 1, Servo_to_percent);
MP_DEFINE_CONST_FUN_OBJ_1(ServoCluster___del___obj, ServoCluster___del__);
// MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_set_rgb_obj, 5, ServoCluster_set_rgb);
// MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_set_hsv_obj, 3, ServoCluster_set_hsv);
// MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_set_brightness_obj, 2, ServoCluster_set_brightness);
// MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_start_obj, 1, ServoCluster_start);
// MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_get_obj, 2, ServoCluster_get);
// MP_DEFINE_CONST_FUN_OBJ_1(ServoCluster_clear_obj, ServoCluster_clear);
// MP_DEFINE_CONST_FUN_OBJ_1(ServoCluster_update_obj, ServoCluster_update);
/***** Binding of Methods *****/
STATIC const mp_rom_map_elem_t Servo_locals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&Servo___del___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_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_to_min), MP_ROM_PTR(&Servo_to_min_obj) },
{ MP_ROM_QSTR(MP_QSTR_to_mid), MP_ROM_PTR(&Servo_to_mid_obj) },
{ MP_ROM_QSTR(MP_QSTR_to_max), MP_ROM_PTR(&Servo_to_max_obj) },
{ MP_ROM_QSTR(MP_QSTR_to_percent), MP_ROM_PTR(&Servo_to_percent_obj) },
};
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_set_rgb), MP_ROM_PTR(&ServoCluster_set_rgb_obj) },
// { MP_ROM_QSTR(MP_QSTR_set_hsv), MP_ROM_PTR(&ServoCluster_set_hsv_obj) },
// { MP_ROM_QSTR(MP_QSTR_set_brightness), MP_ROM_PTR(&ServoCluster_set_brightness_obj) },
// { MP_ROM_QSTR(MP_QSTR_start), MP_ROM_PTR(&ServoCluster_start_obj) },
// { MP_ROM_QSTR(MP_QSTR_get), MP_ROM_PTR(&ServoCluster_get_obj) },
// { MP_ROM_QSTR(MP_QSTR_clear), MP_ROM_PTR(&ServoCluster_clear_obj) },
// { MP_ROM_QSTR(MP_QSTR_update), MP_ROM_PTR(&ServoCluster_update_obj) },
};
STATIC MP_DEFINE_CONST_DICT(Servo_locals_dict, Servo_locals_dict_table);
STATIC MP_DEFINE_CONST_DICT(ServoCluster_locals_dict, ServoCluster_locals_dict_table);
/***** Class Definition *****/
const mp_obj_type_t Servo_type = {
{ &mp_type_type },
.name = MP_QSTR_servo,
.print = Servo_print,
.make_new = Servo_make_new,
.locals_dict = (mp_obj_dict_t*)&Servo_locals_dict,
};
const mp_obj_type_t ServoCluster_type = {
{ &mp_type_type },
.name = MP_QSTR_servo_cluster,
.print = ServoCluster_print,
.make_new = ServoCluster_make_new,
.locals_dict = (mp_obj_dict_t*)&ServoCluster_locals_dict,
};
typedef struct _mp_obj_float_t {
mp_obj_base_t base;
mp_float_t value;
} mp_obj_float_t;
mp_obj_float_t servo2040_shunt_resistor = {{&mp_type_float}, 0.015f};
/***** Globals Table *****/
STATIC const mp_map_elem_t servo2040_globals_table[] = {
{ MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_servo2040) },
{ MP_ROM_QSTR(MP_QSTR_LED_R), MP_ROM_INT(16) },
{ MP_ROM_QSTR(MP_QSTR_LED_G), MP_ROM_INT(17) },
{ MP_ROM_QSTR(MP_QSTR_LED_B), MP_ROM_INT(18) },
{ MP_ROM_QSTR(MP_QSTR_BUTTON_A), MP_ROM_INT(12) },
{ MP_ROM_QSTR(MP_QSTR_BUTTON_B), MP_ROM_INT(13) },
{ MP_ROM_QSTR(MP_QSTR_USER_SW), MP_ROM_INT(23) },
{ MP_ROM_QSTR(MP_QSTR_CLK), MP_ROM_INT(14) },
{ MP_ROM_QSTR(MP_QSTR_DAT), MP_ROM_INT(15) },
{ MP_ROM_QSTR(MP_QSTR_CURRENT_SENSE), MP_ROM_INT(29) },
{ MP_ROM_QSTR(MP_QSTR_SDA), MP_ROM_INT(20) },
{ MP_ROM_QSTR(MP_QSTR_SCL), MP_ROM_INT(21) },
{ MP_ROM_QSTR(MP_QSTR_SHUNT_RESISTOR), MP_ROM_PTR(&servo2040_shunt_resistor) },
{ MP_ROM_QSTR(MP_QSTR_ADC_GAIN), MP_ROM_INT(50) },
};
STATIC MP_DEFINE_CONST_DICT(mp_module_servo2040_globals, servo2040_globals_table);
const mp_obj_module_t servo2040_user_cmodule = {
.base = { &mp_type_module },
.globals = (mp_obj_dict_t*)&mp_module_servo2040_globals,
};
STATIC const mp_map_elem_t servo_globals_table[] = {
{ MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_servo) },
{ MP_OBJ_NEW_QSTR(MP_QSTR_Servo), (mp_obj_t)&Servo_type },
{ MP_OBJ_NEW_QSTR(MP_QSTR_ServoCluster), (mp_obj_t)&ServoCluster_type },
{ MP_OBJ_NEW_QSTR(MP_QSTR_servo2040), (mp_obj_t)&servo2040_user_cmodule },
{ MP_ROM_QSTR(MP_QSTR_ANGULAR), MP_ROM_INT(0x00) },
{ MP_ROM_QSTR(MP_QSTR_LINEAR), MP_ROM_INT(0x01) },
{ MP_ROM_QSTR(MP_QSTR_CONTINUOUS), MP_ROM_INT(0x02) },
};
STATIC MP_DEFINE_CONST_DICT(mp_module_servo_globals, servo_globals_table);
/***** Module Definition *****/
const mp_obj_module_t servo_user_cmodule = {
.base = { &mp_type_module },
.globals = (mp_obj_dict_t*)&mp_module_servo_globals,
};
MP_REGISTER_MODULE(MP_QSTR_servo, servo_user_cmodule, MODULE_SERVO_ENABLED);

470
micropython/modules/servo/servo.cpp Normal file
View File

@ -0,0 +1,470 @@
#include "drivers/servo/servo.hpp"
#include "drivers/servo/servo_cluster.hpp"
#include <cstdio>
#define MP_OBJ_TO_PTR2(o, t) ((t *)(uintptr_t)(o))
// SDA/SCL on even/odd pins, I2C0/I2C1 on even/odd pairs of pins.
#define IS_VALID_SCL(i2c, pin) (((pin) & 1) == 1 && (((pin) & 2) >> 1) == (i2c))
#define IS_VALID_SDA(i2c, pin) (((pin) & 1) == 0 && (((pin) & 2) >> 1) == (i2c))
using namespace servo;
extern "C" {
#include "servo.h"
#include "py/builtin.h"
typedef struct _mp_obj_float_t {
mp_obj_base_t base;
mp_float_t value;
} mp_obj_float_t;
const mp_obj_float_t const_float_1 = {{&mp_type_float}, 1.0f};
/********** Servo **********/
/***** Variables Struct *****/
typedef struct _Servo_obj_t {
mp_obj_base_t base;
Servo* servo;
} _Servo_obj_t;
/***** Print *****/
void Servo_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
(void)kind; //Unused input parameter
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
mp_print_str(print, "Servo(");
mp_print_str(print, "pin = ");
mp_obj_print_helper(print, mp_obj_new_int(self->servo->get_pin()), PRINT_REPR);
mp_print_str(print, ", value = ");
mp_obj_print_helper(print, mp_obj_new_float(self->servo->get_value()), PRINT_REPR);
mp_print_str(print, ", pulse = ");
mp_obj_print_helper(print, mp_obj_new_float(self->servo->get_pulse()), PRINT_REPR);
mp_print_str(print, ", enabled = ");
mp_obj_print_helper(print, self->servo->is_enabled() ? mp_const_true : mp_const_false, PRINT_REPR);
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) {
_Servo_obj_t *self = nullptr;
enum { ARG_pin, ARG_servo_type };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_pin, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_type, MP_ARG_INT, {.u_int = (uint8_t)servo::Type::ANGULAR} },
};
// Parse args.
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
int pin = args[ARG_pin].u_int;
servo::Type servo_type = (servo::Type)args[ARG_servo_type].u_int;
self = m_new_obj_with_finaliser(_Servo_obj_t);
self->base.type = &Servo_type;
self->servo = new Servo(pin, servo_type);
self->servo->init();
return MP_OBJ_FROM_PTR(self);
}
extern mp_obj_t Servo_enable(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
self->servo->enable();
return mp_const_none;
}
extern mp_obj_t Servo_disable(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
self->servo->disable();
return mp_const_none;
}
extern mp_obj_t Servo_is_enabled(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
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 };
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_pulse());
}
else {
enum { ARG_self, ARG_pulse };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_pulse, 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 pulse = mp_obj_get_float(args[ARG_pulse].u_obj);
self->servo->set_pulse(pulse);
return mp_const_none;
}
}
extern mp_obj_t Servo_to_min(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
self->servo->to_min();
return mp_const_none;
}
extern mp_obj_t Servo_to_mid(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
self->servo->to_mid();
return mp_const_none;
}
extern mp_obj_t Servo_to_max(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
self->servo->to_max();
return mp_const_none;
}
extern mp_obj_t Servo_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_in };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in, 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 in = mp_obj_get_float(args[ARG_in].u_obj);
self->servo->to_percent(in);
}
else if(n_args <= 4) {
enum { ARG_self, ARG_in, ARG_in_min, ARG_in_max };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ },
};
// 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 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->servo->to_percent(in, in_min, in_max);
}
else {
enum { ARG_self, ARG_in, ARG_in_min, ARG_in_max, ARG_value_min, ARG_value_max };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_in_max, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_value_min, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_value_max, 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 in = mp_obj_get_float(args[ARG_in].u_obj);
float in_min = mp_obj_get_float(args[ARG_in_min].u_obj);
float in_max = mp_obj_get_float(args[ARG_in_max].u_obj);
float value_min = mp_obj_get_float(args[ARG_value_min].u_obj);
float value_max = mp_obj_get_float(args[ARG_value_max].u_obj);
self->servo->to_percent(in, in_min, in_max, value_min, value_max);
}
return mp_const_none;
}
/********** ServoCluster **********/
/***** Variables Struct *****/
typedef struct _ServoCluster_obj_t {
mp_obj_base_t base;
ServoCluster* cluster;
} _ServoCluster_obj_t;
/***** Print *****/
void ServoCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {
(void)kind; //Unused input parameter
//_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
mp_print_str(print, "ServoCluster(");
// TODO
//mp_print_str(print, "num_leds = ");
//mp_obj_print_helper(print, mp_obj_new_int(self->apa102->num_leds), PRINT_REPR);
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;
// TODO
/*enum {
ARG_num_leds,
ARG_pio,
ARG_sm,
ARG_dat,
ARG_clk,
ARG_freq,
ARG_buffer
};
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_num_leds, 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_dat, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_clk, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_freq, MP_ARG_INT, {.u_int = APA102::DEFAULT_SERIAL_FREQ} },
{ MP_QSTR_buffer, MP_ARG_OBJ, {.u_obj = nullptr} },
};
// Parse args.
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
int num_leds = args[ARG_num_leds].u_int;
PIO pio = args[ARG_pio].u_int == 0 ? pio0 : pio1;
int sm = args[ARG_sm].u_int;
int dat = args[ARG_dat].u_int;
int clk = args[ARG_clk].u_int;
int freq = args[ARG_freq].u_int;
APA102::RGB *buffer = nullptr;
if (args[ARG_buffer].u_obj) {
mp_buffer_info_t bufinfo;
mp_get_buffer_raise(args[ARG_buffer].u_obj, &bufinfo, MP_BUFFER_RW);
buffer = (APA102::RGB *)bufinfo.buf;
if(bufinfo.len < (size_t)(num_leds * 4)) {
mp_raise_ValueError("Supplied buffer is too small for LED count!");
}
// If a bytearray is supplied it'll be raw, uninitialized bytes
// iterate through the RGB elements and call "brightness"
// to set up the SOF bytes, otherwise a flickery mess will happen!
// Oh for such niceties as "placement new"...
for(auto i = 0; i < num_leds; i++) {
buffer[i].brightness(15);
}
}*/
self = m_new_obj_with_finaliser(_ServoCluster_obj_t);
self->base.type = &ServoCluster_type;
self->cluster = new ServoCluster(pio1, 0, 0b11111100); //TODO Expose parameters
return MP_OBJ_FROM_PTR(self);
}
/*
mp_obj_t ServoCluster_clear(mp_obj_t self_in) {
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
self->apa102->clear();
return mp_const_none;
}
mp_obj_t ServoCluster_update(mp_obj_t self_in) {
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(self_in, _ServoCluster_obj_t);
self->apa102->update(true);
return mp_const_none;
}
mp_obj_t ServoCluster_start(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_fps };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_fps, MP_ARG_INT, {.u_int = 60} }
};
// 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);
self->apa102->start(args[ARG_fps].u_int);
return mp_const_none;
}
mp_obj_t ServoCluster_set_brightness(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_brightness };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_brightness, 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);
int brightness = args[ARG_brightness].u_int;
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
self->apa102->set_brightness(brightness);
return mp_const_none;
}
mp_obj_t ServoCluster_set_rgb(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_index, ARG_r, ARG_g, ARG_b };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_index, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_r, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_g, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_b, 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);
int index = args[ARG_index].u_int;
int r = args[ARG_r].u_int;
int g = args[ARG_g].u_int;
int b = args[ARG_b].u_int;
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
self->apa102->set_rgb(index, r, g, b);
return mp_const_none;
}
mp_obj_t ServoCluster_set_hsv(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_index, ARG_h, ARG_s, ARG_v };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_index, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_hue, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_sat, MP_ARG_OBJ, {.u_rom_obj = MP_ROM_PTR(&const_float_1)} },
{ MP_QSTR_val, MP_ARG_OBJ, {.u_rom_obj = MP_ROM_PTR(&const_float_1)} }
};
// 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);
int index = args[ARG_index].u_int;
float h = mp_obj_get_float(args[ARG_h].u_obj);
float s = mp_obj_get_float(args[ARG_s].u_obj);
float v = mp_obj_get_float(args[ARG_v].u_obj);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
self->apa102->set_hsv(index, h, s, v);
return mp_const_none;
}
mp_obj_t ServoCluster_get(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_index };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_index, MP_ARG_REQUIRED | MP_ARG_INT },
};
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);
int index = args[ARG_index].u_int;
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
APA102::RGB rgb = self->apa102->get(index);
mp_obj_t tuple[4];
tuple[0] = mp_obj_new_int(rgb.r);
tuple[1] = mp_obj_new_int(rgb.g);
tuple[2] = mp_obj_new_int(rgb.b);
tuple[3] = mp_obj_new_int(rgb.sof);
return mp_obj_new_tuple(4, tuple);
}
*/
}

33
micropython/modules/servo/servo.h Normal file
View File

@ -0,0 +1,33 @@
// Include MicroPython API.
#include "py/runtime.h"
/***** Extern of Class Definition *****/
extern const mp_obj_type_t Servo_type;
extern const mp_obj_type_t ServoCluster_type;
/***** Extern of Class Methods *****/
extern void Servo_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind);
extern 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);
extern mp_obj_t Servo___del__(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_to_min(mp_obj_t self_in);
extern mp_obj_t Servo_to_mid(mp_obj_t self_in);
extern mp_obj_t Servo_to_max(mp_obj_t self_in);
extern mp_obj_t Servo_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
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_start(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
// extern mp_obj_t ServoCluster_set_rgb(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
// extern mp_obj_t ServoCluster_set_hsv(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
// extern mp_obj_t ServoCluster_set_brightness(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
// extern mp_obj_t ServoCluster_get(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
// extern mp_obj_t ServoCluster_clear(mp_obj_t self_in);
// extern mp_obj_t ServoCluster_update(mp_obj_t self_in);
extern bool Pimoroni_mp_obj_to_i2c(mp_obj_t in, void *out);