Added frequency adjustment to Servo

pull/259/head
ZodiusInfuser 2022-02-19 01:07:19 +00:00
rodzic 93eafc4694
commit 334ff4e9f9
8 zmienionych plików z 178 dodań i 40 usunięć

Wyświetl plik

@ -11,18 +11,28 @@ namespace servo {
}
bool Servo::init() {
pwm_cfg = pwm_get_default_config();
pwm_config_set_wrap(&pwm_cfg, 20000 - 1);
bool success = false;
float div = clock_get_hz(clk_sys) / 1000000;
pwm_config_set_clkdiv(&pwm_cfg, div);
uint16_t period; uint16_t div16;
if(Servo::calculate_pwm_factors(pwm_frequency, period, div16)) {
pwm_period = period;
pwm_init(pwm_gpio_to_slice_num(pin), &pwm_cfg, true);
gpio_set_function(pin, GPIO_FUNC_PWM);
pwm_cfg = pwm_get_default_config();
pwm_set_gpio_level(pin, 0);
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
pwm_config_set_wrap(&pwm_cfg, pwm_period - 1);
return true;
// Apply the divider
pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); // There's no 'pwm_config_set_clkdiv_int_frac' for some reason...
pwm_init(pwm_gpio_to_slice_num(pin), &pwm_cfg, true);
gpio_set_function(pin, GPIO_FUNC_PWM);
pwm_set_gpio_level(pin, 0);
success = true;
}
return success;
}
uint Servo::get_pin() const {
@ -31,12 +41,12 @@ namespace servo {
void Servo::enable() {
float new_pulse = state.enable();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
}
void Servo::disable() {
float new_pulse = state.disable();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
}
bool Servo::is_enabled() const {
@ -49,7 +59,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, 20000));
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
}
float Servo::get_pulse() const {
@ -58,7 +68,54 @@ 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, 20000));
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
}
float Servo::get_frequency() const {
return pwm_frequency;
}
bool Servo::set_frequency(float freq) {
bool success = false;
// Calculate a suitable pwm wrap period for this frequency
uint16_t period; uint16_t div16;
if(Servo::calculate_pwm_factors(freq, period, div16)) {
// Record if the new period will be larger or smaller.
// This is used to apply new pwm values either before or after the wrap is applied,
// to avoid momentary blips in PWM output on SLOW_DECAY
bool pre_update_pwm = (period > pwm_period);
pwm_period = period;
pwm_frequency = freq;
uint pin_num = pwm_gpio_to_slice_num(pin);
// Apply the new divider
uint8_t div = div16 >> 4;
uint8_t mod = div16 % 16;
pwm_set_clkdiv_int_frac(pin_num, div, mod);
// 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));
}
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
pwm_set_wrap(pin_num, pwm_period - 1);
// 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));
}
success = true;
}
return success;
}
float Servo::get_min_value() const {
@ -75,27 +132,27 @@ 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, 20000));
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
}
void Servo::to_mid() {
float new_pulse = state.to_mid();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
}
void Servo::to_max() {
float new_pulse = state.to_max();
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, 20000));
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
}
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, 20000));
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
}
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, 20000));
pwm_set_gpio_level(pin, (uint16_t)ServoState::pulse_to_level(new_pulse, pwm_period, pwm_frequency));
}
Calibration& Servo::calibration() {
@ -105,4 +162,43 @@ namespace servo {
const Calibration& Servo::calibration() const {
return state.calibration();
}
// Derived from the rp2 Micropython implementation: https://github.com/micropython/micropython/blob/master/ports/rp2/machine_pwm.c
bool Servo::calculate_pwm_factors(float freq, uint16_t& top_out, uint16_t& div16_out) {
bool success = false;
uint32_t source_hz = clock_get_hz(clk_sys);
// Check the provided frequency is valid
if((freq >= 1.0f) && (freq <= (float)(source_hz >> 1))) {
uint32_t div16_top = (uint32_t)((float)(source_hz << 4) / freq);
uint32_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_WRAP)) {
div16_top /= 5;
top *= 5;
}
else if((div16_top >= (3 << 4)) && (div16_top % 3 == 0) && (top * 3 <= MAX_PWM_WRAP)) {
div16_top /= 3;
top *= 3;
}
else if((div16_top >= (2 << 4)) && (top * 2 <= MAX_PWM_WRAP)) {
div16_top /= 2;
top *= 2;
}
else {
break;
}
}
if(div16_top >= 16 && div16_top <= (UINT8_MAX << 4)) {
top_out = top;
div16_out = div16_top;
success = true;
}
}
return success;
}
};

Wyświetl plik

@ -11,11 +11,10 @@ namespace servo {
// Constants
//--------------------------------------------------
public:
static const uint16_t DEFAULT_PWM_FREQUENCY = 50; //The standard servo update rate
static const uint16_t DEFAULT_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);
//--------------------------------------------------
@ -25,8 +24,7 @@ namespace servo {
uint pin;
pwm_config pwm_cfg;
uint16_t pwm_period;
float pwm_frequency = DEFAULT_PWM_FREQUENCY;
float pwm_frequency = DEFAULT_FREQUENCY;
ServoState state;
@ -56,6 +54,9 @@ namespace servo {
float get_pulse() const;
void set_pulse(float pulse);
float get_frequency() const;
bool set_frequency(float freq);
float get_min_value() const;
float get_mid_value() const;
float get_max_value() const;
@ -68,6 +69,8 @@ namespace servo {
Calibration& calibration();
const Calibration& calibration() const;
private:
static bool calculate_pwm_factors(float freq, uint16_t& top_out, uint16_t& div16_out);
};
}

Wyświetl plik

@ -31,14 +31,14 @@ namespace servo {
void ServoCluster::enable(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].enable();
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
}
}
void ServoCluster::disable(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].disable();
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
}
}
@ -59,7 +59,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);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
}
}
@ -73,7 +73,7 @@ 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);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
}
}
@ -101,35 +101,35 @@ namespace servo {
void ServoCluster::to_min(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].to_min();
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
}
}
void ServoCluster::to_mid(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].to_mid();
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
}
}
void ServoCluster::to_max(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].to_max();
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), 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);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), 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);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000), load);
multi_pwm.set_chan_level(servo, ServoState::pulse_to_level(new_pulse, 20000, 1000000 / 50), load);
}
}

Wyświetl plik

@ -114,12 +114,12 @@ namespace servo {
return table;
}
uint32_t ServoState::pulse_to_level(float pulse, uint32_t resolution) {
uint32_t ServoState::pulse_to_level(float pulse, uint32_t resolution, float freq) {
uint32_t level = 0;
if(pulse >= MIN_VALID_PULSE) {
// Constrain the level to hardcoded limits to protect the servo
pulse = MIN(MAX(pulse, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT);
level = (uint32_t)((pulse * (float)resolution) / SERVO_PERIOD);
level = (uint32_t)((pulse * (float)resolution * freq) / 1000000);
}
return level;
}

Wyświetl plik

@ -16,7 +16,7 @@ namespace servo {
private:
static constexpr float LOWER_HARD_LIMIT = 500.0f; // The minimum microsecond pulse to send
static constexpr float UPPER_HARD_LIMIT = 2500.0f; // The maximum microsecond pulse to send
static constexpr float SERVO_PERIOD = 1000000 / 50; // This is hardcoded as all servos *should* run at this frequency
static constexpr float SERVO_PERIOD = 20000; // This is hardcoded as all servos *should* run at this frequency
static constexpr float MIN_VALID_PULSE = 1.0f;
@ -67,7 +67,7 @@ namespace servo {
const Calibration& calibration() const;
//--------------------------------------------------
static uint32_t pulse_to_level(float pulse, uint32_t resolution);
static uint32_t pulse_to_level(float pulse, uint32_t resolution, float freq);
};
}

Wyświetl plik

@ -23,6 +23,7 @@ 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_KW(Servo_frequency_obj, 1, Servo_frequency);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_min_value_obj, Servo_min_value);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_mid_value_obj, Servo_mid_value);
MP_DEFINE_CONST_FUN_OBJ_1(Servo_max_value_obj, Servo_max_value);
@ -73,6 +74,7 @@ STATIC const mp_rom_map_elem_t Servo_locals_dict_table[] = {
{ 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_frequency), MP_ROM_PTR(&Servo_frequency_obj) },
{ MP_ROM_QSTR(MP_QSTR_min_value), MP_ROM_PTR(&Servo_min_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_mid_value), MP_ROM_PTR(&Servo_mid_value_obj) },
{ MP_ROM_QSTR(MP_QSTR_max_value), MP_ROM_PTR(&Servo_max_value_obj) },

Wyświetl plik

@ -495,12 +495,14 @@ void Servo_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind
mp_print_str(print, "pin = ");
mp_obj_print_helper(print, mp_obj_new_int(self->servo->get_pin()), 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, ", pulse = ");
mp_obj_print_helper(print, mp_obj_new_float(self->servo->get_pulse()), 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, ", enabled = ");
mp_obj_print_helper(print, self->servo->is_enabled() ? mp_const_true : mp_const_false, PRINT_REPR);
mp_print_str(print, ", freq = ");
mp_obj_print_helper(print, mp_obj_new_float(self->servo->get_frequency()), PRINT_REPR);
mp_print_str(print, ")");
}
@ -631,6 +633,43 @@ extern mp_obj_t Servo_pulse(size_t n_args, const mp_obj_t *pos_args, mp_map_t *k
}
}
extern mp_obj_t Servo_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);
_Servo_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Servo_obj_t);
return mp_obj_new_float(self->servo->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);
_Servo_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Servo_obj_t);
float freq = mp_obj_get_float(args[ARG_freq].u_obj);
if(!self->servo->set_frequency(freq))
mp_raise_ValueError("freq out of range");
else
return mp_const_none;
}
}
extern mp_obj_t Servo_min_value(mp_obj_t self_in) {
_Servo_obj_t *self = MP_OBJ_TO_PTR2(self_in, _Servo_obj_t);
return mp_obj_new_float(self->servo->get_min_value());

Wyświetl plik

@ -32,6 +32,7 @@ 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_frequency(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t Servo_min_value(mp_obj_t self_in);
extern mp_obj_t Servo_mid_value(mp_obj_t self_in);
extern mp_obj_t Servo_max_value(mp_obj_t self_in);
@ -57,7 +58,4 @@ extern mp_obj_t ServoCluster_to_min(size_t n_args, const mp_obj_t *pos_args, mp_
extern mp_obj_t ServoCluster_to_mid(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_to_max(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern mp_obj_t ServoCluster_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
extern bool Pimoroni_mp_obj_to_calibration(mp_obj_t in, void *out);
extern mp_obj_t ServoCluster_calibration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);