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

Improvements to calibration and a calib example

servo-pio
ZodiusInfuser 2022-03-21 23:51:15 +00:00
rodzic 676c54aaed
commit 09e69ff8d9
12 zmienionych plików z 606 dodań i 102 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

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

@ -2,27 +2,26 @@
namespace servo {
Calibration::Point::Point()
: pulse(0.0f), value(0.0f) {
: pulse(0.0f), value(0.0f) {
}
Calibration::Point::Point(float pulse, float value)
: pulse(pulse), value(value) {
: pulse(pulse), value(value) {
}
Calibration::Calibration()
: calibration(nullptr), calibration_size(0), limit_lower(true), limit_upper(true) {
create_default_calibration(ANGULAR);
}
Calibration::Calibration(CalibrationType default_type)
: calibration(nullptr), calibration_size(0), limit_lower(true), limit_upper(true) {
create_default_calibration(default_type);
: Calibration() {
apply_default(default_type);
}
Calibration::Calibration(const Calibration &other)
: calibration(nullptr), calibration_size(0), limit_lower(other.limit_lower), limit_upper(other.limit_upper) {
uint size = other.size();
create_blank_calibration(size);
apply_blank(size);
for(uint i = 0; i < size; i++) {
calibration[i] = other.calibration[i];
}
@ -37,7 +36,7 @@ namespace servo {
Calibration& Calibration::operator=(const Calibration &other) {
uint size = other.size();
create_blank_calibration(size);
apply_blank(size);
for(uint i = 0; i < size; i++) {
calibration[i] = other.calibration[i];
}
@ -47,8 +46,11 @@ namespace servo {
return *this;
}
Calibration::Point& Calibration::operator[](uint8_t index) const {
return calibration[index];
}
void Calibration::create_blank_calibration(uint size) {
void Calibration::apply_blank(uint size) {
if(calibration != nullptr) {
delete[] calibration;
}
@ -63,21 +65,21 @@ namespace servo {
}
}
void Calibration::create_two_point_calibration(float min_pulse, float max_pulse, float min_value, float max_value) {
create_blank_calibration(2);
void Calibration::apply_two_point(float min_pulse, float max_pulse, float min_value, float max_value) {
apply_blank(2);
calibration[0] = Point(min_pulse, min_value);
calibration[1] = Point(max_pulse, max_value);
}
void Calibration::create_three_point_calibration(float min_pulse, float mid_pulse, float max_pulse, float min_value, float mid_value, float max_value) {
create_blank_calibration(3);
void Calibration::apply_three_point(float min_pulse, float mid_pulse, float max_pulse, float min_value, float mid_value, float max_value) {
apply_blank(3);
calibration[0] = Point(min_pulse, min_value);
calibration[1] = Point(mid_pulse, mid_value);
calibration[2] = Point(max_pulse, max_value);
}
void Calibration::create_uniform_calibration(uint size, float min_pulse, float max_pulse, float min_value, float max_value) {
create_blank_calibration(size);
void Calibration::apply_uniform(uint size, float min_pulse, float max_pulse, float min_value, float max_value) {
apply_blank(size);
if(size > 0) {
float size_minus_one = (float)(size - 1);
for(uint i = 0; i < size; i++) {
@ -88,23 +90,21 @@ namespace servo {
}
}
void Calibration::create_default_calibration(CalibrationType default_type) {
void Calibration::apply_default(CalibrationType default_type) {
switch(default_type) {
default:
case ANGULAR:
create_three_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
-90.0f, 0.0f, +90.0f);
apply_three_point(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
-90.0f, 0.0f, +90.0f);
break;
case LINEAR:
create_two_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MAX_PULSE,
0.0f, 1.0f);
apply_two_point(DEFAULT_MIN_PULSE, DEFAULT_MAX_PULSE,
0.0f, 1.0f);
break;
case CONTINUOUS:
create_three_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
-1.0f, 0.0f, +1.0f);
apply_three_point(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
-1.0f, 0.0f, +1.0f);
break;
case EMPTY:
create_blank_calibration(0);
}
}

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

@ -7,8 +7,7 @@ namespace servo {
enum CalibrationType {
ANGULAR = 0,
LINEAR,
CONTINUOUS,
EMPTY
CONTINUOUS
};
class Calibration {
@ -60,17 +59,18 @@ namespace servo {
//--------------------------------------------------
public:
Calibration& operator=(const Calibration &other);
Point& operator[](uint8_t index) const;
//--------------------------------------------------
// Methods
//--------------------------------------------------
public:
void create_blank_calibration(uint size);
void create_two_point_calibration(float min_pulse, float max_pulse, float min_value, float max_value);
void create_three_point_calibration(float min_pulse, float mid_pulse, float max_pulse, float min_value, float mid_value, float max_value);
void create_uniform_calibration(uint size, float min_pulse, float max_pulse, float min_value, float max_value);
void create_default_calibration(CalibrationType default_type);
void apply_blank(uint size);
void apply_two_point(float min_pulse, float max_pulse, float min_value, float max_value);
void apply_three_point(float min_pulse, float mid_pulse, float max_pulse, float min_value, float mid_value, float max_value);
void apply_uniform(uint size, float min_pulse, float max_pulse, float min_value, float max_value);
void apply_default(CalibrationType default_type);
uint size() const;
Point* point_at(uint8_t index) const; // Ensure the points are assigned in ascending value order

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

@ -7,6 +7,10 @@ namespace servo {
: pin(pin), state(type), pwm_frequency(freq) {
}
Servo::Servo(uint pin, const Calibration& calibration, float freq)
: pin(pin), state(calibration), pwm_frequency(freq) {
}
Servo::~Servo() {
gpio_set_function(pin, GPIO_FUNC_NULL);
}

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

@ -23,6 +23,7 @@ namespace servo {
//--------------------------------------------------
public:
Servo(uint pin, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY);
Servo(uint pin, const Calibration& calibration, float freq = ServoState::DEFAULT_FREQUENCY);
~Servo();
//--------------------------------------------------

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

@ -23,6 +23,26 @@ namespace servo {
create_servo_states(default_type, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
: pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_servo_states(calibration, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
: pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_servo_states(calibration, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
: pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_servo_states(calibration, auto_phase);
}
ServoCluster::ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
: pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_servo_states(calibration, auto_phase);
}
ServoCluster::~ServoCluster() {
delete[] states;
delete[] servo_phases;
@ -491,4 +511,17 @@ namespace servo {
}
}
}
void ServoCluster::create_servo_states(const Calibration& calibration, bool auto_phase) {
uint8_t servo_count = pwms.get_chan_count();
if(servo_count > 0) {
states = new ServoState[servo_count];
servo_phases = new float[servo_count];
for(uint servo = 0; servo < servo_count; servo++) {
states[servo] = ServoState(calibration);
servo_phases[servo] = (auto_phase) ? (float)servo / (float)servo_count : 0.0f;
}
}
}
};

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

@ -28,6 +28,11 @@ namespace servo {
ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
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, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type = ANGULAR, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
ServoCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
ServoCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
ServoCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
ServoCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, const Calibration& calibration, float freq = ServoState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
~ServoCluster();
@ -112,6 +117,7 @@ namespace servo {
private:
void apply_pulse(uint servo, float pulse, bool load);
void create_servo_states(CalibrationType default_type, bool auto_phase);
void create_servo_states(const Calibration& calibration, bool auto_phase);
};
}

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

@ -1,8 +1,15 @@
#include "servo_state.hpp"
namespace servo {
ServoState::ServoState() {
}
ServoState::ServoState(CalibrationType default_type)
: table(default_type) {
: calib(default_type) {
}
ServoState::ServoState(const Calibration& calibration)
: calib(calibration) {
}
float ServoState::enable() {
@ -35,7 +42,7 @@ namespace servo {
float ServoState::set_pulse(float pulse) {
if(pulse >= MIN_VALID_PULSE) {
float value_out, pulse_out;
if(table.pulse_to_value(pulse, value_out, pulse_out)) {
if(calib.pulse_to_value(pulse, value_out, pulse_out)) {
servo_value = value_out;
last_enabled_pulse = pulse_out;
return _enable();
@ -50,7 +57,7 @@ namespace servo {
float ServoState::set_value(float value) {
float pulse_out, value_out;
if(table.value_to_pulse(value, pulse_out, value_out)) {
if(calib.value_to_pulse(value, pulse_out, value_out)) {
last_enabled_pulse = pulse_out;
servo_value = value_out;
return _enable();
@ -60,7 +67,7 @@ namespace servo {
float ServoState::get_min_value() const {
float value = 0.0f;
Calibration::Point *point = table.first_point();
Calibration::Point *point = calib.first_point();
if(point != nullptr) {
value = point->value;
}
@ -69,8 +76,8 @@ namespace servo {
float ServoState::get_mid_value() const {
float value = 0.0f;
Calibration::Point *first = table.first_point();
Calibration::Point *last = table.last_point();
Calibration::Point *first = calib.first_point();
Calibration::Point *last = calib.last_point();
if((first != nullptr) && (last != nullptr)) {
value = (first->value + last->value) / 2.0f;
}
@ -79,7 +86,7 @@ namespace servo {
float ServoState::get_max_value() const {
float value = 0.0f;
Calibration::Point *point = table.last_point();
Calibration::Point *point = calib.last_point();
if(point != nullptr) {
value = point->value;
}
@ -109,11 +116,11 @@ namespace servo {
}
Calibration& ServoState::calibration() {
return table;
return calib;
}
const Calibration& ServoState::calibration() const {
return table;
return calib;
}
uint32_t ServoState::pulse_to_level(float pulse, uint32_t resolution, float freq) {

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

@ -28,14 +28,17 @@ namespace servo {
float servo_value = 0.0f;
float last_enabled_pulse = 0.0f;
bool enabled = false;
Calibration table;
Calibration calib;
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
public:
ServoState(CalibrationType default_type = ANGULAR);
ServoState();
ServoState(CalibrationType default_type);
ServoState(const Calibration& calibration);
//--------------------------------------------------
// Methods

82
micropython/examples/servo2040/calibration.py 100644
Wyświetl plik

@ -0,0 +1,82 @@
from servo import *
# --------------------------------------------------
# An example of how to create and modify
# the calibration of an angular servo
# --------------------------------------------------
# Create an angular servo on pin 0. By default its value ranges from -90 to +90
angular_servo = Servo(servo2040.SERVO_1)
# Access its calibration and print it out
cal = angular_servo.calibration()
print("Angular Servo:", cal, end="\n\n")
WIDE_ANGLE_RANGE = 270 # The range we want the anglular servo to cover
# Lets modify the calibration to increase its range
cal.first_value(-WIDE_ANGLE_RANGE / 2)
cal.last_value(WIDE_ANGLE_RANGE / 2)
# Now apply the modified calibration to the servo and confirm it worked
angular_servo.calibration(cal)
print("Wide Angle Servo:", angular_servo.calibration(), end="\n\n")
# --------------------------------------------------
# An example of how to create and modify
# the calibration of a linear servo
# --------------------------------------------------
LINEAR_RANGE = 50 # The range we want the linear servo to cover
# Create a linear servo on pin 1. By default its value ranges from 0.0 to 1.0
linear_servo = Servo(servo2040.SERVO_2, LINEAR)
# Update the linear servo so its max value matches the real distance travelled
cal = linear_servo.calibration()
cal.last_value(LINEAR_RANGE)
# Apply the modified calibration to the servo and confirm it worked
linear_servo.calibration(cal)
print("Linear Servo:", linear_servo.calibration(), end="\n\n")
# --------------------------------------------------
# An example of how to create and modify the
# calibration of a continuous rotation servo
# --------------------------------------------------
CONTINUOUS_SPEED = 10 # The speed we want the continuous servo to cover
# Create a continous rotation servo on pin 2. By default its value ranges from -1.0 to +1.0
continuous_servo = Servo(servo2040.SERVO_3, CONTINUOUS)
# Update the continuous rotation servo so its value matches its real speed
cal = continuous_servo.calibration()
cal.first_value(-CONTINUOUS_SPEED)
cal.last_value(CONTINUOUS_SPEED)
# Apply the modified calibration to the servo and confirm it worked
continuous_servo.calibration(cal)
print("Continuous Servo:", continuous_servo.calibration(), end="\n\n")
# --------------------------------------------------
# An example of how to create a custom
# calibration and build a servo using it
# --------------------------------------------------
# Create an empty calibration
cal = Calibration()
# Give it a range of -45 to 45 degrees, corresponding to pulses of 1000 and 2000 microseconds
cal.apply_two_point(1000, 2000, -45, 45)
# Turn off the lower and upper limits, so the servo can go beyond 45 degrees
cal.limit_to_calibration(False, False)
# Create a servo on pin 3 using the custom calibration and confirmed it worked
custom_servo = Servo(servo2040.SERVO_4, cal)
print("Custom Servo:", custom_servo.calibration(), end="\n\n")

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

@ -3,15 +3,21 @@
/***** Methods *****/
MP_DEFINE_CONST_FUN_OBJ_1(Calibration___del___obj, Calibration___del__);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_create_blank_calibration_obj, 2, Calibration_create_blank_calibration);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_create_two_point_calibration_obj, 5, Calibration_create_two_point_calibration);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_create_three_point_calibration_obj, 7, Calibration_create_three_point_calibration);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_create_uniform_calibration_obj, 6, Calibration_create_uniform_calibration);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_create_default_calibration_obj, 2, Calibration_create_default_calibration);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_apply_blank_obj, 2, Calibration_apply_blank);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_apply_two_point_obj, 5, Calibration_apply_two_point);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_apply_three_point_obj, 7, Calibration_apply_three_point);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_apply_uniform_obj, 6, Calibration_apply_uniform);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_apply_default_obj, 2, Calibration_apply_default);
MP_DEFINE_CONST_FUN_OBJ_1(Calibration_size_obj, Calibration_size);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_point_at_obj, 2, Calibration_point_at);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_first_point_obj, 1, Calibration_first_point);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_last_point_obj, 1, Calibration_last_point);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_point_at_obj, 2, Calibration_pulse_at);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_pulse_at_obj, 2, Calibration_pulse_at);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_value_at_obj, 2, Calibration_value_at);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_first_point_obj, 1, Calibration_first_pulse);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_first_pulse_obj, 1, Calibration_first_pulse);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_first_value_obj, 1, Calibration_first_value);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_last_point_obj, 1, Calibration_last_pulse);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_last_pulse_obj, 1, Calibration_last_pulse);
MP_DEFINE_CONST_FUN_OBJ_KW(Calibration_last_value_obj, 1, Calibration_last_value);
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);
@ -67,15 +73,21 @@ MP_DEFINE_CONST_FUN_OBJ_1(ServoCluster_load_obj, ServoCluster_load);
/***** Binding of Methods *****/
STATIC const mp_rom_map_elem_t Calibration_locals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR___del__), MP_ROM_PTR(&Calibration___del___obj) },
{ MP_ROM_QSTR(MP_QSTR_create_blank_calibration), MP_ROM_PTR(&Calibration_create_blank_calibration_obj) },
{ MP_ROM_QSTR(MP_QSTR_create_two_point_calibration), MP_ROM_PTR(&Calibration_create_two_point_calibration_obj) },
{ MP_ROM_QSTR(MP_QSTR_create_three_point_calibration), MP_ROM_PTR(&Calibration_create_three_point_calibration_obj) },
{ MP_ROM_QSTR(MP_QSTR_create_uniform_calibration), MP_ROM_PTR(&Calibration_create_uniform_calibration_obj) },
{ MP_ROM_QSTR(MP_QSTR_create_default_calibration), MP_ROM_PTR(&Calibration_create_default_calibration_obj) },
{ MP_ROM_QSTR(MP_QSTR_apply_blank), MP_ROM_PTR(&Calibration_apply_blank_obj) },
{ MP_ROM_QSTR(MP_QSTR_apply_two_point), MP_ROM_PTR(&Calibration_apply_two_point_obj) },
{ MP_ROM_QSTR(MP_QSTR_apply_three_point), MP_ROM_PTR(&Calibration_apply_three_point_obj) },
{ MP_ROM_QSTR(MP_QSTR_apply_uniform), MP_ROM_PTR(&Calibration_apply_uniform_obj) },
{ MP_ROM_QSTR(MP_QSTR_apply_default), MP_ROM_PTR(&Calibration_apply_default_obj) },
{ MP_ROM_QSTR(MP_QSTR_size), MP_ROM_PTR(&Calibration_size_obj) },
{ MP_ROM_QSTR(MP_QSTR_point_at), MP_ROM_PTR(&Calibration_point_at_obj) },
{ MP_ROM_QSTR(MP_QSTR_pulse_at), MP_ROM_PTR(&Calibration_pulse_at_obj) },
{ MP_ROM_QSTR(MP_QSTR_value_at), MP_ROM_PTR(&Calibration_value_at_obj) },
{ MP_ROM_QSTR(MP_QSTR_first_point), MP_ROM_PTR(&Calibration_first_point_obj) },
{ MP_ROM_QSTR(MP_QSTR_first_pulse), MP_ROM_PTR(&Calibration_first_pulse_obj) },
{ MP_ROM_QSTR(MP_QSTR_first_value), MP_ROM_PTR(&Calibration_first_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_last_point), MP_ROM_PTR(&Calibration_last_point_obj) },
{ MP_ROM_QSTR(MP_QSTR_last_pulse), MP_ROM_PTR(&Calibration_last_pulse_obj) },
{ MP_ROM_QSTR(MP_QSTR_last_value), MP_ROM_PTR(&Calibration_last_value_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) },
@ -234,7 +246,6 @@ STATIC const mp_map_elem_t servo_globals_table[] = {
{ 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) },
{ MP_ROM_QSTR(MP_QSTR_EMPTY), MP_ROM_INT(0x03) },
};
STATIC MP_DEFINE_CONST_DICT(mp_module_servo_globals, servo_globals_table);

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

@ -57,19 +57,36 @@ mp_obj_t Calibration_make_new(const mp_obj_type_t *type, size_t n_args, size_t n
enum { ARG_type };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_type, MP_ARG_INT, {.u_int = (uint8_t)servo::CalibrationType::ANGULAR} },
{ MP_QSTR_type, MP_ARG_OBJ, {.u_obj = mp_const_none} },
};
// 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);
servo::CalibrationType calibration_type = (servo::CalibrationType)args[ARG_type].u_int;
const mp_obj_t object = args[ARG_type].u_obj;
if(object != mp_const_none) {
if(mp_obj_is_int(object)) {
int type = mp_obj_get_int(object);
if(type < 0 || type >= 3) {
mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)");
}
servo::CalibrationType calibration_type = (servo::CalibrationType)type;
self = m_new_obj_with_finaliser(_Calibtration_obj_t);
self->base.type = &Calibration_type;
self = m_new_obj_with_finaliser(_Calibtration_obj_t);
self->base.type = &Calibration_type;
self->calibration = new Calibration(calibration_type);
}
else {
mp_raise_TypeError("cannot convert object to an integer");
}
}
else {
self = m_new_obj_with_finaliser(_Calibtration_obj_t);
self->base.type = &Calibration_type;
self->calibration = new Calibration();
}
self->calibration = new Calibration(calibration_type);
return MP_OBJ_FROM_PTR(self);
}
@ -83,7 +100,7 @@ mp_obj_t Calibration___del__(mp_obj_t self_in) {
/***** 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_apply_blank(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_size };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
@ -100,12 +117,12 @@ mp_obj_t Calibration_create_blank_calibration(size_t n_args, const mp_obj_t *pos
if(size < 0)
mp_raise_ValueError("size out of range. Expected 0 or greater");
else
self->calibration->create_blank_calibration((uint)size);
self->calibration->apply_blank((uint)size);
return mp_const_none;
}
mp_obj_t Calibration_create_two_point_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
mp_obj_t Calibration_apply_two_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_min_pulse, ARG_max_pulse, ARG_min_value, ARG_max_value };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
@ -125,12 +142,12 @@ mp_obj_t Calibration_create_two_point_calibration(size_t n_args, const mp_obj_t
float max_pulse = mp_obj_get_float(args[ARG_max_pulse].u_obj);
float min_value = mp_obj_get_float(args[ARG_min_value].u_obj);
float max_value = mp_obj_get_float(args[ARG_max_value].u_obj);
self->calibration->create_two_point_calibration(min_pulse, max_pulse, min_value, max_value);
self->calibration->apply_two_point(min_pulse, max_pulse, min_value, max_value);
return mp_const_none;
}
mp_obj_t Calibration_create_three_point_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
mp_obj_t Calibration_apply_three_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_min_pulse, ARG_mid_pulse, ARG_max_pulse, ARG_min_value, ARG_mid_value, ARG_max_value };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
@ -154,12 +171,12 @@ mp_obj_t Calibration_create_three_point_calibration(size_t n_args, const mp_obj_
float min_value = mp_obj_get_float(args[ARG_min_value].u_obj);
float mid_value = mp_obj_get_float(args[ARG_mid_value].u_obj);
float max_value = mp_obj_get_float(args[ARG_max_value].u_obj);
self->calibration->create_three_point_calibration(min_pulse, mid_pulse, max_pulse, min_value, mid_value, max_value);
self->calibration->apply_three_point(min_pulse, mid_pulse, max_pulse, min_value, mid_value, max_value);
return mp_const_none;
}
mp_obj_t Calibration_create_uniform_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
mp_obj_t Calibration_apply_uniform(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_size, ARG_min_pulse, ARG_max_pulse, ARG_min_value, ARG_max_value };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
@ -184,13 +201,13 @@ mp_obj_t Calibration_create_uniform_calibration(size_t n_args, const mp_obj_t *p
float max_pulse = mp_obj_get_float(args[ARG_max_pulse].u_obj);
float min_value = mp_obj_get_float(args[ARG_min_value].u_obj);
float max_value = mp_obj_get_float(args[ARG_max_value].u_obj);
self->calibration->create_uniform_calibration((uint)size, min_pulse, max_pulse, min_value, max_value);
self->calibration->apply_uniform((uint)size, min_pulse, max_pulse, min_value, max_value);
}
return mp_const_none;
}
mp_obj_t Calibration_create_default_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
mp_obj_t Calibration_apply_default(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_type };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
@ -203,8 +220,12 @@ mp_obj_t Calibration_create_default_calibration(size_t n_args, const mp_obj_t *p
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
servo::CalibrationType calibration_type = (servo::CalibrationType)args[ARG_type].u_int;
self->calibration->create_default_calibration(calibration_type);
int type = args[ARG_type].u_int;
if(type < 0 || type >= 3) {
mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)");
}
servo::CalibrationType calibration_type = (servo::CalibrationType)type;
self->calibration->apply_default(calibration_type);
return mp_const_none;
}
@ -237,10 +258,10 @@ mp_obj_t Calibration_point_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t
else {
Calibration::Point *point = self->calibration->point_at((uint)index);
mp_obj_t tuple[2];
tuple[0] = mp_obj_new_float(point->pulse);
tuple[1] = mp_obj_new_float(point->value);
return mp_obj_new_tuple(2, tuple);
mp_obj_t list = mp_obj_new_list(0, NULL);
mp_obj_list_append(list, mp_obj_new_float(point->pulse));
mp_obj_list_append(list, mp_obj_new_float(point->value));
return list;
}
}
else {
@ -277,7 +298,7 @@ mp_obj_t Calibration_point_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t
mp_raise_ValueError("list must contain two numbers");
}
}
else if(!mp_obj_is_type(object, &mp_type_tuple)) {
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]);
@ -296,6 +317,114 @@ mp_obj_t Calibration_point_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t
return mp_const_none;
}
mp_obj_t Calibration_pulse_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 2) {
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 },
};
// 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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
int index = args[ARG_index].u_int;
int calibration_size = (int)self->calibration->size();
if(calibration_size == 0)
mp_raise_ValueError("this calibration does not have any points");
if(index < 0 || index >= calibration_size)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("index out of range. Expected 0 to %d"), calibration_size - 1);
else {
Calibration::Point *point = self->calibration->point_at((uint)index);
return mp_obj_new_float(point->pulse);
}
}
else {
enum { ARG_self, ARG_index, ARG_pulse };
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_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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
int index = args[ARG_index].u_int;
int calibration_size = (int)self->calibration->size();
if(calibration_size == 0)
mp_raise_ValueError("this calibration does not have any points");
if(index < 0 || index >= calibration_size)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("index out of range. Expected 0 to %d"), calibration_size - 1);
else {
Calibration::Point *point = self->calibration->point_at((uint)index);
point->pulse = mp_obj_get_float(args[ARG_pulse].u_obj);
}
}
return mp_const_none;
}
mp_obj_t Calibration_value_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 2) {
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 },
};
// 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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
int index = args[ARG_index].u_int;
int calibration_size = (int)self->calibration->size();
if(calibration_size == 0)
mp_raise_ValueError("this calibration does not have any points");
if(index < 0 || index >= calibration_size)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("index out of range. Expected 0 to %d"), calibration_size - 1);
else {
Calibration::Point *point = self->calibration->point_at((uint)index);
return mp_obj_new_float(point->value);
}
}
else {
enum { ARG_self, ARG_index, ARG_value };
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_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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
int index = args[ARG_index].u_int;
int calibration_size = (int)self->calibration->size();
if(calibration_size == 0)
mp_raise_ValueError("this calibration does not have any points");
if(index < 0 || index >= calibration_size)
mp_raise_msg_varg(&mp_type_ValueError, MP_ERROR_TEXT("index out of range. Expected 0 to %d"), calibration_size - 1);
else {
Calibration::Point *point = self->calibration->point_at((uint)index);
point->value = mp_obj_get_float(args[ARG_value].u_obj);
}
}
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) {
if(n_args <= 1) {
enum { ARG_self };
@ -313,10 +442,10 @@ mp_obj_t Calibration_first_point(size_t n_args, const mp_obj_t *pos_args, mp_map
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
mp_obj_t tuple[2];
tuple[0] = mp_obj_new_float(point->pulse);
tuple[1] = mp_obj_new_float(point->value);
return mp_obj_new_tuple(2, tuple);
mp_obj_t list = mp_obj_new_list(0, NULL);
mp_obj_list_append(list, mp_obj_new_float(point->pulse));
mp_obj_list_append(list, mp_obj_new_float(point->value));
return list;
}
}
else {
@ -347,7 +476,7 @@ mp_obj_t Calibration_first_point(size_t n_args, const mp_obj_t *pos_args, mp_map
mp_raise_ValueError("list must contain two numbers");
}
}
else if(!mp_obj_is_type(object, &mp_type_tuple)) {
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]);
@ -366,6 +495,94 @@ mp_obj_t Calibration_first_point(size_t n_args, const mp_obj_t *pos_args, mp_map
return mp_const_none;
}
mp_obj_t Calibration_first_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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->first_point();
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
return mp_obj_new_float(point->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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->first_point();
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
point->pulse = mp_obj_get_float(args[ARG_pulse].u_obj);
}
}
return mp_const_none;
}
mp_obj_t Calibration_first_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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->first_point();
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
return mp_obj_new_float(point->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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->first_point();
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
point->value = mp_obj_get_float(args[ARG_value].u_obj);
}
}
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) {
if(n_args <= 1) {
enum { ARG_self };
@ -383,10 +600,10 @@ mp_obj_t Calibration_last_point(size_t n_args, const mp_obj_t *pos_args, mp_map_
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
mp_obj_t tuple[2];
tuple[0] = mp_obj_new_float(point->pulse);
tuple[1] = mp_obj_new_float(point->value);
return mp_obj_new_tuple(2, tuple);
mp_obj_t list = mp_obj_new_list(0, NULL);
mp_obj_list_append(list, mp_obj_new_float(point->pulse));
mp_obj_list_append(list, mp_obj_new_float(point->value));
return list;
}
}
else {
@ -417,7 +634,7 @@ mp_obj_t Calibration_last_point(size_t n_args, const mp_obj_t *pos_args, mp_map_
mp_raise_ValueError("list must contain two numbers");
}
}
else if(!mp_obj_is_type(object, &mp_type_tuple)) {
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]);
@ -436,6 +653,94 @@ mp_obj_t Calibration_last_point(size_t n_args, const mp_obj_t *pos_args, mp_map_
return mp_const_none;
}
mp_obj_t Calibration_last_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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->last_point();
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
return mp_obj_new_float(point->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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->last_point();
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
point->pulse = mp_obj_get_float(args[ARG_pulse].u_obj);
}
}
return mp_const_none;
}
mp_obj_t Calibration_last_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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->last_point();
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
return mp_obj_new_float(point->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);
_Calibration_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Calibration_obj_t);
Calibration::Point *point = self->calibration->last_point();
if(point == nullptr)
mp_raise_ValueError("this calibration does not have any points");
else {
point->value = mp_obj_get_float(args[ARG_value].u_obj);
}
}
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;
@ -558,10 +863,10 @@ void Servo_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind
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_type, ARG_freq };
enum { ARG_pin, ARG_calibration, ARG_freq };
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::CalibrationType::ANGULAR} },
{ MP_QSTR_calibration, MP_ARG_OBJ, {.u_obj = mp_const_none} },
{ MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} },
};
@ -570,7 +875,25 @@ mp_obj_t Servo_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c
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::CalibrationType calibration_type = (servo::CalibrationType)args[ARG_type].u_int;
servo::Calibration *calib = nullptr;
servo::CalibrationType calibration_type = servo::CalibrationType::ANGULAR;
const mp_obj_t calib_object = args[ARG_calibration].u_obj;
if(calib_object != mp_const_none) {
if(mp_obj_is_int(calib_object)) {
int type = mp_obj_get_int(calib_object);
if(type < 0 || type >= 3) {
mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)");
}
calibration_type = (servo::CalibrationType)type;
}
else if(mp_obj_is_type(calib_object, &Calibration_type)) {
calib = (MP_OBJ_TO_PTR2(calib_object, _Calibration_obj_t)->calibration);
}
else {
mp_raise_TypeError("cannot convert object to an integer or a Calibration class instance");
}
}
float freq = servo::ServoState::DEFAULT_FREQUENCY;
if(args[ARG_freq].u_obj != mp_const_none) {
@ -580,7 +903,10 @@ mp_obj_t Servo_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c
self = m_new_obj_with_finaliser(_Servo_obj_t);
self->base.type = &Servo_type;
self->servo = new Servo(pin, calibration_type, freq);
if(calib != nullptr)
self->servo = new Servo(pin, *calib, freq);
else
self->servo = new Servo(pin, calibration_type, freq);
self->servo->init();
return MP_OBJ_FROM_PTR(self);
@ -924,12 +1250,12 @@ void ServoCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind
mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
_ServoCluster_obj_t *self = nullptr;
enum { ARG_pio, ARG_sm, ARG_pins, ARG_type, ARG_freq, ARG_auto_phase };
enum { ARG_pio, ARG_sm, ARG_pins, ARG_calibration, ARG_freq, ARG_auto_phase };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT },
{ MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_type, MP_ARG_INT, {.u_int = (uint8_t)servo::CalibrationType::ANGULAR} },
{ MP_QSTR_calibration, 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} },
};
@ -985,7 +1311,24 @@ mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t
}
}
servo::CalibrationType calibration_type = (servo::CalibrationType)args[ARG_type].u_int;
servo::Calibration *calib = nullptr;
servo::CalibrationType calibration_type = servo::CalibrationType::ANGULAR;
const mp_obj_t calib_object = args[ARG_calibration].u_obj;
if(calib_object != mp_const_none) {
if(mp_obj_is_int(calib_object)) {
int type = mp_obj_get_int(calib_object);
if(type < 0 || type >= 3) {
mp_raise_ValueError("type out of range. Expected ANGULAR (0), LINEAR (1) or CONTINUOUS (2)");
}
calibration_type = (servo::CalibrationType)mp_obj_get_int(calib_object);
}
else if(mp_obj_is_type(calib_object, &Calibration_type)) {
calib = (MP_OBJ_TO_PTR2(calib_object, _Calibration_obj_t)->calibration);
}
else {
mp_raise_TypeError("cannot convert object to an integer or a Calibration class instance");
}
}
float freq = servo::ServoState::DEFAULT_FREQUENCY;
if(args[ARG_freq].u_obj != mp_const_none) {
@ -997,10 +1340,18 @@ mp_obj_t ServoCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t
ServoCluster *cluster;
PWMCluster::Sequence *seq_buffer = m_new(PWMCluster::Sequence, PWMCluster::NUM_BUFFERS * 2);
PWMCluster::TransitionData *dat_buffer = m_new(PWMCluster::TransitionData, PWMCluster::BUFFER_SIZE * 2);
if(mask_provided)
cluster = new ServoCluster(pio, sm, pin_mask, calibration_type, freq, auto_phase, seq_buffer, dat_buffer);
else
cluster = new ServoCluster(pio, sm, pins, pin_count, calibration_type, freq, auto_phase, seq_buffer, dat_buffer);
if(mask_provided) {
if(calib != nullptr)
cluster = new ServoCluster(pio, sm, pin_mask, *calib, freq, auto_phase, seq_buffer, dat_buffer);
else
cluster = new ServoCluster(pio, sm, pin_mask, calibration_type, freq, auto_phase, seq_buffer, dat_buffer);
}
else {
if(calib != nullptr)
cluster = new ServoCluster(pio, sm, pins, pin_count, *calib, freq, auto_phase, seq_buffer, dat_buffer);
else
cluster = new ServoCluster(pio, sm, pins, pin_count, calibration_type, freq, auto_phase, seq_buffer, dat_buffer);
}
// Cleanup the pins array
if(pins != nullptr)

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

@ -10,15 +10,21 @@ extern const mp_obj_type_t ServoCluster_type;
extern void Calibration_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind);
extern 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);
extern mp_obj_t Calibration___del__(mp_obj_t self_in);
extern mp_obj_t Calibration_create_blank_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_create_two_point_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_create_three_point_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_create_uniform_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_create_default_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_apply_blank(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_apply_two_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_apply_three_point(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_apply_uniform(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_apply_default(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
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_pulse_at(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_value_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_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_first_value(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_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Calibration_last_value(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);