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

Raised servo functions to a base class, and added ServoCluster which uses the PIO PWM.

This commit is contained in:
ZodiusInfuser 2022-02-16 22:06:07 +00:00
parent d4012a271e
commit f5836e56df
11 changed files with 708 additions and 363 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
drivers/servo/calibration.cpp Normal file
View File

@ -0,0 +1,225 @@
#include "calibration.hpp"
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(ANGULAR);
}
Calibration::Calibration(Type type)
: calibration(nullptr), calibration_points(0), limit_lower(true), limit_upper(true) {
create_default_calibration(type);
}
Calibration::~Calibration() {
if(calibration != nullptr) {
delete[] calibration;
calibration = nullptr;
}
}
void Calibration::create_default_calibration(Type type) {
switch(type) {
default:
case ANGULAR:
create_three_point_calibration(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);
break;
case CONTINUOUS:
create_three_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
-1.0f, 0.0f, +1.0f);
break;
}
}
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_two_point_calibration(float min_pulse, float max_pulse, float min_value, float max_value) {
create_blank_calibration(2);
calibration[0] = CalibrationPoint(min_pulse, min_value);
calibration[1] = CalibrationPoint(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);
calibration[0] = CalibrationPoint(min_pulse, min_value);
calibration[1] = CalibrationPoint(mid_pulse, mid_value);
calibration[2] = CalibrationPoint(max_pulse, max_value);
}
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;
}
float Converter::min_value() {
float value = 0.0f;
if(calibration_points >= 2) {
value = calibration[0].value;
}
return value;
}
float Converter::mid_value() {
float value = 0.0f;
if(calibration_points >= 2) {
value = (calibration[0].value + calibration[calibration_points - 1].value) / 2.0f;
}
return value;
}
float Converter::max_value() {
float value = 0.0f;
if(calibration_points >= 2) {
value = calibration[calibration_points - 1].value;
}
return value;
}
float Converter::value_to_pulse(float value) {
float pulse = 0.0f;
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_float(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_float(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_float(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::value_from_pulse(float pulse) {
float value = 0.0f;
if(calibration_points >= 2) {
uint8_t last = calibration_points - 1;
// Is the pulse below the bottom most calibration point?
if(pulse < calibration[0].pulse) {
// Should the pulse be limited to the calibration or projected below it?
if(limit_lower)
value = calibration[0].value;
else
value = map_float(pulse, calibration[0].pulse, calibration[1].pulse,
calibration[0].value, calibration[1].value);
}
// Is the pulse above the top most calibration point?
else if(pulse > calibration[last].pulse) {
// Should the pulse be limited to the calibration or projected above it?
if(limit_upper)
value = calibration[last].value;
else
value = map_float(pulse, calibration[last - 1].pulse, calibration[last].pulse,
calibration[last - 1].value, calibration[last].value);
}
else {
// The pulse must between two calibration points, so iterate through them to find which ones
for(uint8_t i = 0; i < last; i++) {
if(pulse <= calibration[i + 1].pulse) {
value = map_float(pulse, calibration[i].pulse, calibration[i + 1].pulse,
calibration[i].value, calibration[i + 1].value);
break; // No need to continue checking so break out of the loop
}
}
}
}
return value;
}
uint32_t Converter::pulse_to_level(float pulse, uint32_t resolution) {
uint32_t level = 0;
if(pulse >= Converter::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);
}
return level;
}
float Converter::map_float(float in, float in_min, float in_max, float out_min, float out_max) {
return (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
}
};

121
drivers/servo/calibration.hpp Normal file
View File

@ -0,0 +1,121 @@
#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 {
enum Type {
ANGULAR = 0,
LINEAR,
CONTINUOUS
};
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();
Calibration(Type type);
virtual ~Calibration();
//--------------------------------------------------
// Methods
//--------------------------------------------------
public:
void create_default_calibration(Type type);
bool create_blank_calibration(uint num_points); // Must have at least two points
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);
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
//--------------------------------------------------
public:
static constexpr float MIN_VALID_PULSE = 1.0f;
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
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
public:
Converter() : Calibration() {}
Converter(Type type) : Calibration(type) {}
virtual ~Converter() {}
//--------------------------------------------------
// Methods
//--------------------------------------------------
public:
float min_value();
float mid_value();
float max_value();
float value_to_pulse(float value);
float value_from_pulse(float pulse);
static uint32_t pulse_to_level(float pulse, uint32_t resolution);
static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
};
}

4
drivers/servo/servo.cmake
View File

@ -3,6 +3,9 @@ add_library(${DRIVER_NAME} INTERFACE)
target_sources(${DRIVER_NAME} INTERFACE
${CMAKE_CURRENT_LIST_DIR}/servo.cpp
${CMAKE_CURRENT_LIST_DIR}/servo_cluster.cpp
${CMAKE_CURRENT_LIST_DIR}/calibration.cpp
${CMAKE_CURRENT_LIST_DIR}/servo_state.cpp
)
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
@ -10,4 +13,5 @@ target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
target_link_libraries(${DRIVER_NAME} INTERFACE
pico_stdlib
hardware_pwm
multi_pwm
)

276
drivers/servo/servo.cpp
View File

@ -1,222 +1,8 @@
#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(Type type)
: calibration(nullptr), calibration_points(0), limit_lower(true), limit_upper(true) {
create_default_calibration(type);
}
Calibration::~Calibration() {
if(calibration != nullptr) {
delete[] calibration;
calibration = nullptr;
}
}
void Calibration::create_default_calibration(Type type) {
switch(type) {
default:
case ANGULAR:
create_three_point_calibration(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);
break;
case CONTINUOUS:
create_three_point_calibration(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
-1.0f, 0.0f, +1.0f);
break;
}
}
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_two_point_calibration(float min_pulse, float max_pulse, float min_value, float max_value) {
create_blank_calibration(2);
calibration[0] = CalibrationPoint(min_pulse, min_value);
calibration[1] = CalibrationPoint(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);
calibration[0] = CalibrationPoint(min_pulse, min_value);
calibration[1] = CalibrationPoint(mid_pulse, mid_value);
calibration[2] = CalibrationPoint(max_pulse, max_value);
}
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;
}
float Converter::min_value() {
float value = 0.0f;
if(calibration_points >= 2) {
value = calibration[0].value;
}
return value;
}
float Converter::mid_value() {
float value = 0.0f;
if(calibration_points >= 2) {
value = (calibration[0].value + calibration[calibration_points - 1].value) / 2.0f;
}
return value;
}
float Converter::max_value() {
float value = 0.0f;
if(calibration_points >= 2) {
value = calibration[calibration_points - 1].value;
}
return value;
}
float Converter::value_to_pulse(float value) {
float pulse = 0.0f;
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_float(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_float(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_float(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::value_from_pulse(float pulse) {
float value = 0.0f;
if(calibration_points >= 2) {
uint8_t last = calibration_points - 1;
// Is the pulse below the bottom most calibration point?
if(pulse < calibration[0].pulse) {
// Should the pulse be limited to the calibration or projected below it?
if(limit_lower)
value = calibration[0].value;
else
value = map_float(pulse, calibration[0].pulse, calibration[1].pulse,
calibration[0].value, calibration[1].value);
}
// Is the pulse above the top most calibration point?
else if(pulse > calibration[last].pulse) {
// Should the pulse be limited to the calibration or projected above it?
if(limit_upper)
value = calibration[last].value;
else
value = map_float(pulse, calibration[last - 1].pulse, calibration[last].pulse,
calibration[last - 1].value, calibration[last].value);
}
else {
// The pulse must between two calibration points, so iterate through them to find which ones
for(uint8_t i = 0; i < last; i++) {
if(pulse <= calibration[i + 1].pulse) {
value = map_float(pulse, calibration[i].pulse, calibration[i + 1].pulse,
calibration[i].value, calibration[i + 1].value);
break; // No need to continue checking so break out of the loop
}
}
}
}
return value;
}
uint32_t Converter::pulse_to_level(float pulse, uint32_t resolution) {
// 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);
}
float Converter::map_float(float in, float in_min, float in_max, float out_min, float out_max) {
return (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
}
Servo::Servo(uint pin, Type type)
: pin(pin), converter(type) {
: pin(pin), state(type) {
}
Servo::~Servo() {
@ -239,79 +25,63 @@ namespace servo {
}
bool Servo::is_enabled() {
return enabled;
return state.is_enabled();
}
void Servo::enable() {
if(last_enabled_pulse < MIN_VALID_PULSE) {
servo_value = converter.mid_value();
last_enabled_pulse = converter.value_to_pulse(servo_value);
}
pwm_set_gpio_level(pin, (uint16_t)converter.pulse_to_level(last_enabled_pulse, 20000));
enabled = true;
float new_pulse = state.enable();
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
void Servo::disable() {
pwm_set_gpio_level(pin, 0);
enabled = false;
float new_pulse = state.disable();
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
float Servo::get_value() {
return servo_value;
return state.get_value();
}
void Servo::set_value(float value) {
servo_value = value;
float pulse = converter.value_to_pulse(value);
if(pulse >= MIN_VALID_PULSE) {
last_enabled_pulse = pulse;
pwm_set_gpio_level(pin, (uint16_t)converter.pulse_to_level(last_enabled_pulse, 20000));
enabled = true;
}
else {
disable();
}
float new_pulse = state.set_value(value);
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
float Servo::get_pulse() {
return last_enabled_pulse;
return state.get_pulse();
}
void Servo::set_pulse(float pulse) {
if(pulse >= MIN_VALID_PULSE) {
servo_value = converter.value_from_pulse(pulse);
last_enabled_pulse = pulse;
pwm_set_gpio_level(pin, (uint16_t)converter.pulse_to_level(last_enabled_pulse, 20000));
enabled = true;
}
else {
disable();
}
float new_pulse = state.set_pulse(pulse);
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
void Servo::to_min() {
set_value(converter.min_value());
float new_pulse = state.to_min();
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
void Servo::to_mid() {
set_value(converter.mid_value());
float new_pulse = state.to_mid();
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
void Servo::to_max() {
set_value(converter.max_value());
float new_pulse = state.to_max();
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
void Servo::to_percent(float in, float in_min, float in_max) {
float value = Converter::map_float(in, in_min, in_max, converter.min_value(), converter.max_value());
set_value(value);
float new_pulse = state.to_percent(in, in_min, in_max);
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
void Servo::to_percent(float in, float in_min, float in_max, float value_min, float value_max) {
float value = Converter::map_float(in, in_min, in_max, value_min, value_max);
set_value(value);
float new_pulse = state.to_percent(in, in_min, in_max, value_min, value_max);
pwm_set_gpio_level(pin, (uint16_t)Converter::pulse_to_level(new_pulse, 20000));
}
Calibration& Servo::calibration() {
return converter;
return state.calibration();
}
};

112
drivers/servo/servo.hpp
View File

@ -7,113 +7,11 @@
#include "hardware/pwm.h"
#include "hardware/clocks.h"
#include "common/pimoroni_common.hpp"
#include "calibration.hpp"
#include "servo_state.hpp"
namespace servo {
enum Type {
ANGULAR = 0,
LINEAR,
CONTINUOUS
};
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(Type type);
virtual ~Calibration();
//--------------------------------------------------
// Methods
//--------------------------------------------------
public:
void create_default_calibration(Type type);
bool create_blank_calibration(uint num_points); // Must have at least two points
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);
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
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
public:
Converter(Type type) : Calibration(type) {}
virtual ~Converter() {}
//--------------------------------------------------
// Methods
//--------------------------------------------------
public:
float min_value();
float mid_value();
float max_value();
float value_to_pulse(float value);
float value_from_pulse(float pulse);
static uint32_t pulse_to_level(float pulse, uint32_t resolution);
static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
};
class Servo {
//--------------------------------------------------
// Constants
@ -137,11 +35,7 @@ namespace servo {
uint16_t pwm_period;
float pwm_frequency = DEFAULT_PWM_FREQUENCY;
float servo_value = 0.0f;
float last_enabled_pulse = 0.0f;
bool enabled = false;
Converter converter;
ServoState state;
//--------------------------------------------------

117
drivers/servo/servo_cluster.cpp Normal file
View File

@ -0,0 +1,117 @@
#include "servo_cluster.hpp"
namespace servo {
ServoCluster::ServoCluster(PIO pio, uint sm, uint channel_mask)
: multi_pwm(pio, sm, channel_mask) {
multi_pwm.set_wrap(20000);
}
ServoCluster::~ServoCluster() {
}
bool ServoCluster::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);
// pwm_set_gpio_level(pin, 0);
return true;
}
bool ServoCluster::is_enabled(uint servo) {
if(servo < NUM_BANK0_GPIOS)
return servos[servo].is_enabled();
else
return false;
}
void ServoCluster::enable(uint servo, bool load) {
if(servo < NUM_BANK0_GPIOS) {
float new_pulse = servos[servo].enable();
multi_pwm.set_chan_level(servo, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), load);
}
}
float ServoCluster::get_value(uint servo) {
if(servo < NUM_BANK0_GPIOS)
return servos[servo].get_value();
else
return 0.0f;
}
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, Converter::pulse_to_level(new_pulse, 20000), load);
}
}
float ServoCluster::get_pulse(uint servo) {
if(servo < NUM_BANK0_GPIOS)
return servos[servo].get_pulse();
else
return 0.0f;
}
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, Converter::pulse_to_level(new_pulse, 20000), load);
}
}
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, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), 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, Converter::pulse_to_level(new_pulse, 20000), load);
}
}
Calibration* ServoCluster::calibration(uint servo) {
if(servo < NUM_BANK0_GPIOS)
return &servos[servo].calibration();
else
return nullptr;
}
};

69
drivers/servo/servo_cluster.hpp Normal file
View File

@ -0,0 +1,69 @@
#pragma once
#include <stdint.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/clocks.h"
#include "common/pimoroni_common.hpp"
#include "calibration.hpp"
#include "multi_pwm.hpp"
#include "servo_state.hpp"
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);
static constexpr float MIN_VALID_PULSE = 1.0f;
//--------------------------------------------------
// Variables
//--------------------------------------------------
private:
MultiPWM multi_pwm;
ServoState servos[NUM_BANK0_GPIOS];
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
public:
ServoCluster(PIO pio, uint sm, uint channel_mask);
~ServoCluster();
//--------------------------------------------------
// Methods
//--------------------------------------------------
public:
bool init();
bool is_enabled(uint servo);
void enable(uint servo, bool load = true);
void disable(uint servo, bool load = true);
float get_value(uint servo);
void set_value(uint servo, float value, bool load = true);
float get_pulse(uint servo);
void set_pulse(uint servo, float pulse, bool load = true);
void to_min(uint servo, bool load = true);
void to_mid(uint servo, bool load = true);
void to_max(uint servo, bool load = true);
void to_percent(uint servo, float in, float in_min = 0.0f, float in_max = 1.0f, bool load = true);
void to_percent(uint servo, float in, float in_min, float in_max, float value_min, float value_max, bool load = true);
Calibration* calibration(uint servo);
};
}

84
drivers/servo/servo_state.cpp Normal file
View File

@ -0,0 +1,84 @@
#include "servo_state.hpp"
namespace servo {
ServoState::ServoState(/*uint pin, */Type type)
: /*pin(pin), */converter(type) {
}
bool ServoState::is_enabled() {
return enabled;
}
float ServoState::enable() {
if(last_enabled_pulse < Converter::MIN_VALID_PULSE) {
servo_value = converter.mid_value();
last_enabled_pulse = converter.value_to_pulse(servo_value);
}
enabled = true;
return last_enabled_pulse;
}
float ServoState::disable() {
enabled = false;
return 0.0f; // A zero pulse
}
float ServoState::get_value() {
return servo_value;
}
float ServoState::set_value(float value) {
servo_value = value;
float pulse = converter.value_to_pulse(value);
if(pulse >= Converter::MIN_VALID_PULSE) {
last_enabled_pulse = pulse;
enabled = true;
}
else {
pulse = disable();
}
return pulse;
}
float ServoState::get_pulse() {
return last_enabled_pulse;
}
float ServoState::set_pulse(float pulse) {
if(pulse >= Converter::MIN_VALID_PULSE) {
servo_value = converter.value_from_pulse(pulse);
last_enabled_pulse = pulse;
enabled = true;
}
else {
pulse = disable();
}
return pulse;
}
float ServoState::to_min() {
return set_value(converter.min_value());
}
float ServoState::to_mid() {
return set_value(converter.mid_value());
}
float ServoState::to_max() {
return set_value(converter.max_value());
}
float ServoState::to_percent(float in, float in_min, float in_max) {
float value = Converter::map_float(in, in_min, in_max, converter.min_value(), converter.max_value());
return set_value(value);
}
float ServoState::to_percent(float in, float in_min, float in_max, float value_min, float value_max) {
float value = Converter::map_float(in, in_min, in_max, value_min, value_max);
return set_value(value);
}
Calibration& ServoState::calibration() {
return converter;
}
};

57
drivers/servo/servo_state.hpp Normal file
View File

@ -0,0 +1,57 @@
#pragma once
#include <stdint.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/clocks.h"
#include "common/pimoroni_common.hpp"
#include "calibration.hpp"
namespace servo {
class ServoState {
//--------------------------------------------------
// Variables
//--------------------------------------------------
private:
//uint pin;
float servo_value = 0.0f;
float last_enabled_pulse = 0.0f;
bool enabled = false;
Converter converter;
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
public:
ServoState(/*uint pin, */Type type = ANGULAR);
//--------------------------------------------------
// Methods
//--------------------------------------------------
public:
bool init();
bool is_enabled();
float enable();
float disable();
float get_value();
float set_value(float value);
float get_pulse();
float set_pulse(float pulse);
float to_min();
float to_mid();
float to_max();
float to_percent(float in, float in_min, float in_max);
float to_percent(float in, float in_min, float in_max, float value_min, float value_max);
Calibration& calibration();
};
}

5
examples/servo2040/servo2040_functions.cpp
View File

@ -58,6 +58,7 @@ int main() {
//MultiPWM pwms(pio1, 0, 0b111111111111111);
//pwms.set_wrap(20000);
ServoCluster cluster(pio1, 0, 0b111100);
int speed = DEFAULT_SPEED;
float offset = 0.0f;
@ -83,9 +84,11 @@ int main() {
count = 0;
//pwms.set_chan_level(servo_seq, 2000);//toggle ? 2000 : 1000);
cluster.set_pulse(servo_seq + 2, toggle ? 2000 : 1000);
//pwms.set_chan_polarity(servo_seq, toggle);
//pwms.set_chan_offset(servo_seq, toggle ? 19000 : 0);
simple_servo.set_pulse(servo_seq + 1500);
//cluster.set_pulse(0, servo_seq + 1500);
servo_seq++;
if(servo_seq >= 4) {
servo_seq = 0;
@ -95,7 +98,7 @@ int main() {
//pwms.set_clkdiv(div);
simple_servo.disable();
}
printf("Angle = %f, Pulse = %f, Enabled = %s\n", simple_servo.get_value(), simple_servo.get_pulse(), simple_servo.is_enabled() ? "true" : "false");
//printf("Angle = %f, Pulse = %f, Enabled = %s\n", simple_servo.get_value(), simple_servo.get_pulse(), simple_servo.is_enabled() ? "true" : "false");
//pwms.load_pwm();
}

1
libraries/servo2040/servo2040.hpp
View File

@ -4,6 +4,7 @@
#include "ws2812.hpp"
#include "multi_pwm.hpp"
#include "servo.hpp"
#include "servo_cluster.hpp"
namespace servo2040 {
const uint SERVO_1 = 0;