kopia lustrzana https://github.com/pimoroni/pimoroni-pico
Started a hardware PWM servo class, with calibration support
ZodiusInfuser
rodzic
5957304a46
commit
2e58841b5e
|
|
@ -1 +1,2 @@
|
|||
include(servo.cmake)
|
||||
include(servo.cmake)
|
||||
include(multi_pwm.cmake)
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
set(DRIVER_NAME multi_pwm)
|
||||
add_library(${DRIVER_NAME} INTERFACE)
|
||||
|
||||
target_sources(${DRIVER_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/multi_pwm.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
target_link_libraries(${DRIVER_NAME} INTERFACE
|
||||
pico_stdlib
|
||||
hardware_pio
|
||||
hardware_dma
|
||||
)
|
||||
|
||||
pico_generate_pio_header(${DRIVER_NAME} ${CMAKE_CURRENT_LIST_DIR}/multi_pwm.pio)
|
||||
|
|
@ -2,15 +2,12 @@ set(DRIVER_NAME servo)
|
|||
add_library(${DRIVER_NAME} INTERFACE)
|
||||
|
||||
target_sources(${DRIVER_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/multi_pwm.cpp
|
||||
${CMAKE_CURRENT_LIST_DIR}/servo.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
target_link_libraries(${DRIVER_NAME} INTERFACE
|
||||
pico_stdlib
|
||||
hardware_pio
|
||||
hardware_dma
|
||||
)
|
||||
|
||||
pico_generate_pio_header(${DRIVER_NAME} ${CMAKE_CURRENT_LIST_DIR}/multi_pwm.pio)
|
||||
hardware_pwm
|
||||
)
|
||||
|
|
@ -0,0 +1,215 @@
|
|||
#include "servo.hpp"
|
||||
#include <cstdio>
|
||||
|
||||
namespace servo {
|
||||
Calibration::CalibrationPoint::CalibrationPoint()
|
||||
: pulse(0.0f), value(0.0f) {
|
||||
}
|
||||
|
||||
Calibration::CalibrationPoint::CalibrationPoint(uint16_t pulse, float value)
|
||||
: pulse(pulse), value(value) {
|
||||
}
|
||||
|
||||
Calibration::Calibration()
|
||||
: calibration(nullptr), calibration_points(0), limit_lower(true), limit_upper(true) {
|
||||
create_default_calibration();
|
||||
}
|
||||
|
||||
Calibration::~Calibration() {
|
||||
if(calibration != nullptr) {
|
||||
delete[] calibration;
|
||||
calibration = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void Calibration::create_default_calibration() {
|
||||
create_three_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE);
|
||||
}
|
||||
|
||||
bool Calibration::create_blank_calibration(uint num_points) {
|
||||
bool success = false;
|
||||
if(num_points >= 2) {
|
||||
if(calibration != nullptr)
|
||||
delete[] calibration;
|
||||
|
||||
calibration = new CalibrationPoint[num_points];
|
||||
calibration_points = num_points;
|
||||
success = true;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
void Calibration::create_three_point_calibration(float minus_pulse, float zero_pulse, float plus_pulse, float value_extent) {
|
||||
create_blank_calibration(3);
|
||||
calibration[0] = CalibrationPoint(minus_pulse, -value_extent);
|
||||
calibration[1] = CalibrationPoint(zero_pulse, 0.0f);
|
||||
calibration[2] = CalibrationPoint(plus_pulse, +value_extent);
|
||||
}
|
||||
|
||||
bool Calibration::create_uniform_calibration(uint num_points, float min_pulse, float min_value, float max_pulse, float max_value) {
|
||||
bool success = false;
|
||||
if(create_blank_calibration(num_points)) {
|
||||
float points_minus_one = (float)(num_points - 1);
|
||||
for(uint i = 0; i < num_points; i++) {
|
||||
float pulse = ((max_pulse - min_pulse) * (float)i) / points_minus_one;
|
||||
float value = ((max_value - min_value) * (float)i) / points_minus_one;
|
||||
calibration[i] = CalibrationPoint(pulse, value);
|
||||
}
|
||||
success = true;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
uint Calibration::points() {
|
||||
return calibration_points;
|
||||
}
|
||||
|
||||
bool Calibration::get_point(uint8_t index, CalibrationPoint& point_out) {
|
||||
bool success = false;
|
||||
if(index < calibration_points) {
|
||||
point_out = CalibrationPoint(calibration[index]);
|
||||
success = true;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
void Calibration::set_point(uint8_t index, const CalibrationPoint& point) {
|
||||
if(index < calibration_points) {
|
||||
calibration[index] = CalibrationPoint(point);
|
||||
}
|
||||
}
|
||||
|
||||
void Calibration::limit_to_calibration(bool lower, bool upper) {
|
||||
limit_lower = lower;
|
||||
limit_upper = upper;
|
||||
}
|
||||
|
||||
|
||||
uint32_t Converter::pulse_to_level(float pulse, uint32_t resolution) {
|
||||
if(pulse != 0) {
|
||||
// Constrain the level to hardcoded limits to protect the servo
|
||||
pulse = MIN(MAX(pulse, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT);
|
||||
}
|
||||
return (uint32_t)((pulse * (float)resolution) / SERVO_PERIOD);
|
||||
}
|
||||
|
||||
uint32_t Converter::pulse_to_level(uint16_t pulse, uint32_t resolution) {
|
||||
if(pulse != 0) {
|
||||
// Constrain the level to hardcoded limits to protect the servo
|
||||
pulse = MIN(MAX(pulse, LOWER_HARD_LIMIT_I), UPPER_HARD_LIMIT_I);
|
||||
}
|
||||
return (uint32_t)(((uint64_t)pulse * (uint64_t)resolution) / SERVO_PERIOD);
|
||||
}
|
||||
|
||||
float Converter::value_to_pulse(float value) {
|
||||
float pulse = 0;
|
||||
if(calibration_points >= 2) {
|
||||
uint8_t last = calibration_points - 1;
|
||||
|
||||
// Is the value below the bottom most calibration point?
|
||||
if(value < calibration[0].value) {
|
||||
// Should the value be limited to the calibration or projected below it?
|
||||
if(limit_lower)
|
||||
pulse = calibration[0].pulse;
|
||||
else
|
||||
pulse = map_pulse(value, calibration[0].value, calibration[1].value,
|
||||
calibration[0].pulse, calibration[1].pulse);
|
||||
}
|
||||
// Is the value above the top most calibration point?
|
||||
else if(value > calibration[last].value) {
|
||||
// Should the value be limited to the calibration or projected above it?
|
||||
if(limit_upper)
|
||||
pulse = calibration[last].pulse;
|
||||
else
|
||||
pulse = map_pulse(value, calibration[last - 1].value, calibration[last].value,
|
||||
calibration[last - 1].pulse, calibration[last].pulse);
|
||||
}
|
||||
else {
|
||||
// The value must between two calibration points, so iterate through them to find which ones
|
||||
for(uint8_t i = 0; i < last; i++) {
|
||||
if(value <= calibration[i + 1].value) {
|
||||
pulse = map_pulse(value, calibration[i].value, calibration[i + 1].value,
|
||||
calibration[i].pulse, calibration[i + 1].pulse);
|
||||
break; // No need to continue checking so break out of the loop
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return pulse;
|
||||
}
|
||||
|
||||
float Converter::map_pulse(float value, float min_value, float max_value, float min_pulse, float max_pulse) {
|
||||
return (((value - min_value) * (max_pulse - min_pulse)) / (max_value - min_value)) + min_pulse;
|
||||
}
|
||||
|
||||
Servo::Servo(uint pin)
|
||||
: pin(pin) {
|
||||
};
|
||||
|
||||
Servo::~Servo() {
|
||||
gpio_set_function(pin, GPIO_FUNC_NULL);
|
||||
}
|
||||
|
||||
bool Servo::init() {
|
||||
pwm_cfg = pwm_get_default_config();
|
||||
pwm_config_set_wrap(&pwm_cfg, 20000 - 1);
|
||||
|
||||
float div = clock_get_hz(clk_sys) / 1000000;
|
||||
pwm_config_set_clkdiv(&pwm_cfg, div);
|
||||
|
||||
pwm_init(pwm_gpio_to_slice_num(pin), &pwm_cfg, true);
|
||||
gpio_set_function(pin, GPIO_FUNC_PWM);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Servo::set_value(float value) {
|
||||
float pulse = converter.value_to_pulse(value);
|
||||
uint16_t level = (uint16_t)converter.pulse_to_level(pulse, 20000);
|
||||
pwm_set_gpio_level(pin, level);
|
||||
}
|
||||
|
||||
// void RGBLED::set_brightness(uint8_t brightness) {
|
||||
// led_brightness = brightness;
|
||||
// update_pwm();
|
||||
// }
|
||||
|
||||
// void RGBLED::set_hsv(float h, float s, float v) {
|
||||
// float i = floor(h * 6.0f);
|
||||
// float f = h * 6.0f - i;
|
||||
// v *= 255.0f;
|
||||
// uint8_t p = v * (1.0f - s);
|
||||
// uint8_t q = v * (1.0f - f * s);
|
||||
// uint8_t t = v * (1.0f - (1.0f - f) * s);
|
||||
|
||||
// switch (int(i) % 6) {
|
||||
// case 0: led_r = v; led_g = t; led_b = p; break;
|
||||
// case 1: led_r = q; led_g = v; led_b = p; break;
|
||||
// case 2: led_r = p; led_g = v; led_b = t; break;
|
||||
// case 3: led_r = p; led_g = q; led_b = v; break;
|
||||
// case 4: led_r = t; led_g = p; led_b = v; break;
|
||||
// case 5: led_r = v; led_g = p; led_b = q; break;
|
||||
// }
|
||||
|
||||
// update_pwm();
|
||||
// }
|
||||
|
||||
// void RGBLED::update_pwm() {
|
||||
// uint16_t r16 = GAMMA[led_r];
|
||||
// uint16_t g16 = GAMMA[led_g];
|
||||
// uint16_t b16 = GAMMA[led_b];
|
||||
// r16 *= led_brightness;
|
||||
// g16 *= led_brightness;
|
||||
// b16 *= led_brightness;
|
||||
// if(polarity == Polarity::ACTIVE_LOW) {
|
||||
// r16 = UINT16_MAX - r16;
|
||||
// g16 = UINT16_MAX - g16;
|
||||
// b16 = UINT16_MAX - b16;
|
||||
// }
|
||||
// pwm_set_gpio_level(pin_r, r16);
|
||||
// pwm_set_gpio_level(pin_g, g16);
|
||||
// pwm_set_gpio_level(pin_b, b16);
|
||||
// }
|
||||
};
|
||||
|
|
@ -0,0 +1,165 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "hardware/clocks.h"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
|
||||
namespace servo {
|
||||
|
||||
class Calibration {
|
||||
//--------------------------------------------------
|
||||
// Constants
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
static constexpr float DEFAULT_MIN_PULSE = 500.0f; // in microseconds
|
||||
static constexpr float DEFAULT_MID_PULSE = 1500.0f; // in microseconds
|
||||
static constexpr float DEFAULT_MAX_PULSE = 2500.0f; // in microseconds
|
||||
static constexpr float DEFAULT_VALUE_EXTENT = 90.0f; // a range of -90 to +90
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Substructures
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
struct CalibrationPoint {
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
CalibrationPoint();
|
||||
CalibrationPoint(uint16_t pulse, float value);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
float pulse;
|
||||
float value;
|
||||
};
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
protected:
|
||||
Calibration();
|
||||
virtual ~Calibration();
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
void create_default_calibration();
|
||||
bool create_blank_calibration(uint num_points); // Must have at least two points
|
||||
void create_three_point_calibration(float minus_pulse, float zero_pulse, float plus_pulse, float value_extent = DEFAULT_VALUE_EXTENT);
|
||||
bool create_uniform_calibration(uint num_points, float min_pulse, float min_value, float max_pulse, float max_value); // Must have at least two points
|
||||
|
||||
uint points();
|
||||
bool get_point(uint8_t index, CalibrationPoint& point_out);
|
||||
void set_point(uint8_t index, const CalibrationPoint& point); // Ensure the points are entered in ascending value order
|
||||
|
||||
void limit_to_calibration(bool lower, bool upper);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
protected:
|
||||
CalibrationPoint* calibration;
|
||||
uint calibration_points;
|
||||
bool limit_lower;
|
||||
bool limit_upper;
|
||||
};
|
||||
|
||||
class Converter : public Calibration {
|
||||
//--------------------------------------------------
|
||||
// Constants
|
||||
//--------------------------------------------------
|
||||
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
|
||||
|
||||
//Integer equivalents
|
||||
static const uint16_t LOWER_HARD_LIMIT_I = (uint16_t)LOWER_HARD_LIMIT;
|
||||
static const uint16_t UPPER_HARD_LIMIT_I = (uint16_t)UPPER_HARD_LIMIT;
|
||||
static const uint64_t SERVO_PERIOD_I = (uint64_t)SERVO_PERIOD;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Converter() : Calibration() {}
|
||||
virtual ~Converter() {}
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
static uint32_t pulse_to_level(float pulse, uint32_t resolution);
|
||||
static uint32_t pulse_to_level(uint16_t pulse, uint32_t resolution);
|
||||
float value_to_pulse(float value);
|
||||
private:
|
||||
static float map_pulse(float value, float min_value, float max_value, float min_pulse, float max_pulse);
|
||||
};
|
||||
|
||||
class Servo {
|
||||
//--------------------------------------------------
|
||||
// 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:
|
||||
uint pin;
|
||||
pwm_config pwm_cfg;
|
||||
uint16_t pwm_period;
|
||||
float pwm_frequency = DEFAULT_PWM_FREQUENCY;
|
||||
|
||||
float servo_angle = 0.0f;
|
||||
|
||||
Converter converter;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Servo(uint pin);
|
||||
~Servo();
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
bool init();
|
||||
|
||||
void enable_servo();
|
||||
void disable_servo();
|
||||
|
||||
void set_pulse();
|
||||
void set_value(float value);
|
||||
|
||||
Calibration& calibration() {
|
||||
return converter;
|
||||
}
|
||||
|
||||
private:
|
||||
//void update_pwm();
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -40,6 +40,8 @@ WS2812 led_bar(N_LEDS, pio0, 0, servo2040::LED_DAT);
|
|||
|
||||
Button user_sw(servo2040::USER_SW, Polarity::ACTIVE_LOW, 0);
|
||||
|
||||
Servo simple_servo(0);
|
||||
|
||||
uint count = 0;
|
||||
uint servo_seq = 0;
|
||||
int main() {
|
||||
|
|
@ -49,8 +51,13 @@ int main() {
|
|||
|
||||
sleep_ms(5000);
|
||||
|
||||
MultiPWM pwms(pio1, 0, 0b111111111111111);
|
||||
pwms.set_wrap(20000);
|
||||
//Calibration& calib = simple_servo.calibration();
|
||||
//calib.create_three_point_calibration(500, 1500, 2500, 4);
|
||||
|
||||
simple_servo.init();
|
||||
|
||||
//MultiPWM pwms(pio1, 0, 0b111111111111111);
|
||||
//pwms.set_wrap(20000);
|
||||
|
||||
int speed = DEFAULT_SPEED;
|
||||
float offset = 0.0f;
|
||||
|
|
@ -75,16 +82,17 @@ int main() {
|
|||
if(count >= 100) {
|
||||
count = 0;
|
||||
|
||||
pwms.set_chan_level(servo_seq, 2000);//toggle ? 2000 : 1000);
|
||||
//pwms.set_chan_level(servo_seq, 2000);//toggle ? 2000 : 1000);
|
||||
//pwms.set_chan_polarity(servo_seq, toggle);
|
||||
//pwms.set_chan_offset(servo_seq, toggle ? 19000 : 0);
|
||||
simple_servo.set_value(servo_seq);
|
||||
servo_seq++;
|
||||
if(servo_seq >= 4) {
|
||||
servo_seq = 0;
|
||||
toggle = !toggle;
|
||||
//pwms.set_wrap(toggle ? 30000 : 20000);
|
||||
float div = clock_get_hz(clk_sys) / (toggle ? 500000 : 5000000);
|
||||
pwms.set_clkdiv(div);
|
||||
//float div = clock_get_hz(clk_sys) / (toggle ? 500000 : 5000000);
|
||||
//pwms.set_clkdiv(div);
|
||||
}
|
||||
|
||||
//pwms.load_pwm();
|
||||
|
|
|
|||
|
|
@ -3,4 +3,4 @@ add_library(servo2040 INTERFACE)
|
|||
target_include_directories(servo2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(servo2040 INTERFACE pico_stdlib plasma servo)
|
||||
target_link_libraries(servo2040 INTERFACE pico_stdlib plasma multi_pwm servo)
|
||||
|
|
@ -3,6 +3,7 @@
|
|||
|
||||
#include "ws2812.hpp"
|
||||
#include "multi_pwm.hpp"
|
||||
#include "servo.hpp"
|
||||
|
||||
namespace servo2040 {
|
||||
const uint SERVO_1 = 0;
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue