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

Added set_frequency support to ServoCluster

This commit is contained in:
ZodiusInfuser 2022-02-20 15:12:02 +00:00
parent aeb9705d15
commit d94a7c0718
11 changed files with 193 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
drivers/pwm/pwm_cluster.cpp
View File

@ -182,6 +182,7 @@ PWMCluster::PWMCluster(PIO pio, uint sm, uint channel_mask) : pio(pio), sm(sm),
for(uint i = 0; i < NUM_BUFFERS; i++) {
Sequence& seq = sequences[i];
seq = Sequence();
seq.data[0].delay = 10; // Need to set a delay otherwise a lockup occurs when first changing frequency
}
// Manually call the handler once, to trigger the first transfer
@ -382,4 +383,44 @@ void PWMCluster::sorted_insert(TransitionData array[], uint &size, const Transit
//printf("Added %d, %ld, %d\n", data.channel, data.level, data.state);
size++;
}
// Derived from the rp2 Micropython implementation: https://github.com/micropython/micropython/blob/master/ports/rp2/machine_pwm.c
bool PWMCluster::calculate_pwm_factors(float freq, uint32_t& top_out, uint16_t& div16_out) {
bool success = false;
uint32_t source_hz = clock_get_hz(clk_sys) / PWM_CLUSTER_CYCLES;
// Check the provided frequency is valid
if((freq >= 0.01f) && (freq <= (float)(source_hz >> 1))) {
uint32_t div16_top = (uint32_t)((float)(source_hz << 4) / freq);
uint64_t top = 1;
while(true) {
// Try a few small prime factors to get close to the desired frequency.
if((div16_top >= (5 << 4)) && (div16_top % 5 == 0) && (top * 5 <= MAX_PWM_CLUSTER_WRAP)) {
div16_top /= 5;
top *= 5;
}
else if((div16_top >= (3 << 4)) && (div16_top % 3 == 0) && (top * 3 <= MAX_PWM_CLUSTER_WRAP)) {
div16_top /= 3;
top *= 3;
}
else if((div16_top >= (2 << 4)) && (top * 2 <= MAX_PWM_CLUSTER_WRAP)) {
div16_top /= 2;
top *= 2;
}
else {
break;
}
}
// Only return valid factors if the divisor is actually achievable
if(div16_top >= 16 && div16_top <= (UINT8_MAX << 4)) {
top_out = top;
div16_out = div16_top;
success = true;
}
}
return success;
}
}

5
drivers/pwm/pwm_cluster.hpp
View File

@ -46,5 +46,10 @@ namespace pimoroni {
uint channel_offsets[NUM_BANK0_GPIOS];
uint channel_polarities;
uint wrap_level;
public:
static bool calculate_pwm_factors(float freq, uint32_t& top_out, uint16_t& div16_out);
private:
static const uint64_t MAX_PWM_CLUSTER_WRAP = UINT16_MAX; // UINT32_MAX works too, but seems to produce less accurate counters
};
}

2
drivers/pwm/pwm_cluster.pio
View File

@ -32,7 +32,7 @@
; Debounce Constants
; --------------------------------------------------
.define public MULT_PWM_CYCLES 5
.define public PWM_CLUSTER_CYCLES 5
; PWM Program

38
drivers/servo/servo.cpp
View File

@ -41,13 +41,11 @@ namespace servo {
}
void Servo::enable() {
float new_pulse = state.enable();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
apply_pulse(state.enable());
}
void Servo::disable() {
float new_pulse = state.disable();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
apply_pulse(state.disable());
}
bool Servo::is_enabled() const {
@ -59,8 +57,7 @@ namespace servo {
}
void Servo::set_value(float value) {
float new_pulse = state.set_value(value);
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
apply_pulse(state.set_value(value));
}
float Servo::get_pulse() const {
@ -68,8 +65,7 @@ namespace servo {
}
void Servo::set_pulse(float pulse) {
float new_pulse = state.set_pulse(pulse);
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
apply_pulse(state.set_pulse(pulse));
}
float Servo::get_frequency() const {
@ -79,7 +75,7 @@ namespace servo {
bool Servo::set_frequency(float freq) {
bool success = false;
if((freq >= MIN_FREQUENCY) && (freq <= MAX_FREQUENCY)) {
if((freq >= ServoState::MIN_FREQUENCY) && (freq <= ServoState::MAX_FREQUENCY)) {
// Calculate a suitable pwm wrap period for this frequency
uint16_t period; uint16_t div16;
if(pimoroni::calculate_pwm_factors(freq, period, div16)) {
@ -101,8 +97,7 @@ namespace servo {
// If the the period is larger, update the pwm before setting the new wraps
if(pre_update_pwm) {
float current_pulse = get_pulse();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency));
apply_pulse(state.get_pulse());
}
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
@ -110,8 +105,7 @@ namespace servo {
// If the the period is smaller, update the pwm after setting the new wraps
if(!pre_update_pwm) {
float current_pulse = get_pulse();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(current_pulse, pwm_period, pwm_frequency));
apply_pulse(state.get_pulse());
}
success = true;
@ -133,28 +127,23 @@ namespace servo {
}
void Servo::to_min() {
float new_pulse = state.to_min();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
apply_pulse(state.to_min());
}
void Servo::to_mid() {
float new_pulse = state.to_mid();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
apply_pulse(state.to_mid());
}
void Servo::to_max() {
float new_pulse = state.to_max();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
apply_pulse(state.to_max());
}
void Servo::to_percent(float in, float in_min, float in_max) {
float new_pulse = state.to_percent(in, in_min, in_max);
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
apply_pulse(state.to_percent(in, in_min, in_max));
}
void Servo::to_percent(float in, float in_min, float in_max, float value_min, float value_max) {
float new_pulse = state.to_percent(in, in_min, in_max, value_min, value_max);
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
apply_pulse(state.to_percent(in, in_min, in_max, value_min, value_max));
}
Calibration& Servo::calibration() {
@ -165,4 +154,7 @@ namespace servo {
return state.calibration();
}
void Servo::apply_pulse(float pulse) {
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(pulse, pwm_period, pwm_frequency));
}
};

19
drivers/servo/servo.hpp
View File

@ -7,18 +7,6 @@
namespace servo {
class Servo {
//--------------------------------------------------
// Constants
//--------------------------------------------------
public:
static constexpr float DEFAULT_FREQUENCY = 50.0f; // The standard servo update rate
private:
static constexpr float MIN_FREQUENCY = 10.0f; // Lowest achievable with hardware PWM with good resolution
static constexpr float MAX_FREQUENCY = 350.0f; // Highest nice value that still allows the full uS pulse range
// Some servos are rated for 333Hz for instance
//--------------------------------------------------
// Variables
//--------------------------------------------------
@ -26,7 +14,7 @@ namespace servo {
uint pin;
pwm_config pwm_cfg;
uint16_t pwm_period;
float pwm_frequency = DEFAULT_FREQUENCY;
float pwm_frequency = ServoState::DEFAULT_FREQUENCY;
ServoState state;
@ -59,6 +47,7 @@ namespace servo {
float get_frequency() const;
bool set_frequency(float freq);
//--------------------------------------------------
float get_min_value() const;
float get_mid_value() const;
float get_max_value() const;
@ -71,6 +60,10 @@ namespace servo {
Calibration& calibration();
const Calibration& calibration() const;
//--------------------------------------------------
private:
void apply_pulse(float pulse);
};
}

81
drivers/servo/servo_cluster.cpp
View File

@ -1,9 +1,30 @@
#include "servo_cluster.hpp"
#include "pwm.hpp"
#include <cstdio>
namespace servo {
ServoCluster::ServoCluster(PIO pio, uint sm, uint channel_mask)
: pwms(pio, sm, channel_mask) {
pwms.set_wrap(20000);
// Calculate a suitable pwm wrap period for this frequency
uint32_t period; uint16_t div16;
if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div16)) {
pwm_period = period;
// Update the pwm before setting the new wrap
for(uint servo = 0; servo < NUM_BANK0_GPIOS; servo++) {
pwms.set_chan_level(servo, 0, false);
}
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
pwms.set_wrap(pwm_period); // NOTE Minus 1 not needed here. Maybe should change Wrap behaviour so it is needed, for consistency with hardware pwm?
// Apply the new divider
// This is done after loading new PWM values to avoid a lockup condition
uint8_t div = div16 >> 4;
uint8_t mod = div16 % 16;
pwms.set_clkdiv_int_frac(div, mod);
}
}
ServoCluster::~ServoCluster() {
@ -31,14 +52,14 @@ namespace servo {
void ServoCluster::enable(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].enable();
pwms.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 50), load);
apply_pulse(servo, new_pulse, load);
}
}
void ServoCluster::disable(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].disable();
pwms.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 50), load);
apply_pulse(servo, new_pulse, load);
}
}
@ -59,7 +80,7 @@ namespace servo {
void ServoCluster::set_value(uint servo, float value, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].set_value(value);
pwms.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 50), load);
apply_pulse(servo, new_pulse, load);
}
}
@ -73,10 +94,46 @@ namespace servo {
void ServoCluster::set_pulse(uint servo, float pulse, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].set_pulse(pulse);
pwms.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 50), load);
apply_pulse(servo, new_pulse, load);
}
}
float ServoCluster::get_frequency() const {
return pwm_frequency;
}
bool ServoCluster::set_frequency(float freq) {
bool success = false;
if((freq >= ServoState::MIN_FREQUENCY) && (freq <= ServoState::MAX_FREQUENCY)) {
// Calculate a suitable pwm wrap period for this frequency
uint32_t period; uint16_t div16;
if(pimoroni::PWMCluster::calculate_pwm_factors(freq, period, div16)) {
pwm_period = period;
pwm_frequency = freq;
// Update the pwm before setting the new wrap
for(uint servo = 0; servo < NUM_BANK0_GPIOS; servo++) {
float current_pulse = servos[servo].get_pulse();
apply_pulse(servo, current_pulse, false);
}
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
pwms.set_wrap(pwm_period, true);
// Apply the new divider
// This is done after loading new PWM values to avoid a lockup condition
uint8_t div = div16 >> 4;
uint8_t mod = div16 % 16;
pwms.set_clkdiv_int_frac(div, mod);
success = true;
}
}
return success;
}
float ServoCluster::get_min_value(uint servo) const {
if(servo < NUM_BANK0_GPIOS)
return servos[servo].get_min_value();
@ -101,35 +158,35 @@ namespace servo {
void ServoCluster::to_min(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].to_min();
pwms.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 50), load);
apply_pulse(servo, new_pulse, load);
}
}
void ServoCluster::to_mid(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].to_mid();
pwms.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 50), load);
apply_pulse(servo, new_pulse, load);
}
}
void ServoCluster::to_max(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].to_max();
pwms.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 50), load);
apply_pulse(servo, new_pulse, load);
}
}
void ServoCluster::to_percent(uint servo, float in, float in_min, float in_max, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].to_percent(in, in_min, in_max);
pwms.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 50), load);
apply_pulse(servo, new_pulse, load);
}
}
void ServoCluster::to_percent(uint servo, float in, float in_min, float in_max, float value_min, float value_max, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].to_percent(in, in_min, in_max, value_min, value_max);
pwms.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 50), load);
apply_pulse(servo, new_pulse, load);
}
}
@ -146,4 +203,8 @@ namespace servo {
else
return nullptr;
}
void ServoCluster::apply_pulse(uint servo, float pulse, bool load) {
pwms.set_chan_level(servo, ServoState::pulse_to_level(pulse, pwm_period, pwm_frequency), load);
}
};

21
drivers/servo/servo_cluster.hpp
View File

@ -7,22 +7,13 @@
namespace servo {
class ServoCluster {
//--------------------------------------------------
// Constants
//--------------------------------------------------
public:
static const uint16_t DEFAULT_PWM_FREQUENCY = 50; //The standard servo update rate
private:
static const uint32_t MAX_PWM_WRAP = UINT16_MAX;
static constexpr uint16_t MAX_PWM_DIVIDER = (1 << 7);
//--------------------------------------------------
// Variables
//--------------------------------------------------
private:
pimoroni::PWMCluster pwms;
uint32_t pwm_period;
float pwm_frequency = ServoState::DEFAULT_FREQUENCY;
ServoState servos[NUM_BANK0_GPIOS]; // TODO change this to array of pointers
// so that only the servos actually assigned
// to this cluster have states
@ -54,6 +45,10 @@ namespace servo {
float get_pulse(uint servo) const;
void set_pulse(uint servo, float pulse, bool load = true);
float get_frequency() const;
bool set_frequency(float freq);
//--------------------------------------------------
float get_min_value(uint servo) const;
float get_mid_value(uint servo) const;
float get_max_value(uint servo) const;
@ -66,6 +61,10 @@ namespace servo {
Calibration* calibration(uint servo);
const Calibration* calibration(uint servo) const;
//--------------------------------------------------
private:
void apply_pulse(uint servo, float pulse, bool load);
};
}

4
drivers/servo/servo_state.hpp
View File

@ -10,6 +10,10 @@ namespace servo {
// Constants
//--------------------------------------------------
public:
static constexpr float DEFAULT_FREQUENCY = 50.0f; // The standard servo update rate
static constexpr float MIN_FREQUENCY = 10.0f; // Lowest achievable with hardware PWM with good resolution
static constexpr float MAX_FREQUENCY = 350.0f; // Highest nice value that still allows the full uS pulse range
// Some servos are rated for 333Hz for instance
static constexpr float ZERO_PERCENT = 0.0f;
static constexpr float ONEHUNDRED_PERCENT = 1.0f;

2
micropython/modules/servo/servo.c
View File

@ -40,6 +40,7 @@ MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_disable_obj, 2, ServoCluster_disable);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_is_enabled_obj, 2, ServoCluster_is_enabled);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_value_obj, 2, ServoCluster_value);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_pulse_obj, 2, ServoCluster_pulse);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_frequency_obj, 1, ServoCluster_frequency);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_min_value_obj, 2, ServoCluster_min_value);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_mid_value_obj, 2, ServoCluster_mid_value);
MP_DEFINE_CONST_FUN_OBJ_KW(ServoCluster_max_value_obj, 2, ServoCluster_max_value);
@ -93,6 +94,7 @@ STATIC const mp_rom_map_elem_t ServoCluster_locals_dict_table[] = {
{ 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_frequency), MP_ROM_PTR(&ServoCluster_frequency_obj) },
{ MP_ROM_QSTR(MP_QSTR_min_value), MP_ROM_PTR(&ServoCluster_min_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_mid_value), MP_ROM_PTR(&ServoCluster_mid_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_max_value), MP_ROM_PTR(&ServoCluster_max_value_obj) },

37
micropython/modules/servo/servo.cpp
View File

@ -1055,6 +1055,43 @@ extern mp_obj_t ServoCluster_pulse(size_t n_args, const mp_obj_t *pos_args, mp_m
}
}
extern mp_obj_t ServoCluster_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
if(n_args <= 1) {
enum { ARG_self };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
};
// Parse args.
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
_ServoCluster_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _ServoCluster_obj_t);
return mp_obj_new_float(self->cluster->get_frequency());
}
else {
enum { ARG_self, ARG_freq };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ },
{ MP_QSTR_freq, 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);
float freq = mp_obj_get_float(args[ARG_freq].u_obj);
if(!self->cluster->set_frequency(freq))
mp_raise_ValueError("freq out of range. Expected 10Hz to 350Hz");
else
return mp_const_none;
}
}
extern mp_obj_t ServoCluster_min_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_self, ARG_servo };
static const mp_arg_t allowed_args[] = {

1
micropython/modules/servo/servo.h
View File

@ -51,6 +51,7 @@ extern mp_obj_t ServoCluster_disable(size_t n_args, const mp_obj_t *pos_args, mp
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_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_min_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_mid_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_max_value(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);