pimoroni-pico/drivers/servo/servo.cpp

160 lines
4.0 KiB
C++

#include "servo.hpp"
#include "hardware/clocks.h"
#include "pwm.hpp"
namespace servo {
Servo::Servo(uint pin, CalibrationType type, float freq)
: pin(pin), state(type), pwm_frequency(freq) {
}
Servo::~Servo() {
gpio_set_function(pin, GPIO_FUNC_NULL);
}
bool Servo::init() {
bool success = false;
uint16_t period; uint16_t div16;
if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) {
pwm_period = period;
pwm_cfg = pwm_get_default_config();
// 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);
// 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 {
return pin;
}
void Servo::enable() {
apply_pulse(state.enable());
}
void Servo::disable() {
apply_pulse(state.disable());
}
bool Servo::is_enabled() const {
return state.is_enabled();
}
float Servo::get_value() const {
return state.get_value();
}
void Servo::set_value(float value) {
apply_pulse(state.set_value(value));
}
float Servo::get_pulse() const {
return state.get_pulse();
}
void Servo::set_pulse(float pulse) {
apply_pulse(state.set_pulse(pulse));
}
float Servo::get_frequency() const {
return pwm_frequency;
}
bool Servo::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
uint16_t period; uint16_t div16;
if(pimoroni::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) {
apply_pulse(state.get_pulse());
}
// 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) {
apply_pulse(state.get_pulse());
}
success = true;
}
}
return success;
}
float Servo::get_min_value() const {
return state.get_min_value();
}
float Servo::get_mid_value() const {
return state.get_mid_value();
}
float Servo::get_max_value() const {
return state.get_max_value();
}
void Servo::to_min() {
apply_pulse(state.to_min());
}
void Servo::to_mid() {
apply_pulse(state.to_mid());
}
void Servo::to_max() {
apply_pulse(state.to_max());
}
void Servo::to_percent(float in, float in_min, float in_max) {
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) {
apply_pulse(state.to_percent(in, in_min, in_max, value_min, value_max));
}
Calibration& Servo::calibration() {
return state.calibration();
}
const Calibration& Servo::calibration() const {
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));
}
};