kopia lustrzana https://github.com/pimoroni/pimoroni-pico
Added initial structure of new motor classes
ZodiusInfuser
rodzic
8a36102c53
commit
e59bdc34c4
|
|
@ -1 +1,3 @@
|
|||
include(motor.cmake)
|
||||
include(motor.cmake)
|
||||
include(motor2.cmake)
|
||||
include(motor_cluster.cmake)
|
||||
|
|
@ -0,0 +1,338 @@
|
|||
#include "calibration.hpp"
|
||||
|
||||
namespace motor {
|
||||
Calibration::Pair::Pair()
|
||||
: duty(0.0f), speed(0.0f) {
|
||||
}
|
||||
|
||||
Calibration::Pair::Pair(float duty, float speed)
|
||||
: duty(duty), speed(speed) {
|
||||
}
|
||||
|
||||
Calibration::Calibration()
|
||||
: calibration(nullptr), calibration_size(0), limit_lower(true), limit_upper(true) {
|
||||
}
|
||||
|
||||
Calibration::Calibration(CalibrationType default_type)
|
||||
: Calibration() {
|
||||
apply_default_pairs(default_type);
|
||||
}
|
||||
|
||||
Calibration::Calibration(const Calibration &other)
|
||||
: calibration(nullptr), calibration_size(0), limit_lower(other.limit_lower), limit_upper(other.limit_upper) {
|
||||
uint size = other.size();
|
||||
apply_blank_pairs(size);
|
||||
for(uint i = 0; i < size; i++) {
|
||||
calibration[i] = other.calibration[i];
|
||||
}
|
||||
}
|
||||
|
||||
Calibration::~Calibration() {
|
||||
if(calibration != nullptr) {
|
||||
delete[] calibration;
|
||||
calibration = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
Calibration &Calibration::operator=(const Calibration &other) {
|
||||
uint size = other.size();
|
||||
apply_blank_pairs(size);
|
||||
for(uint i = 0; i < size; i++) {
|
||||
calibration[i] = other.calibration[i];
|
||||
}
|
||||
limit_lower = other.limit_lower;
|
||||
limit_upper = other.limit_upper;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Calibration::Pair &Calibration::operator[](uint8_t index) {
|
||||
assert(index < calibration_size);
|
||||
return calibration[index];
|
||||
}
|
||||
|
||||
const Calibration::Pair &Calibration::operator[](uint8_t index) const {
|
||||
assert(index < calibration_size);
|
||||
return calibration[index];
|
||||
}
|
||||
|
||||
void Calibration::apply_blank_pairs(uint size) {
|
||||
if(calibration != nullptr) {
|
||||
delete[] calibration;
|
||||
}
|
||||
|
||||
if(size > 0) {
|
||||
calibration = new Pair[size];
|
||||
calibration_size = size;
|
||||
}
|
||||
else {
|
||||
calibration = nullptr;
|
||||
calibration_size = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void Calibration::apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed) {
|
||||
apply_blank_pairs(2);
|
||||
calibration[0] = Pair(min_duty, min_speed);
|
||||
calibration[1] = Pair(max_duty, max_speed);
|
||||
}
|
||||
|
||||
void Calibration::apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed) {
|
||||
apply_blank_pairs(3);
|
||||
calibration[0] = Pair(min_duty, min_speed);
|
||||
calibration[1] = Pair(mid_duty, mid_speed);
|
||||
calibration[2] = Pair(max_duty, max_speed);
|
||||
}
|
||||
|
||||
void Calibration::apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed) {
|
||||
apply_blank_pairs(size);
|
||||
if(size > 0) {
|
||||
float size_minus_one = (float)(size - 1);
|
||||
for(uint i = 0; i < size; i++) {
|
||||
float duty = Calibration::map_float((float)i, 0.0f, size_minus_one, min_duty, max_duty);
|
||||
float speed = Calibration::map_float((float)i, 0.0f, size_minus_one, min_speed, max_speed);
|
||||
calibration[i] = Pair(duty, speed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Calibration::apply_default_pairs(CalibrationType default_type) {
|
||||
switch(default_type) {
|
||||
default:
|
||||
case ANGULAR:
|
||||
apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
|
||||
-90.0f, 0.0f, +90.0f);
|
||||
break;
|
||||
case LINEAR:
|
||||
apply_two_pairs(DEFAULT_MIN_PULSE, DEFAULT_MAX_PULSE,
|
||||
0.0f, 1.0f);
|
||||
break;
|
||||
case CONTINUOUS:
|
||||
apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE,
|
||||
-1.0f, 0.0f, +1.0f);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
uint Calibration::size() const {
|
||||
return calibration_size;
|
||||
}
|
||||
|
||||
Calibration::Pair &Calibration::pair(uint8_t index) {
|
||||
assert(index < calibration_size);
|
||||
return calibration[index];
|
||||
}
|
||||
|
||||
const Calibration::Pair &Calibration::pair(uint8_t index) const {
|
||||
assert(index < calibration_size);
|
||||
return calibration[index];
|
||||
}
|
||||
|
||||
float Calibration::duty(uint8_t index) const {
|
||||
return pair(index).duty;
|
||||
}
|
||||
|
||||
void Calibration::duty(uint8_t index, float duty) {
|
||||
pair(index).duty = duty;
|
||||
}
|
||||
|
||||
float Calibration::speed(uint8_t index) const {
|
||||
return pair(index).speed;
|
||||
}
|
||||
|
||||
void Calibration::speed(uint8_t index, float speed) {
|
||||
pair(index).speed = speed;
|
||||
}
|
||||
|
||||
Calibration::Pair &Calibration::first() {
|
||||
assert(calibration_size > 0);
|
||||
return calibration[0];
|
||||
}
|
||||
|
||||
const Calibration::Pair &Calibration::first() const {
|
||||
assert(calibration_size > 0);
|
||||
return calibration[0];
|
||||
}
|
||||
|
||||
float Calibration::first_duty() const {
|
||||
return first().duty;
|
||||
}
|
||||
|
||||
void Calibration::first_duty(float duty) {
|
||||
first().duty = duty;
|
||||
}
|
||||
|
||||
float Calibration::first_speed() const {
|
||||
return first().speed;
|
||||
}
|
||||
|
||||
void Calibration::first_speed(float speed) {
|
||||
first().speed = speed;
|
||||
}
|
||||
|
||||
Calibration::Pair &Calibration::last() {
|
||||
assert(calibration_size > 0);
|
||||
return calibration[calibration_size - 1];
|
||||
}
|
||||
|
||||
const Calibration::Pair &Calibration::last() const {
|
||||
assert(calibration_size > 0);
|
||||
return calibration[calibration_size - 1];
|
||||
}
|
||||
|
||||
float Calibration::last_duty() const {
|
||||
return last().duty;
|
||||
}
|
||||
|
||||
void Calibration::last_duty(float duty) {
|
||||
last().duty = duty;
|
||||
}
|
||||
|
||||
float Calibration::last_speed() const {
|
||||
return last().speed;
|
||||
}
|
||||
|
||||
void Calibration::last_speed(float speed) {
|
||||
last().speed = speed;
|
||||
}
|
||||
|
||||
bool Calibration::has_lower_limit() const {
|
||||
return limit_lower;
|
||||
}
|
||||
|
||||
bool Calibration::has_upper_limit() const {
|
||||
return limit_upper;
|
||||
}
|
||||
|
||||
void Calibration::limit_to_calibration(bool lower, bool upper) {
|
||||
limit_lower = lower;
|
||||
limit_upper = upper;
|
||||
}
|
||||
|
||||
bool Calibration::speed_to_duty(float speed, float &duty_out, float &speed_out) const {
|
||||
bool success = false;
|
||||
if(calibration_size >= 2) {
|
||||
uint8_t last = calibration_size - 1;
|
||||
|
||||
speed_out = speed;
|
||||
|
||||
// Is the speed below the bottom most calibration pair?
|
||||
if(speed < calibration[0].speed) {
|
||||
// Should the speed be limited to the calibration or projected below it?
|
||||
if(limit_lower) {
|
||||
duty_out = calibration[0].duty;
|
||||
speed_out = calibration[0].speed;
|
||||
}
|
||||
else {
|
||||
duty_out = map_float(speed, calibration[0].speed, calibration[1].speed,
|
||||
calibration[0].duty, calibration[1].duty);
|
||||
}
|
||||
}
|
||||
// Is the speed above the top most calibration pair?
|
||||
else if(speed > calibration[last].speed) {
|
||||
// Should the speed be limited to the calibration or projected above it?
|
||||
if(limit_upper) {
|
||||
duty_out = calibration[last].duty;
|
||||
speed_out = calibration[last].speed;
|
||||
}
|
||||
else {
|
||||
duty_out = map_float(speed, calibration[last - 1].speed, calibration[last].speed,
|
||||
calibration[last - 1].duty, calibration[last].duty);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// The speed must between two calibration pairs, so iterate through them to find which ones
|
||||
for(uint8_t i = 0; i < last; i++) {
|
||||
if(speed <= calibration[i + 1].speed) {
|
||||
duty_out = map_float(speed, calibration[i].speed, calibration[i + 1].speed,
|
||||
calibration[i].duty, calibration[i + 1].duty);
|
||||
break; // No need to continue checking so break out of the loop
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Clamp the duty between the hard limits
|
||||
if(duty_out < LOWER_HARD_LIMIT || duty_out > UPPER_HARD_LIMIT) {
|
||||
duty_out = MIN(MAX(duty_out, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT);
|
||||
|
||||
// Is the duty below the bottom most calibration pair?
|
||||
if(duty_out < calibration[0].duty) {
|
||||
speed_out = map_float(duty_out, calibration[0].duty, calibration[1].duty,
|
||||
calibration[0].speed, calibration[1].speed);
|
||||
}
|
||||
// Is the duty above the top most calibration pair?
|
||||
else if(duty_out > calibration[last].duty) {
|
||||
speed_out = map_float(duty_out, calibration[last - 1].duty, calibration[last].duty,
|
||||
calibration[last - 1].speed, calibration[last].speed);
|
||||
}
|
||||
else {
|
||||
// The duty must between two calibration pairs, so iterate through them to find which ones
|
||||
for(uint8_t i = 0; i < last; i++) {
|
||||
if(duty_out <= calibration[i + 1].duty) {
|
||||
speed_out = map_float(duty_out, calibration[i].duty, calibration[i + 1].duty,
|
||||
calibration[i].speed, calibration[i + 1].speed);
|
||||
break; // No need to continue checking so break out of the loop
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
success = true;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
bool Calibration::duty_to_speed(float duty, float &speed_out, float &duty_out) const {
|
||||
bool success = false;
|
||||
if(calibration_size >= 2) {
|
||||
uint8_t last = calibration_size - 1;
|
||||
|
||||
// Clamp the duty between the hard limits
|
||||
duty_out = MIN(MAX(duty, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT);
|
||||
|
||||
// Is the duty below the bottom most calibration pair?
|
||||
if(duty_out < calibration[0].duty) {
|
||||
// Should the duty be limited to the calibration or projected below it?
|
||||
if(limit_lower) {
|
||||
speed_out = calibration[0].speed;
|
||||
duty_out = calibration[0].duty;
|
||||
}
|
||||
else {
|
||||
speed_out = map_float(duty, calibration[0].duty, calibration[1].duty,
|
||||
calibration[0].speed, calibration[1].speed);
|
||||
}
|
||||
}
|
||||
// Is the duty above the top most calibration pair?
|
||||
else if(duty > calibration[last].duty) {
|
||||
// Should the duty be limited to the calibration or projected above it?
|
||||
if(limit_upper) {
|
||||
speed_out = calibration[last].speed;
|
||||
duty_out = calibration[last].duty;
|
||||
}
|
||||
else {
|
||||
speed_out = map_float(duty, calibration[last - 1].duty, calibration[last].duty,
|
||||
calibration[last - 1].speed, calibration[last].speed);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// The duty must between two calibration pairs, so iterate through them to find which ones
|
||||
for(uint8_t i = 0; i < last; i++) {
|
||||
if(duty <= calibration[i + 1].duty) {
|
||||
speed_out = map_float(duty, calibration[i].duty, calibration[i + 1].duty,
|
||||
calibration[i].speed, calibration[i + 1].speed);
|
||||
break; // No need to continue checking so break out of the loop
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
success = true;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
float Calibration::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;
|
||||
}
|
||||
};
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
#pragma once
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
namespace motor {
|
||||
|
||||
enum CalibrationType {
|
||||
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
|
||||
|
||||
private:
|
||||
static constexpr float LOWER_HARD_LIMIT = 400.0f; // The minimum microsecond duty to send
|
||||
static constexpr float UPPER_HARD_LIMIT = 2600.0f; // The maximum microsecond duty to send
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Substructures
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
struct Pair {
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
Pair();
|
||||
Pair(float duty, float speed);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
float duty;
|
||||
float speed;
|
||||
};
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Calibration();
|
||||
Calibration(CalibrationType default_type);
|
||||
Calibration(const Calibration &other);
|
||||
virtual ~Calibration();
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Operators
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Calibration &operator=(const Calibration &other);
|
||||
Pair &operator[](uint8_t index);
|
||||
const Pair &operator[](uint8_t index) const;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
void apply_blank_pairs(uint size);
|
||||
void apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed);
|
||||
void apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed);
|
||||
void apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed);
|
||||
void apply_default_pairs(CalibrationType default_type);
|
||||
|
||||
uint size() const;
|
||||
|
||||
Pair &pair(uint8_t index); // Ensure the pairs are assigned in ascending speed order
|
||||
const Pair &pair(uint8_t index) const; // Ensure the pairs are assigned in ascending speed order
|
||||
float duty(uint8_t index) const;
|
||||
void duty(uint8_t index, float duty);
|
||||
float speed(uint8_t index) const;
|
||||
void speed(uint8_t index, float speed);
|
||||
|
||||
Pair &first();
|
||||
const Pair &first() const;
|
||||
float first_duty() const;
|
||||
void first_duty(float duty);
|
||||
float first_speed() const;
|
||||
void first_speed(float speed);
|
||||
|
||||
Pair &last();
|
||||
const Pair &last() const;
|
||||
float last_duty() const;
|
||||
void last_duty(float duty);
|
||||
float last_speed() const;
|
||||
void last_speed(float speed);
|
||||
|
||||
bool has_lower_limit() const;
|
||||
bool has_upper_limit() const;
|
||||
void limit_to_calibration(bool lower, bool upper);
|
||||
|
||||
bool speed_to_duty(float speed, float &duty_out, float &speed_out) const;
|
||||
bool duty_to_speed(float duty, float &speed_out, float &duty_out) const;
|
||||
|
||||
//static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
Pair* calibration;
|
||||
uint calibration_size;
|
||||
bool limit_lower;
|
||||
bool limit_upper;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
set(DRIVER_NAME motor2)
|
||||
add_library(${DRIVER_NAME} INTERFACE)
|
||||
|
||||
target_sources(${DRIVER_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor2.cpp
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
target_link_libraries(${DRIVER_NAME} INTERFACE
|
||||
pico_stdlib
|
||||
hardware_pwm
|
||||
pwm
|
||||
)
|
||||
|
|
@ -0,0 +1,211 @@
|
|||
#include "motor2.hpp"
|
||||
#include "hardware/clocks.h"
|
||||
#include "pwm.hpp"
|
||||
|
||||
namespace motor {
|
||||
Motor2::Motor2(uint pin_pos, uint pin_neg, float freq, MotorState::DecayMode mode)
|
||||
: motor_pin_pos(pin_pos), motor_pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) {
|
||||
}
|
||||
|
||||
// Motor2::Motor2(uint pin, /*const Calibration& calibration,*/ float freq)
|
||||
// : motor_pin_pos(pin), /*state(calibration),*/ pwm_frequency(freq) {
|
||||
// }
|
||||
|
||||
Motor2::~Motor2() {
|
||||
gpio_set_function(motor_pin_pos, GPIO_FUNC_NULL);
|
||||
gpio_set_function(motor_pin_neg, GPIO_FUNC_NULL);
|
||||
}
|
||||
|
||||
bool Motor2::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(motor_pin_pos), &pwm_cfg, true);
|
||||
gpio_set_function(motor_pin_pos, GPIO_FUNC_PWM);
|
||||
|
||||
pwm_init(pwm_gpio_to_slice_num(motor_pin_neg), &pwm_cfg, true);
|
||||
gpio_set_function(motor_pin_neg, GPIO_FUNC_PWM);
|
||||
|
||||
pwm_set_gpio_level(motor_pin_pos, 0);
|
||||
pwm_set_gpio_level(motor_pin_neg, 0);
|
||||
|
||||
success = true;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
uint Motor2::pin() const {
|
||||
return motor_pin_pos;
|
||||
}
|
||||
|
||||
void Motor2::enable() {
|
||||
apply_duty(state.enable());
|
||||
}
|
||||
|
||||
void Motor2::disable() {
|
||||
apply_duty(state.disable());
|
||||
}
|
||||
|
||||
bool Motor2::is_enabled() const {
|
||||
return state.is_enabled();
|
||||
}
|
||||
|
||||
// float Motor2::duty() const {
|
||||
// return state.get_duty();
|
||||
// }
|
||||
|
||||
// void Motor2::duty(float duty) {
|
||||
// apply_duty(state.set_duty_with_return(duty));
|
||||
// }
|
||||
|
||||
float Motor2::speed() const {
|
||||
return state.get_speed();
|
||||
}
|
||||
|
||||
void Motor2::speed(float speed) {
|
||||
apply_duty(state.set_speed_with_return(speed));
|
||||
}
|
||||
|
||||
float Motor2::frequency() const {
|
||||
return pwm_frequency;
|
||||
}
|
||||
|
||||
bool Motor2::frequency(float freq) {
|
||||
bool success = false;
|
||||
|
||||
if((freq >= MotorState::MIN_FREQUENCY) && (freq <= MotorState::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 speeds 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(motor_pin_pos);
|
||||
|
||||
// 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(state.is_enabled() && pre_update_pwm) {
|
||||
apply_duty(state.get_duty());
|
||||
}
|
||||
|
||||
// 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(state.is_enabled() && !pre_update_pwm) {
|
||||
apply_duty(state.get_duty());
|
||||
}
|
||||
|
||||
success = true;
|
||||
}
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
MotorState::DecayMode Motor2::decay_mode() {
|
||||
return motor_decay_mode;
|
||||
}
|
||||
|
||||
void Motor2::decay_mode(MotorState::DecayMode mode) {
|
||||
motor_decay_mode = mode;
|
||||
apply_duty(state.get_duty());
|
||||
}
|
||||
|
||||
void Motor2::stop() {
|
||||
apply_duty(state.set_speed_with_return(0.0f));
|
||||
}
|
||||
|
||||
void Motor2::coast() {
|
||||
disable();
|
||||
}
|
||||
|
||||
float Motor2::min_speed() const {
|
||||
return state.get_min_speed();
|
||||
}
|
||||
|
||||
// float Motor2::mid_speed() const {
|
||||
// return state.get_mid_speed();
|
||||
// }
|
||||
|
||||
float Motor2::max_speed() const {
|
||||
return state.get_max_speed();
|
||||
}
|
||||
|
||||
void Motor2::to_min() {
|
||||
apply_duty(state.to_min_with_return());
|
||||
}
|
||||
|
||||
// void Motor2::to_mid() {
|
||||
// apply_duty(state.to_mid_with_return());
|
||||
// }
|
||||
|
||||
void Motor2::to_max() {
|
||||
apply_duty(state.to_max_with_return());
|
||||
}
|
||||
|
||||
void Motor2::to_percent(float in, float in_min, float in_max) {
|
||||
apply_duty(state.to_percent_with_return(in, in_min, in_max));
|
||||
}
|
||||
|
||||
void Motor2::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) {
|
||||
apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max));
|
||||
}
|
||||
|
||||
// Calibration& Motor2::calibration() {
|
||||
// return state.calibration();
|
||||
// }
|
||||
|
||||
// const Calibration& Motor2::calibration() const {
|
||||
// return state.calibration();
|
||||
// }
|
||||
|
||||
void Motor2::apply_duty(float duty) {
|
||||
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
|
||||
|
||||
switch(motor_decay_mode) {
|
||||
case MotorState::SLOW_DECAY: //aka 'Braking'
|
||||
if(signed_level >= 0) {
|
||||
pwm_set_gpio_level(motor_pin_pos, pwm_period);
|
||||
pwm_set_gpio_level(motor_pin_neg, pwm_period - signed_level);
|
||||
}
|
||||
else {
|
||||
pwm_set_gpio_level(motor_pin_pos, pwm_period + signed_level);
|
||||
pwm_set_gpio_level(motor_pin_neg, pwm_period);
|
||||
}
|
||||
break;
|
||||
|
||||
case MotorState::FAST_DECAY: //aka 'Coasting'
|
||||
default:
|
||||
if(signed_level >= 0) {
|
||||
pwm_set_gpio_level(motor_pin_pos, signed_level);
|
||||
pwm_set_gpio_level(motor_pin_neg, 0);
|
||||
}
|
||||
else {
|
||||
pwm_set_gpio_level(motor_pin_pos, 0);
|
||||
pwm_set_gpio_level(motor_pin_neg, 0 - signed_level);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
#pragma once
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "motor_state.hpp"
|
||||
|
||||
namespace motor {
|
||||
|
||||
class Motor2 {
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
uint motor_pin_pos;
|
||||
uint motor_pin_neg;
|
||||
MotorState state;
|
||||
pwm_config pwm_cfg;
|
||||
uint16_t pwm_period;
|
||||
float pwm_frequency;
|
||||
MotorState::DecayMode motor_decay_mode;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Motor2(uint pin_pos, uint pin_neg, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE);
|
||||
//Motor2(uint pin, /*const Calibration& calibration,*/ float freq = MotorState::DEFAULT_FREQUENCY);
|
||||
~Motor2();
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
bool init();
|
||||
|
||||
// For print access in micropython
|
||||
uint pin() const;
|
||||
|
||||
void enable();
|
||||
void disable();
|
||||
bool is_enabled() const;
|
||||
|
||||
//float duty() const;
|
||||
//void duty(float duty);
|
||||
|
||||
float speed() const;
|
||||
void speed(float speed);
|
||||
|
||||
float frequency() const;
|
||||
bool frequency(float freq);
|
||||
|
||||
MotorState::DecayMode decay_mode();
|
||||
void decay_mode(MotorState::DecayMode mode);
|
||||
|
||||
void stop();
|
||||
void coast(); // An alias for disable
|
||||
|
||||
//--------------------------------------------------
|
||||
float min_speed() const;
|
||||
//float mid_speed() const;
|
||||
float max_speed() const;
|
||||
|
||||
void to_min();
|
||||
//void to_mid();
|
||||
void to_max();
|
||||
void to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT);
|
||||
void to_percent(float in, float in_min, float in_max, float speed_min, float speed_max);
|
||||
|
||||
//Calibration& calibration();
|
||||
//const Calibration& calibration() const;
|
||||
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
void apply_duty(float duty);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
set(DRIVER_NAME motor_cluster)
|
||||
add_library(${DRIVER_NAME} INTERFACE)
|
||||
|
||||
target_sources(${DRIVER_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor_cluster.cpp
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
target_link_libraries(${DRIVER_NAME} INTERFACE
|
||||
pico_stdlib
|
||||
pwm_cluster
|
||||
)
|
||||
|
|
@ -0,0 +1,527 @@
|
|||
#include "motor_cluster.hpp"
|
||||
#include "pwm.hpp"
|
||||
#include <cstdio>
|
||||
|
||||
namespace motor {
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(default_type, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(default_type, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(default_type, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(default_type, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(calibration, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(calibration, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(calibration, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(calibration, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::~MotorCluster() {
|
||||
delete[] states;
|
||||
delete[] motor_phases;
|
||||
}
|
||||
|
||||
bool MotorCluster::init() {
|
||||
bool success = false;
|
||||
|
||||
if(pwms.init()) {
|
||||
// Calculate a suitable pwm wrap period for this frequency
|
||||
uint32_t period; uint32_t div256;
|
||||
if(pimoroni::PWMCluster::calculate_pwm_factors(pwm_frequency, period, div256)) {
|
||||
pwm_period = period;
|
||||
|
||||
// Update the pwm before setting the new wrap
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
pwms.set_chan_level(motor, 0, false);
|
||||
pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
|
||||
}
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
pwms.set_wrap(pwm_period, true); // 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 speeds to avoid a lockup condition
|
||||
uint8_t div = div256 >> 8;
|
||||
uint8_t mod = div256 % 256;
|
||||
pwms.set_clkdiv_int_frac(div, mod);
|
||||
|
||||
success = true;
|
||||
}
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
uint8_t MotorCluster::count() const {
|
||||
return pwms.get_chan_count();
|
||||
}
|
||||
|
||||
uint8_t MotorCluster::pin(uint8_t motor) const {
|
||||
return pwms.get_chan_pin(motor);
|
||||
}
|
||||
|
||||
void MotorCluster::enable(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
float new_pulse = states[motor].enable();
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
}
|
||||
|
||||
void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
enable(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::enable(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
enable(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::enable_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
enable(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::disable(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
float new_pulse = states[motor].disable();
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
}
|
||||
|
||||
void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
disable(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::disable(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
disable(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::disable_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
disable(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
bool MotorCluster::is_enabled(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
return states[motor].is_enabled();
|
||||
}
|
||||
|
||||
float MotorCluster::pulse(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
return states[motor].get_pulse();
|
||||
}
|
||||
|
||||
void MotorCluster::pulse(uint8_t motor, float pulse, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
float new_pulse = states[motor].set_pulse_with_return(pulse);
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
}
|
||||
|
||||
void MotorCluster::pulse(const uint8_t *motors, uint8_t length, float pulse, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->pulse(motors[i], pulse, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::pulse(std::initializer_list<uint8_t> motors, float pulse, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->pulse(motor, pulse, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_pulse(float pulse, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->pulse(motor, pulse, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
float MotorCluster::speed(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
return states[motor].get_speed();
|
||||
}
|
||||
|
||||
void MotorCluster::speed(uint8_t motor, float speed, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
float new_pulse = states[motor].set_speed_with_return(speed);
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
}
|
||||
|
||||
void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->speed(motors[i], speed, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::speed(std::initializer_list<uint8_t> motors, float speed, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->speed(motor, speed, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_speed(float speed, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->speed(motor, speed, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
float MotorCluster::phase(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
return motor_phases[motor];
|
||||
}
|
||||
|
||||
void MotorCluster::phase(uint8_t motor, float phase, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
motor_phases[motor] = MIN(MAX(phase, 0.0f), 1.0f);
|
||||
pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwms.get_wrap()), load);
|
||||
}
|
||||
|
||||
void MotorCluster::phase(const uint8_t *motors, uint8_t length, float phase, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->phase(motors[i], phase, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::phase(std::initializer_list<uint8_t> motors, float phase, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->phase(motor, phase, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_phase(float phase, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->phase(motor, phase, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
float MotorCluster::frequency() const {
|
||||
return pwm_frequency;
|
||||
}
|
||||
|
||||
bool MotorCluster::frequency(float freq) {
|
||||
bool success = false;
|
||||
|
||||
if((freq >= MotorState::MIN_FREQUENCY) && (freq <= MotorState::MAX_FREQUENCY)) {
|
||||
// Calculate a suitable pwm wrap period for this frequency
|
||||
uint32_t period; uint32_t div256;
|
||||
if(pimoroni::PWMCluster::calculate_pwm_factors(freq, period, div256)) {
|
||||
|
||||
pwm_period = period;
|
||||
pwm_frequency = freq;
|
||||
|
||||
// Update the pwm before setting the new wrap
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint motor = 0; motor < motor_count; motor++) {
|
||||
if(states[motor].is_enabled()) {
|
||||
apply_pulse(motor, states[motor].get_pulse(), false);
|
||||
}
|
||||
pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), 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
|
||||
uint16_t div = div256 >> 8;
|
||||
uint8_t mod = div256 % 256;
|
||||
pwms.set_clkdiv_int_frac(div, mod);
|
||||
|
||||
success = true;
|
||||
}
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
float MotorCluster::min_speed(uint8_t motor) const {
|
||||
assert(is_assigned(motor));
|
||||
return states[motor].get_min_speed();
|
||||
}
|
||||
|
||||
float MotorCluster::mid_speed(uint8_t motor) const {
|
||||
assert(is_assigned(motor));
|
||||
return states[motor].get_mid_speed();
|
||||
}
|
||||
|
||||
float MotorCluster::max_speed(uint8_t motor) const {
|
||||
assert(is_assigned(motor));
|
||||
return states[motor].get_max_speed();
|
||||
}
|
||||
|
||||
void MotorCluster::to_min(uint8_t motor, bool load) {
|
||||
assert(is_assigned(motor));
|
||||
float new_pulse = states[motor].to_min_with_return();
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_min(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_min(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_min(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
to_min(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_min(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_min(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_mid(uint8_t motor, bool load) {
|
||||
assert(is_assigned(motor));
|
||||
float new_pulse = states[motor].to_mid_with_return();
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_mid(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_mid(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_mid(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
to_mid(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_mid(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_mid(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_max(uint8_t motor, bool load) {
|
||||
assert(is_assigned(motor));
|
||||
float new_pulse = states[motor].to_max_with_return();
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_max(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_max(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_max(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
to_max(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_max(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_max(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) {
|
||||
assert(is_assigned(motor));
|
||||
float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max);
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_percent(motors[i], in, in_min, in_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(std::initializer_list<uint8_t> motors, float in, float in_min, float in_max, bool load) {
|
||||
for(auto motor : motors) {
|
||||
to_percent(motor, in, in_min, in_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_percent(float in, float in_min, float in_max, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_percent(motor, in, in_min, in_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
assert(is_assigned(motor));
|
||||
float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max);
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_percent(motors[i], in, in_min, in_max, speed_min, speed_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(std::initializer_list<uint8_t> motors, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
for(auto motor : motors) {
|
||||
to_percent(motor, in, in_min, in_max, speed_min, speed_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_percent(float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_percent(motor, in, in_min, in_max, speed_min, speed_max, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
Calibration& MotorCluster::calibration(uint8_t motor) {
|
||||
assert(is_assigned(motor));
|
||||
return states[motor].calibration();
|
||||
}
|
||||
|
||||
const Calibration& MotorCluster::calibration(uint8_t motor) const {
|
||||
assert(is_assigned(motor));
|
||||
return states[motor].calibration();
|
||||
}
|
||||
|
||||
void MotorCluster::load() {
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::apply_pulse(uint8_t motor, float pulse, bool load) {
|
||||
pwms.set_chan_level(motor, MotorState::pulse_to_level(pulse, pwm_period, pwm_frequency), load);
|
||||
}
|
||||
|
||||
void MotorCluster::create_motor_states(CalibrationType default_type, bool auto_phase) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
if(motor_count > 0) {
|
||||
states = new MotorState[motor_count];
|
||||
motor_phases = new float[motor_count];
|
||||
|
||||
for(uint motor = 0; motor < motor_count; motor++) {
|
||||
states[motor] = MotorState(default_type);
|
||||
motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::create_motor_states(const Calibration& calibration, bool auto_phase) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
if(motor_count > 0) {
|
||||
states = new MotorState[motor_count];
|
||||
motor_phases = new float[motor_count];
|
||||
|
||||
for(uint motor = 0; motor < motor_count; motor++) {
|
||||
states[motor] = MotorState(calibration);
|
||||
motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
@ -0,0 +1,123 @@
|
|||
#pragma once
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "pwm_cluster.hpp"
|
||||
#include "motor_state.hpp"
|
||||
|
||||
using namespace pimoroni;
|
||||
|
||||
namespace motor {
|
||||
|
||||
class MotorCluster {
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
PWMCluster pwms;
|
||||
uint32_t pwm_period;
|
||||
float pwm_frequency;
|
||||
MotorState* states;
|
||||
float* motor_phases;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
|
||||
MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
~MotorCluster();
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
bool init();
|
||||
|
||||
uint8_t count() const;
|
||||
uint8_t pin(uint8_t motor) const;
|
||||
|
||||
void enable(uint8_t motor, bool load = true);
|
||||
void enable(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void enable(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void enable_all(bool load = true);
|
||||
|
||||
void disable(uint8_t motor, bool load = true);
|
||||
void disable(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void disable(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void disable_all(bool load = true);
|
||||
|
||||
bool is_enabled(uint8_t motor) const;
|
||||
|
||||
float duty(uint8_t motor) const;
|
||||
void duty(uint8_t motor, float duty, bool load = true);
|
||||
void duty(const uint8_t *motors, uint8_t length, float duty, bool load = true);
|
||||
void duty(std::initializer_list<uint8_t> motors, float duty, bool load = true);
|
||||
void all_to_duty(float duty, bool load = true);
|
||||
|
||||
float speed(uint8_t motor) const;
|
||||
void speed(uint8_t motor, float speed, bool load = true);
|
||||
void speed(const uint8_t *motors, uint8_t length, float speed, bool load = true);
|
||||
void speed(std::initializer_list<uint8_t> motors, float speed, bool load = true);
|
||||
void all_to_speed(float speed, bool load = true);
|
||||
|
||||
float phase(uint8_t motor) const;
|
||||
void phase(uint8_t motor, float phase, bool load = true);
|
||||
void phase(const uint8_t *motors, uint8_t length, float phase, bool load = true);
|
||||
void phase(std::initializer_list<uint8_t> motors, float phase, bool load = true);
|
||||
void all_to_phase(float phase, bool load = true);
|
||||
|
||||
float frequency() const;
|
||||
bool frequency(float freq);
|
||||
|
||||
//--------------------------------------------------
|
||||
float min_speed(uint8_t motor) const;
|
||||
float mid_speed(uint8_t motor) const;
|
||||
float max_speed(uint8_t motor) const;
|
||||
|
||||
void to_min(uint8_t motor, bool load = true);
|
||||
void to_min(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void to_min(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_to_min(bool load = true);
|
||||
|
||||
void to_mid(uint8_t motor, bool load = true);
|
||||
void to_mid(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void to_mid(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_to_mid(bool load = true);
|
||||
|
||||
void to_max(uint8_t motor, bool load = true);
|
||||
void to_max(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void to_max(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_to_max(bool load = true);
|
||||
|
||||
void to_percent(uint8_t motor, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
void to_percent(const uint8_t *motors, uint8_t length, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
void to_percent(std::initializer_list<uint8_t> motors, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
void all_to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
|
||||
void to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
|
||||
void to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
|
||||
void to_percent(std::initializer_list<uint8_t> motors, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
|
||||
void all_to_percent(float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true);
|
||||
|
||||
Calibration& calibration(uint8_t motor);
|
||||
const Calibration& calibration(uint8_t motor) const;
|
||||
|
||||
void load();
|
||||
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
void apply_duty(uint8_t motor, float duty, bool load);
|
||||
void create_motor_states(CalibrationType default_type, bool auto_phase);
|
||||
void create_motor_states(const Calibration& calibration, bool auto_phase);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,129 @@
|
|||
#include "motor_state.hpp"
|
||||
|
||||
namespace motor {
|
||||
MotorState::MotorState() {
|
||||
}
|
||||
|
||||
//MotorState::MotorState(CalibrationType default_type)
|
||||
// : calib(default_type) {
|
||||
//}
|
||||
|
||||
//MotorState::MotorState(const Calibration& calibration)
|
||||
// : calib(calibration) {
|
||||
//}
|
||||
|
||||
float MotorState::enable() {
|
||||
return _enable();
|
||||
}
|
||||
|
||||
float MotorState::disable() {
|
||||
enabled = false;
|
||||
return 0.0f; // A zero duty
|
||||
}
|
||||
|
||||
float MotorState::_enable() {
|
||||
enabled = true;
|
||||
return last_enabled_duty;
|
||||
}
|
||||
|
||||
bool MotorState::is_enabled() const {
|
||||
return enabled;
|
||||
}
|
||||
|
||||
float MotorState::get_duty() const {
|
||||
return last_enabled_duty;
|
||||
}
|
||||
|
||||
float MotorState::set_duty_with_return(float duty) {
|
||||
//TODO
|
||||
// float speed_out, duty_out;
|
||||
// if(calib.duty_to_speed(duty, speed_out, duty_out)) {
|
||||
// motor_speed = speed_out;
|
||||
// last_enabled_duty = duty_out;
|
||||
// return _enable();
|
||||
// }
|
||||
return disable();
|
||||
}
|
||||
|
||||
float MotorState::get_speed() const {
|
||||
return motor_speed;
|
||||
}
|
||||
|
||||
float MotorState::set_speed_with_return(float speed) {
|
||||
//TODO
|
||||
// float duty_out, speed_out;
|
||||
// if(calib.speed_to_duty(speed, duty_out, speed_out)) {
|
||||
// last_enabled_duty = duty_out;
|
||||
// motor_speed = speed_out;
|
||||
// return _enable();
|
||||
// }
|
||||
return disable();
|
||||
}
|
||||
|
||||
float MotorState::get_min_speed() const {
|
||||
float speed = 0.0f;
|
||||
//TODO
|
||||
//if(calib.size() > 0) {
|
||||
// speed = calib.first().speed;
|
||||
//}
|
||||
return speed;
|
||||
}
|
||||
|
||||
// float MotorState::get_mid_speed() const {
|
||||
// float speed = 0.0f;
|
||||
// if(calib.size() > 0) {
|
||||
// const Calibration::Pair &first = calib.first();
|
||||
// const Calibration::Pair &last = calib.last();
|
||||
// speed = (first.speed + last.speed) / 2.0f;
|
||||
// }
|
||||
// return speed;
|
||||
// }
|
||||
|
||||
float MotorState::get_max_speed() const {
|
||||
float speed = 0.0f;
|
||||
//TODO
|
||||
//if(calib.size() > 0) {
|
||||
// speed = calib.last().speed;
|
||||
//}
|
||||
return speed;
|
||||
}
|
||||
|
||||
float MotorState::to_min_with_return() {
|
||||
return set_speed_with_return(get_min_speed());
|
||||
}
|
||||
|
||||
// float MotorState::to_mid_with_return() {
|
||||
// return set_speed_with_return(get_mid_speed());
|
||||
// }
|
||||
|
||||
float MotorState::to_max_with_return() {
|
||||
return set_speed_with_return(get_max_speed());
|
||||
}
|
||||
|
||||
float MotorState::to_percent_with_return(float in, float in_min, float in_max) {
|
||||
float speed = MotorState::map_float(in, in_min, in_max, get_min_speed(), get_max_speed());
|
||||
return set_speed_with_return(speed);
|
||||
}
|
||||
|
||||
float MotorState::to_percent_with_return(float in, float in_min, float in_max, float speed_min, float speed_max) {
|
||||
float speed = MotorState::map_float(in, in_min, in_max, speed_min, speed_max);
|
||||
return set_speed_with_return(speed);
|
||||
}
|
||||
|
||||
// Calibration& MotorState::calibration() {
|
||||
// return calib;
|
||||
// }
|
||||
|
||||
// const Calibration& MotorState::calibration() const {
|
||||
// return calib;
|
||||
// }
|
||||
|
||||
int32_t MotorState::duty_to_level(float duty, uint32_t resolution) {
|
||||
return (int32_t)(duty * (float)resolution);
|
||||
}
|
||||
|
||||
float MotorState::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;
|
||||
}
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,90 @@
|
|||
#pragma once
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "calibration.hpp"
|
||||
|
||||
namespace motor {
|
||||
|
||||
class MotorState {
|
||||
//--------------------------------------------------
|
||||
// Enums
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
enum DecayMode {
|
||||
FAST_DECAY = 0, //aka 'Coasting'
|
||||
SLOW_DECAY = 1, //aka 'Braking'
|
||||
};
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constants
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
static constexpr float DEFAULT_FREQUENCY = 25000.0f; // The standard motor update rate
|
||||
static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY;
|
||||
static constexpr float MIN_FREQUENCY = 10.0f; // Lowest achievable with hardware PWM with good resolution
|
||||
static constexpr float MAX_FREQUENCY = 50000.0f; // Highest nice speed
|
||||
static constexpr float ZERO_PERCENT = 0.0f;
|
||||
static constexpr float ONEHUNDRED_PERCENT = 1.0f;
|
||||
|
||||
private:
|
||||
static constexpr float MIN_VALID_DUTY = 1.0f;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
float motor_speed = 0.0f;
|
||||
float last_enabled_duty = 0.0f;
|
||||
bool enabled = false;
|
||||
//Calibration calib;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
MotorState();
|
||||
//MotorState(CalibrationType default_type);
|
||||
//MotorState(const Calibration& calibration);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
float enable();
|
||||
float disable();
|
||||
bool is_enabled() const;
|
||||
private:
|
||||
float _enable(); // Internal version of enable without convenient initialisation to the middle
|
||||
public:
|
||||
float get_duty() const;
|
||||
float set_duty_with_return(float duty);
|
||||
|
||||
float get_speed() const;
|
||||
float set_speed_with_return(float speed);
|
||||
|
||||
public:
|
||||
float get_min_speed() const;
|
||||
//float get_mid_speed() const;
|
||||
float get_max_speed() const;
|
||||
|
||||
float to_min_with_return();
|
||||
//float to_mid_with_return();
|
||||
float to_max_with_return();
|
||||
float to_percent_with_return(float in, float in_min = ZERO_PERCENT, float in_max = ONEHUNDRED_PERCENT);
|
||||
float to_percent_with_return(float in, float in_min, float in_max, float speed_min, float speed_max);
|
||||
|
||||
//Calibration& calibration();
|
||||
//const Calibration& calibration() const;
|
||||
|
||||
//--------------------------------------------------
|
||||
static int32_t duty_to_level(float duty, uint32_t resolution);
|
||||
|
||||
private:
|
||||
static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -42,3 +42,4 @@ add_subdirectory(plasma2040)
|
|||
add_subdirectory(badger2040)
|
||||
add_subdirectory(interstate75)
|
||||
add_subdirectory(servo2040)
|
||||
add_subdirectory(motor2040)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,10 @@
|
|||
#include(servo2040_calibration.cmake)
|
||||
#include(servo2040_current_meter.cmake)
|
||||
#include(servo2040_led_rainbow.cmake)
|
||||
#include(servo2040_multiple_servos.cmake)
|
||||
#include(servo2040_read_sensors.cmake)
|
||||
#include(servo2040_sensor_feedback.cmake)
|
||||
#include(servo2040_servo_cluster.cmake)
|
||||
#include(servo2040_servo_wave.cmake)
|
||||
#include(servo2040_simple_easing.cmake)
|
||||
include(motor2040_single_motor.cmake)
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
set(OUTPUT_NAME motor2040_single_motor)
|
||||
add_executable(${OUTPUT_NAME} motor2040_single_motor.cpp)
|
||||
|
||||
target_link_libraries(${OUTPUT_NAME}
|
||||
pico_stdlib
|
||||
motor2040
|
||||
)
|
||||
|
||||
# enable usb output
|
||||
pico_enable_stdio_usb(${OUTPUT_NAME} 1)
|
||||
|
||||
pico_add_extra_outputs(${OUTPUT_NAME})
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
#include "pico/stdlib.h"
|
||||
|
||||
#include "motor2040.hpp"
|
||||
|
||||
/*
|
||||
Demonstrates how to create a Motor object and control it.
|
||||
*/
|
||||
|
||||
using namespace motor;
|
||||
|
||||
// How many sweeps of the motor to perform
|
||||
const uint SWEEPS = 3;
|
||||
|
||||
// The number of discrete sweep steps
|
||||
const uint STEPS = 10;
|
||||
|
||||
// The time in milliseconds between each step of the sequence
|
||||
const uint STEPS_INTERVAL_MS = 500;
|
||||
|
||||
// How far from zero to move the motor when sweeping
|
||||
constexpr float SWEEP_EXTENT = 90.0f;
|
||||
|
||||
|
||||
// Create a motor on pin 0 and 1
|
||||
Motor2 m = Motor2(motor2040::SERVO_1, motor2040::SERVO_2);
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
|
||||
// Initialise the motor
|
||||
m.init();
|
||||
|
||||
// Enable the motor (this puts it at the middle)
|
||||
m.enable();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Go to min
|
||||
m.to_min();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Go to max
|
||||
m.to_max();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Go back to mid
|
||||
//m.to_mid();
|
||||
//sleep_ms(2000);
|
||||
|
||||
// Do a sine sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < 360; i++) {
|
||||
m.speed(sin(((float)i * (float)M_PI) / 180.0f) * SWEEP_EXTENT);
|
||||
sleep_ms(20);
|
||||
}
|
||||
}
|
||||
|
||||
// Do a stepped sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
m.to_percent(i, 0, STEPS, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
for(auto i = 0u; i < STEPS; i++) {
|
||||
m.to_percent(i, STEPS, 0, 0.0 - SWEEP_EXTENT, SWEEP_EXTENT);
|
||||
sleep_ms(STEPS_INTERVAL_MS);
|
||||
}
|
||||
}
|
||||
|
||||
// Disable the motor
|
||||
m.disable();
|
||||
}
|
||||
|
|
@ -29,3 +29,4 @@ add_subdirectory(pico_wireless)
|
|||
add_subdirectory(plasma2040)
|
||||
add_subdirectory(badger2040)
|
||||
add_subdirectory(servo2040)
|
||||
add_subdirectory(motor2040)
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
include(motor2040.cmake)
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
add_library(motor2040 INTERFACE)
|
||||
|
||||
target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2) # motor_cluster)
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
#pragma once
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
#include "ws2812.hpp"
|
||||
#include "motor2.hpp"
|
||||
#include "motor_cluster.hpp"
|
||||
|
||||
namespace motor {
|
||||
namespace motor2040 {
|
||||
const uint SERVO_1 = 0;
|
||||
const uint SERVO_2 = 1;
|
||||
const uint SERVO_3 = 2;
|
||||
const uint SERVO_4 = 3;
|
||||
const uint SERVO_5 = 4;
|
||||
const uint SERVO_6 = 5;
|
||||
const uint SERVO_7 = 6;
|
||||
const uint SERVO_8 = 7;
|
||||
const uint SERVO_9 = 8;
|
||||
const uint SERVO_10 = 9;
|
||||
const uint SERVO_11 = 10;
|
||||
const uint SERVO_12 = 11;
|
||||
const uint SERVO_13 = 12;
|
||||
const uint SERVO_14 = 13;
|
||||
const uint SERVO_15 = 14;
|
||||
const uint SERVO_16 = 15;
|
||||
const uint SERVO_17 = 16;
|
||||
const uint SERVO_18 = 17;
|
||||
const uint NUM_SERVOS = 18;
|
||||
|
||||
const uint LED_DATA = 18;
|
||||
const uint NUM_LEDS = 1;
|
||||
|
||||
const uint USER_SW = 23;
|
||||
|
||||
const uint ADC_ADDR_0 = 22;
|
||||
const uint ADC_ADDR_1 = 24;
|
||||
const uint ADC_ADDR_2 = 25;
|
||||
|
||||
const uint ADC0 = 26;
|
||||
const uint ADC1 = 27;
|
||||
const uint ADC2 = 28;
|
||||
const uint SHARED_ADC = 29; // The pin used for the board's sensing features
|
||||
|
||||
const uint SENSOR_1_ADDR = 0b000;
|
||||
const uint SENSOR_2_ADDR = 0b001;
|
||||
const uint SENSOR_3_ADDR = 0b010;
|
||||
const uint SENSOR_4_ADDR = 0b011;
|
||||
const uint SENSOR_5_ADDR = 0b100;
|
||||
const uint SENSOR_6_ADDR = 0b101;
|
||||
const uint NUM_SENSORS = 6;
|
||||
|
||||
const uint VOLTAGE_SENSE_ADDR = 0b110;
|
||||
const uint CURRENT_SENSE_ADDR = 0b111;
|
||||
|
||||
constexpr float SHUNT_RESISTOR = 0.003f;
|
||||
constexpr float CURRENT_GAIN = 69;
|
||||
constexpr float VOLTAGE_GAIN = 3.9f / 13.9f;
|
||||
constexpr float CURRENT_OFFSET = -0.02f;
|
||||
}
|
||||
}
|
||||
Ładowanie…
Reference in New Issue