mirror
/
pimoroni-pico
kopia lustrzana https://github.com/pimoroni/pimoroni-pico
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Various motor work

motor-pio
ZodiusInfuser 2022-04-04 20:00:03 +01:00
rodzic e59bdc34c4
commit 9a9c3832a2
19 zmienionych plików z 332 dodań i 884 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

338
drivers/motor/calibration.cpp
Wyświetl plik

@ -1,338 +0,0 @@
#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;
}
};

119
drivers/motor/calibration.hpp
Wyświetl plik

@ -1,119 +0,0 @@
#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;
};
}

1
drivers/motor/motor.cmake
Wyświetl plik

@ -3,6 +3,7 @@ add_library(${DRIVER_NAME} INTERFACE)
target_sources(${DRIVER_NAME} INTERFACE target_sources(${DRIVER_NAME} INTERFACE
${CMAKE_CURRENT_LIST_DIR}/motor.cpp ${CMAKE_CURRENT_LIST_DIR}/motor.cpp
${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp
) )
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})

42
drivers/motor/motor.cpp
Wyświetl plik

@ -1,14 +1,14 @@
#include "motor.hpp" #include "motor.hpp"
#include "pwm.hpp" #include "pwm.hpp"
namespace pimoroni { namespace motor {
Motor::Motor(uint pin_pos, uint pin_neg, float freq, DecayMode mode) Motor::Motor(const MotorPins &pins, float freq, DecayMode mode)
: pin_pos(pin_pos), pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) { : pins(pins), pwm_frequency(freq), motor_decay_mode(mode) {
} }
Motor::~Motor() { Motor::~Motor() {
gpio_set_function(pin_pos, GPIO_FUNC_NULL); gpio_set_function(pins.positive, GPIO_FUNC_NULL);
gpio_set_function(pin_neg, GPIO_FUNC_NULL); gpio_set_function(pins.negative, GPIO_FUNC_NULL);
} }
bool Motor::init() { bool Motor::init() {
@ -26,11 +26,11 @@ namespace pimoroni {
//Apply the divider //Apply the divider
pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f);
pwm_init(pwm_gpio_to_slice_num(pin_pos), &pwm_cfg, true); pwm_init(pwm_gpio_to_slice_num(pins.positive), &pwm_cfg, true);
gpio_set_function(pin_pos, GPIO_FUNC_PWM); gpio_set_function(pins.positive, GPIO_FUNC_PWM);
pwm_init(pwm_gpio_to_slice_num(pin_neg), &pwm_cfg, true); pwm_init(pwm_gpio_to_slice_num(pins.negative), &pwm_cfg, true);
gpio_set_function(pin_neg, GPIO_FUNC_PWM); gpio_set_function(pins.negative, GPIO_FUNC_PWM);
update_pwm(); update_pwm();
success = true; success = true;
@ -66,8 +66,8 @@ namespace pimoroni {
pwm_period = period; pwm_period = period;
pwm_frequency = freq; pwm_frequency = freq;
uint pos_num = pwm_gpio_to_slice_num(pin_pos); uint pos_num = pwm_gpio_to_slice_num(pins.positive);
uint neg_num = pwm_gpio_to_slice_num(pin_neg); uint neg_num = pwm_gpio_to_slice_num(pins.negative);
//Apply the new divider //Apply the new divider
uint8_t div = div16 >> 4; uint8_t div = div16 >> 4;
@ -112,8 +112,8 @@ namespace pimoroni {
void Motor::disable() { void Motor::disable() {
motor_speed = 0.0f; motor_speed = 0.0f;
pwm_set_gpio_level(pin_pos, 0); pwm_set_gpio_level(pins.positive, 0);
pwm_set_gpio_level(pin_neg, 0); pwm_set_gpio_level(pins.negative, 0);
} }
void Motor::update_pwm() { void Motor::update_pwm() {
@ -122,24 +122,24 @@ namespace pimoroni {
switch(motor_decay_mode) { switch(motor_decay_mode) {
case SLOW_DECAY: //aka 'Braking' case SLOW_DECAY: //aka 'Braking'
if(signed_duty_cycle >= 0) { if(signed_duty_cycle >= 0) {
pwm_set_gpio_level(pin_pos, pwm_period); pwm_set_gpio_level(pins.positive, pwm_period);
pwm_set_gpio_level(pin_neg, pwm_period - signed_duty_cycle); pwm_set_gpio_level(pins.negative, pwm_period - signed_duty_cycle);
} }
else { else {
pwm_set_gpio_level(pin_pos, pwm_period + signed_duty_cycle); pwm_set_gpio_level(pins.positive, pwm_period + signed_duty_cycle);
pwm_set_gpio_level(pin_neg, pwm_period); pwm_set_gpio_level(pins.negative, pwm_period);
} }
break; break;
case FAST_DECAY: //aka 'Coasting' case FAST_DECAY: //aka 'Coasting'
default: default:
if(signed_duty_cycle >= 0) { if(signed_duty_cycle >= 0) {
pwm_set_gpio_level(pin_pos, signed_duty_cycle); pwm_set_gpio_level(pins.positive, signed_duty_cycle);
pwm_set_gpio_level(pin_neg, 0); pwm_set_gpio_level(pins.negative, 0);
} }
else { else {
pwm_set_gpio_level(pin_pos, 0); pwm_set_gpio_level(pins.positive, 0);
pwm_set_gpio_level(pin_neg, 0 - signed_duty_cycle); pwm_set_gpio_level(pins.negative, 0 - signed_duty_cycle);
} }
break; break;
} }

8
drivers/motor/motor.hpp
Wyświetl plik

@ -2,8 +2,9 @@
#include "pico/stdlib.h" #include "pico/stdlib.h"
#include "hardware/pwm.h" #include "hardware/pwm.h"
#include "motor_state.hpp"
namespace pimoroni { namespace motor {
class Motor { class Motor {
//-------------------------------------------------- //--------------------------------------------------
@ -28,8 +29,7 @@ namespace pimoroni {
// Variables // Variables
//-------------------------------------------------- //--------------------------------------------------
private: private:
uint pin_pos; MotorPins pins;
uint pin_neg;
pwm_config pwm_cfg; pwm_config pwm_cfg;
uint16_t pwm_period; uint16_t pwm_period;
float pwm_frequency = DEFAULT_PWM_FREQUENCY; float pwm_frequency = DEFAULT_PWM_FREQUENCY;
@ -42,7 +42,7 @@ namespace pimoroni {
// Constructors/Destructor // Constructors/Destructor
//-------------------------------------------------- //--------------------------------------------------
public: public:
Motor(uint pin_pos, uint pin_neg, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); Motor(const MotorPins &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE);
~Motor(); ~Motor();

119
drivers/motor/motor2.cpp
Wyświetl plik

@ -3,17 +3,13 @@
#include "pwm.hpp" #include "pwm.hpp"
namespace motor { namespace motor {
Motor2::Motor2(uint pin_pos, uint pin_neg, float freq, MotorState::DecayMode mode) Motor2::Motor2(const MotorPins &pins, float speed_scale, float freq, MotorState::DecayMode mode)
: motor_pin_pos(pin_pos), motor_pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) { : motor_pins(pins), state(speed_scale, false), 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() { Motor2::~Motor2() {
gpio_set_function(motor_pin_pos, GPIO_FUNC_NULL); gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL);
gpio_set_function(motor_pin_neg, GPIO_FUNC_NULL); gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL);
} }
bool Motor2::init() { bool Motor2::init() {
@ -31,43 +27,43 @@ namespace motor {
// Apply the divider // 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_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); pwm_init(pwm_gpio_to_slice_num(motor_pins.positive), &pwm_cfg, true);
gpio_set_function(motor_pin_pos, GPIO_FUNC_PWM); gpio_set_function(motor_pins.positive, GPIO_FUNC_PWM);
pwm_init(pwm_gpio_to_slice_num(motor_pin_neg), &pwm_cfg, true); pwm_init(pwm_gpio_to_slice_num(motor_pins.negative), &pwm_cfg, true);
gpio_set_function(motor_pin_neg, GPIO_FUNC_PWM); gpio_set_function(motor_pins.negative, GPIO_FUNC_PWM);
pwm_set_gpio_level(motor_pin_pos, 0); pwm_set_gpio_level(motor_pins.positive, 0);
pwm_set_gpio_level(motor_pin_neg, 0); pwm_set_gpio_level(motor_pins.negative, 0);
success = true; success = true;
} }
return success; return success;
} }
uint Motor2::pin() const { MotorPins Motor2::pins() const {
return motor_pin_pos; return motor_pins;
} }
void Motor2::enable() { void Motor2::enable() {
apply_duty(state.enable()); apply_duty(state.enable_with_return());
} }
void Motor2::disable() { void Motor2::disable() {
apply_duty(state.disable()); apply_duty(state.disable_with_return());
} }
bool Motor2::is_enabled() const { bool Motor2::is_enabled() const {
return state.is_enabled(); return state.is_enabled();
} }
// float Motor2::duty() const { float Motor2::duty() const {
// return state.get_duty(); return state.get_duty();
// } }
// void Motor2::duty(float duty) { void Motor2::duty(float duty) {
// apply_duty(state.set_duty_with_return(duty)); apply_duty(state.set_duty_with_return(duty));
// } }
float Motor2::speed() const { float Motor2::speed() const {
return state.get_speed(); return state.get_speed();
@ -77,6 +73,22 @@ namespace motor {
apply_duty(state.set_speed_with_return(speed)); apply_duty(state.set_speed_with_return(speed));
} }
float Motor2::speed_scale() const {
return state.get_speed_scale();
}
void Motor2::speed_scale(float speed_scale) {
state.set_speed_scale(speed_scale);
}
void Motor2::invert_direction(bool invert) {
state.invert_direction(invert);
}
bool Motor2::is_inverted() const {
return state.is_inverted();
}
float Motor2::frequency() const { float Motor2::frequency() const {
return pwm_frequency; return pwm_frequency;
} }
@ -97,12 +109,15 @@ namespace motor {
pwm_period = period; pwm_period = period;
pwm_frequency = freq; pwm_frequency = freq;
uint pin_num = pwm_gpio_to_slice_num(motor_pin_pos); uint pos_pin_num = pwm_gpio_to_slice_num(motor_pins.positive);
uint neg_pin_num = pwm_gpio_to_slice_num(motor_pins.negative);
// Apply the new divider // Apply the new divider
uint8_t div = div16 >> 4; uint8_t div = div16 >> 4;
uint8_t mod = div16 % 16; uint8_t mod = div16 % 16;
pwm_set_clkdiv_int_frac(pin_num, div, mod); pwm_set_clkdiv_int_frac(pos_pin_num, div, mod);
if(neg_pin_num != pos_pin_num)
pwm_set_clkdiv_int_frac(neg_pin_num, div, mod);
// If the the period is larger, update the pwm before setting the new wraps // If the the period is larger, update the pwm before setting the new wraps
if(state.is_enabled() && pre_update_pwm) { if(state.is_enabled() && pre_update_pwm) {
@ -110,7 +125,9 @@ namespace motor {
} }
// Set the new wrap (should be 1 less than the period to get full 0 to 100%) // 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); pwm_set_wrap(pos_pin_num, pwm_period - 1);
if(neg_pin_num != pos_pin_num)
pwm_set_wrap(neg_pin_num, pwm_period - 1);
// If the the period is smaller, update the pwm after setting the new wraps // If the the period is smaller, update the pwm after setting the new wraps
if(state.is_enabled() && !pre_update_pwm) { if(state.is_enabled() && !pre_update_pwm) {
@ -140,28 +157,12 @@ namespace motor {
disable(); disable();
} }
float Motor2::min_speed() const { void Motor2::to_full_negative() {
return state.get_min_speed(); apply_duty(state.to_full_negative_with_return());
} }
// float Motor2::mid_speed() const { void Motor2::to_full_positive() {
// return state.get_mid_speed(); apply_duty(state.to_full_positive_with_return());
// }
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) { void Motor2::to_percent(float in, float in_min, float in_max) {
@ -172,38 +173,30 @@ namespace motor {
apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, 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) { void Motor2::apply_duty(float duty) {
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
switch(motor_decay_mode) { switch(motor_decay_mode) {
case MotorState::SLOW_DECAY: //aka 'Braking' case MotorState::SLOW_DECAY: //aka 'Braking'
if(signed_level >= 0) { if(signed_level >= 0) {
pwm_set_gpio_level(motor_pin_pos, pwm_period); pwm_set_gpio_level(motor_pins.positive, pwm_period);
pwm_set_gpio_level(motor_pin_neg, pwm_period - signed_level); pwm_set_gpio_level(motor_pins.negative, pwm_period - signed_level);
} }
else { else {
pwm_set_gpio_level(motor_pin_pos, pwm_period + signed_level); pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level);
pwm_set_gpio_level(motor_pin_neg, pwm_period); pwm_set_gpio_level(motor_pins.negative, pwm_period);
} }
break; break;
case MotorState::FAST_DECAY: //aka 'Coasting' case MotorState::FAST_DECAY: //aka 'Coasting'
default: default:
if(signed_level >= 0) { if(signed_level >= 0) {
pwm_set_gpio_level(motor_pin_pos, signed_level); pwm_set_gpio_level(motor_pins.positive, signed_level);
pwm_set_gpio_level(motor_pin_neg, 0); pwm_set_gpio_level(motor_pins.negative, 0);
} }
else { else {
pwm_set_gpio_level(motor_pin_pos, 0); pwm_set_gpio_level(motor_pins.positive, 0);
pwm_set_gpio_level(motor_pin_neg, 0 - signed_level); pwm_set_gpio_level(motor_pins.negative, 0 - signed_level);
} }
break; break;
} }

30
drivers/motor/motor2.hpp
Wyświetl plik

@ -11,8 +11,7 @@ namespace motor {
// Variables // Variables
//-------------------------------------------------- //--------------------------------------------------
private: private:
uint motor_pin_pos; MotorPins motor_pins;
uint motor_pin_neg;
MotorState state; MotorState state;
pwm_config pwm_cfg; pwm_config pwm_cfg;
uint16_t pwm_period; uint16_t pwm_period;
@ -24,8 +23,7 @@ namespace motor {
// Constructors/Destructor // Constructors/Destructor
//-------------------------------------------------- //--------------------------------------------------
public: public:
Motor2(uint pin_pos, uint pin_neg, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE); Motor2(const MotorPins &pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE);
//Motor2(uint pin, /*const Calibration& calibration,*/ float freq = MotorState::DEFAULT_FREQUENCY);
~Motor2(); ~Motor2();
@ -36,18 +34,24 @@ namespace motor {
bool init(); bool init();
// For print access in micropython // For print access in micropython
uint pin() const; MotorPins pins() const;
void enable(); void enable();
void disable(); void disable();
bool is_enabled() const; bool is_enabled() const;
//float duty() const; float duty() const;
//void duty(float duty); void duty(float duty);
float speed() const; float speed() const;
void speed(float speed); void speed(float speed);
float speed_scale() const;
void speed_scale(float speed_scale);
void invert_direction(bool invert);
bool is_inverted() const;
float frequency() const; float frequency() const;
bool frequency(float freq); bool frequency(float freq);
@ -58,19 +62,11 @@ namespace motor {
void coast(); // An alias for disable void coast(); // An alias for disable
//-------------------------------------------------- //--------------------------------------------------
float min_speed() const; void to_full_negative();
//float mid_speed() const; void to_full_positive();
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 = 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); void to_percent(float in, float in_min, float in_max, float speed_min, float speed_max);
//Calibration& calibration();
//const Calibration& calibration() const;
//-------------------------------------------------- //--------------------------------------------------
private: private:
void apply_duty(float duty); void apply_duty(float duty);

193
drivers/motor/motor_cluster.cpp
Wyświetl plik

@ -3,44 +3,24 @@
#include <cstdio> #include <cstdio>
namespace motor { 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) MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, float speed_scale, bool inverted, 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) { : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_motor_states(default_type, auto_phase); create_motor_states(speed_scale, inverted, 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) MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, float speed_scale, bool inverted, 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) { : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_motor_states(default_type, auto_phase); create_motor_states(speed_scale, inverted, 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) MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, float speed_scale, bool inverted, 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) { : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_motor_states(default_type, auto_phase); create_motor_states(speed_scale, inverted, 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) MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
: pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) { : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) {
create_motor_states(default_type, auto_phase); create_motor_states(speed_scale, inverted, 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() { MotorCluster::~MotorCluster() {
@ -90,8 +70,8 @@ namespace motor {
void MotorCluster::enable(uint8_t motor, bool load) { void MotorCluster::enable(uint8_t motor, bool load) {
assert(motor < pwms.get_chan_count()); assert(motor < pwms.get_chan_count());
float new_pulse = states[motor].enable(); float new_duty = states[motor].enable_with_return();
apply_pulse(motor, new_pulse, load); apply_duty(motor, new_duty, load);
} }
void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) { void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) {
@ -122,8 +102,8 @@ namespace motor {
void MotorCluster::disable(uint8_t motor, bool load) { void MotorCluster::disable(uint8_t motor, bool load) {
assert(motor < pwms.get_chan_count()); assert(motor < pwms.get_chan_count());
float new_pulse = states[motor].disable(); float new_duty = states[motor].disable_with_return();
apply_pulse(motor, new_pulse, load); apply_duty(motor, new_duty, load);
} }
void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) { void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) {
@ -157,38 +137,38 @@ namespace motor {
return states[motor].is_enabled(); return states[motor].is_enabled();
} }
float MotorCluster::pulse(uint8_t motor) const { float MotorCluster::duty(uint8_t motor) const {
assert(motor < pwms.get_chan_count()); assert(motor < pwms.get_chan_count());
return states[motor].get_pulse(); return states[motor].get_duty();
} }
void MotorCluster::pulse(uint8_t motor, float pulse, bool load) { void MotorCluster::duty(uint8_t motor, float duty, bool load) {
assert(motor < pwms.get_chan_count()); assert(motor < pwms.get_chan_count());
float new_pulse = states[motor].set_pulse_with_return(pulse); float new_duty = states[motor].set_duty_with_return(duty);
apply_pulse(motor, new_pulse, load); apply_duty(motor, new_duty, load);
} }
void MotorCluster::pulse(const uint8_t *motors, uint8_t length, float pulse, bool load) { void MotorCluster::duty(const uint8_t *motors, uint8_t length, float duty, bool load) {
assert(motors != nullptr); assert(motors != nullptr);
for(uint8_t i = 0; i < length; i++) { for(uint8_t i = 0; i < length; i++) {
this->pulse(motors[i], pulse, false); this->duty(motors[i], duty, false);
} }
if(load) if(load)
pwms.load_pwm(); pwms.load_pwm();
} }
void MotorCluster::pulse(std::initializer_list<uint8_t> motors, float pulse, bool load) { void MotorCluster::duty(std::initializer_list<uint8_t> motors, float duty, bool load) {
for(auto motor : motors) { for(auto motor : motors) {
this->pulse(motor, pulse, false); this->duty(motor, duty, false);
} }
if(load) if(load)
pwms.load_pwm(); pwms.load_pwm();
} }
void MotorCluster::all_to_pulse(float pulse, bool load) { void MotorCluster::all_to_duty(float duty, bool load) {
uint8_t motor_count = pwms.get_chan_count(); uint8_t motor_count = pwms.get_chan_count();
for(uint8_t motor = 0; motor < motor_count; motor++) { for(uint8_t motor = 0; motor < motor_count; motor++) {
this->pulse(motor, pulse, false); this->duty(motor, duty, false);
} }
if(load) if(load)
pwms.load_pwm(); pwms.load_pwm();
@ -201,8 +181,8 @@ namespace motor {
void MotorCluster::speed(uint8_t motor, float speed, bool load) { void MotorCluster::speed(uint8_t motor, float speed, bool load) {
assert(motor < pwms.get_chan_count()); assert(motor < pwms.get_chan_count());
float new_pulse = states[motor].set_speed_with_return(speed); float new_duty = states[motor].set_speed_with_return(speed);
apply_pulse(motor, new_pulse, load); apply_duty(motor, new_duty, load);
} }
void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) { void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) {
@ -287,7 +267,7 @@ namespace motor {
uint8_t motor_count = pwms.get_chan_count(); uint8_t motor_count = pwms.get_chan_count();
for(uint motor = 0; motor < motor_count; motor++) { for(uint motor = 0; motor < motor_count; motor++) {
if(states[motor].is_enabled()) { if(states[motor].is_enabled()) {
apply_pulse(motor, states[motor].get_pulse(), false); apply_duty(motor, states[motor].get_duty(), false);
} }
pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false); pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
} }
@ -306,112 +286,70 @@ namespace motor {
return success; return success;
} }
float MotorCluster::min_speed(uint8_t motor) const { float MotorCluster::speed_scale(uint8_t motor) const {
assert(is_assigned(motor)); assert(is_assigned(motor));
return states[motor].get_min_speed(); return states[motor].get_speed_scale();
} }
float MotorCluster::mid_speed(uint8_t motor) const { void MotorCluster::to_full_negative(uint8_t motor, bool load) {
assert(is_assigned(motor)); assert(is_assigned(motor));
return states[motor].get_mid_speed(); float new_duty = states[motor].to_full_negative_with_return();
apply_duty(motor, new_duty, load);
} }
float MotorCluster::max_speed(uint8_t motor) const { void MotorCluster::to_full_negative(const uint8_t *motors, uint8_t length, bool load) {
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); assert(motors != nullptr);
for(uint8_t i = 0; i < length; i++) { for(uint8_t i = 0; i < length; i++) {
to_min(motors[i], false); to_full_negative(motors[i], false);
} }
if(load) if(load)
pwms.load_pwm(); pwms.load_pwm();
} }
void MotorCluster::to_min(std::initializer_list<uint8_t> motors, bool load) { void MotorCluster::to_full_negative(std::initializer_list<uint8_t> motors, bool load) {
for(auto motor : motors) { for(auto motor : motors) {
to_min(motor, false); to_full_negative(motor, false);
} }
if(load) if(load)
pwms.load_pwm(); pwms.load_pwm();
} }
void MotorCluster::all_to_min(bool load) { void MotorCluster::all_to_full_negative(bool load) {
uint8_t motor_count = pwms.get_chan_count(); uint8_t motor_count = pwms.get_chan_count();
for(uint8_t motor = 0; motor < motor_count; motor++) { for(uint8_t motor = 0; motor < motor_count; motor++) {
to_min(motor, false); to_full_negative(motor, false);
} }
if(load) if(load)
pwms.load_pwm(); pwms.load_pwm();
} }
void MotorCluster::to_mid(uint8_t motor, bool load) { void MotorCluster::to_full_positive(uint8_t motor, bool load) {
assert(is_assigned(motor)); assert(is_assigned(motor));
float new_pulse = states[motor].to_mid_with_return(); float new_duty = states[motor].to_full_positive_with_return();
apply_pulse(motor, new_pulse, load); apply_duty(motor, new_duty, load);
} }
void MotorCluster::to_mid(const uint8_t *motors, uint8_t length, bool load) { void MotorCluster::to_full_positive(const uint8_t *motors, uint8_t length, bool load) {
assert(motors != nullptr); assert(motors != nullptr);
for(uint8_t i = 0; i < length; i++) { for(uint8_t i = 0; i < length; i++) {
to_mid(motors[i], false); to_full_positive(motors[i], false);
} }
if(load) if(load)
pwms.load_pwm(); pwms.load_pwm();
} }
void MotorCluster::to_mid(std::initializer_list<uint8_t> motors, bool load) { void MotorCluster::to_full_positive(std::initializer_list<uint8_t> motors, bool load) {
for(auto motor : motors) { for(auto motor : motors) {
to_mid(motor, false); to_full_positive(motor, false);
} }
if(load) if(load)
pwms.load_pwm(); pwms.load_pwm();
} }
void MotorCluster::all_to_mid(bool load) { void MotorCluster::all_to_full_positive(bool load) {
uint8_t motor_count = pwms.get_chan_count(); uint8_t motor_count = pwms.get_chan_count();
for(uint8_t motor = 0; motor < motor_count; motor++) { for(uint8_t motor = 0; motor < motor_count; motor++) {
to_mid(motor, false); to_full_positive(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) if(load)
pwms.load_pwm(); pwms.load_pwm();
@ -419,8 +357,8 @@ namespace motor {
void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) { void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) {
assert(is_assigned(motor)); assert(is_assigned(motor));
float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max); float new_duty = states[motor].to_percent_with_return(in, in_min, in_max);
apply_pulse(motor, new_pulse, load); apply_duty(motor, new_duty, load);
} }
void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) { void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) {
@ -451,8 +389,8 @@ namespace motor {
void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) { 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)); assert(is_assigned(motor));
float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max); float new_duty = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max);
apply_pulse(motor, new_pulse, load); apply_duty(motor, new_duty, 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) { 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) {
@ -481,45 +419,22 @@ namespace motor {
pwms.load_pwm(); 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() { void MotorCluster::load() {
pwms.load_pwm(); pwms.load_pwm();
} }
void MotorCluster::apply_pulse(uint8_t motor, float pulse, bool load) { void MotorCluster::apply_duty(uint8_t motor, float duty, bool load) {
pwms.set_chan_level(motor, MotorState::pulse_to_level(pulse, pwm_period, pwm_frequency), load); pwms.set_chan_level(motor, MotorState::duty_to_level(duty, pwm_period), load);
} }
void MotorCluster::create_motor_states(CalibrationType default_type, bool auto_phase) { void MotorCluster::create_motor_states(float speed_scale, bool inverted, bool auto_phase) {
uint8_t motor_count = pwms.get_chan_count(); uint8_t motor_count = pwms.get_chan_count();
if(motor_count > 0) { if(motor_count > 0) {
states = new MotorState[motor_count]; states = new MotorState[motor_count];
motor_phases = new float[motor_count]; motor_phases = new float[motor_count];
for(uint motor = 0; motor < motor_count; motor++) { for(uint motor = 0; motor < motor_count; motor++) {
states[motor] = MotorState(default_type); states[motor] = MotorState(speed_scale, inverted);
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; motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
} }
} }

44
drivers/motor/motor_cluster.hpp
Wyświetl plik

@ -24,15 +24,10 @@ namespace motor {
// Constructors/Destructor // Constructors/Destructor
//-------------------------------------------------- //--------------------------------------------------
public: 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_mask, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, 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, uint pin_base, uint pin_count, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, 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, const uint8_t *pins, uint32_t length, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, 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, std::initializer_list<uint8_t> pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, 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(); ~MotorCluster();
@ -79,24 +74,17 @@ namespace motor {
bool frequency(float freq); bool frequency(float freq);
//-------------------------------------------------- //--------------------------------------------------
float min_speed(uint8_t motor) const; float speed_scale(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_full_negative(uint8_t motor, bool load = true);
void to_min(const uint8_t *motors, uint8_t length, bool load = true); void to_full_negative(const uint8_t *motors, uint8_t length, bool load = true);
void to_min(std::initializer_list<uint8_t> motors, bool load = true); void to_full_negative(std::initializer_list<uint8_t> motors, bool load = true);
void all_to_min(bool load = true); void all_to_full_negative(bool load = true);
void to_mid(uint8_t motor, bool load = true); void to_full_positive(uint8_t motor, bool load = true);
void to_mid(const uint8_t *motors, uint8_t length, bool load = true); void to_full_positive(const uint8_t *motors, uint8_t length, bool load = true);
void to_mid(std::initializer_list<uint8_t> motors, bool load = true); void to_full_positive(std::initializer_list<uint8_t> motors, bool load = true);
void all_to_mid(bool load = true); void all_to_full_positive(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(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(const uint8_t *motors, uint8_t length, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
@ -108,16 +96,12 @@ namespace motor {
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 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); 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(); void load();
//-------------------------------------------------- //--------------------------------------------------
private: private:
void apply_duty(uint8_t motor, float duty, bool load); void apply_duty(uint8_t motor, float duty, bool load);
void create_motor_states(CalibrationType default_type, bool auto_phase); void create_motor_states(float speed_scale, bool inverted, bool auto_phase);
void create_motor_states(const Calibration& calibration, bool auto_phase);
}; };
} }

127
drivers/motor/motor_state.cpp
Wyświetl plik

@ -1,31 +1,33 @@
#include "motor_state.hpp" #include "motor_state.hpp"
#include "float.h"
namespace motor { namespace motor {
MotorState::MotorState() { MotorState::MotorState()
: motor_speed(0.0f), motor_scale(DEFAULT_SPEED_SCALE), inverted(false), last_enabled_duty(0.0f), enabled(false) {
} }
//MotorState::MotorState(CalibrationType default_type) MotorState::MotorState(float speed_scale, bool inverted)
// : calib(default_type) { : motor_speed(0.0f), motor_scale(MAX(speed_scale, FLT_EPSILON)), inverted(inverted), last_enabled_duty(0.0f), enabled(false) {
//}
//MotorState::MotorState(const Calibration& calibration)
// : calib(calibration) {
//}
float MotorState::enable() {
return _enable();
} }
float MotorState::disable() { float MotorState::enable_with_return() {
enabled = true;
float duty = 0.0f;
if((duty <= 0.0f - deadzone_percent) || (duty >= deadzone_percent)) {
if(inverted)
duty = 0.0f - last_enabled_duty;
else
duty = last_enabled_duty;
}
return duty;
}
float MotorState::disable_with_return() {
enabled = false; enabled = false;
return 0.0f; // A zero duty return 0.0f; // A zero duty
} }
float MotorState::_enable() {
enabled = true;
return last_enabled_duty;
}
bool MotorState::is_enabled() const { bool MotorState::is_enabled() const {
return enabled; return enabled;
} }
@ -35,14 +37,11 @@ namespace motor {
} }
float MotorState::set_duty_with_return(float duty) { float MotorState::set_duty_with_return(float duty) {
//TODO // Clamp the duty between the hard limits
// float speed_out, duty_out; last_enabled_duty = MIN(MAX(duty, -1.0f), 1.0f);
// if(calib.duty_to_speed(duty, speed_out, duty_out)) { motor_speed = last_enabled_duty * motor_scale;
// motor_speed = speed_out;
// last_enabled_duty = duty_out; return enable_with_return();
// return _enable();
// }
return disable();
} }
float MotorState::get_speed() const { float MotorState::get_speed() const {
@ -50,58 +49,48 @@ namespace motor {
} }
float MotorState::set_speed_with_return(float speed) { float MotorState::set_speed_with_return(float speed) {
//TODO // Clamp the speed between the hard limits
// float duty_out, speed_out; motor_speed = MIN(MAX(speed, -motor_scale), motor_scale);
// if(calib.speed_to_duty(speed, duty_out, speed_out)) { last_enabled_duty = motor_speed / motor_scale;
// last_enabled_duty = duty_out;
// motor_speed = speed_out; return enable_with_return();
// return _enable();
// }
return disable();
} }
float MotorState::get_min_speed() const { float MotorState::get_speed_scale() const {
float speed = 0.0f; return motor_scale;
//TODO
//if(calib.size() > 0) {
// speed = calib.first().speed;
//}
return speed;
} }
// float MotorState::get_mid_speed() const { void MotorState::set_speed_scale(float speed_scale) {
// float speed = 0.0f; motor_scale = MAX(speed_scale, FLT_EPSILON);
// if(calib.size() > 0) { motor_speed = last_enabled_duty * motor_scale;
// 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() { void MotorState::invert_direction(bool invert) {
return set_speed_with_return(get_min_speed()); if(invert != inverted) {
inverted = invert;
motor_speed = 0.0f - motor_speed;
last_enabled_duty = 0.0f - last_enabled_duty;
}
} }
// float MotorState::to_mid_with_return() { bool MotorState::is_inverted() const {
// return set_speed_with_return(get_mid_speed()); return inverted;
// } }
float MotorState::to_max_with_return() { float MotorState::stop_with_return() {
return set_speed_with_return(get_max_speed()); return set_duty_with_return(0.0f);
}
float MotorState::to_full_negative_with_return() {
return set_duty_with_return(-1.0f);
}
float MotorState::to_full_positive_with_return() {
return set_duty_with_return(1.0f);
} }
float MotorState::to_percent_with_return(float in, float in_min, float in_max) { 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()); float speed = MotorState::map_float(in, in_min, in_max, 0.0f - get_speed_scale(), get_speed_scale());
return set_speed_with_return(speed); return set_speed_with_return(speed);
} }
@ -110,14 +99,6 @@ namespace motor {
return set_speed_with_return(speed); 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) { int32_t MotorState::duty_to_level(float duty, uint32_t resolution) {
return (int32_t)(duty * (float)resolution); return (int32_t)(duty * (float)resolution);
} }

62
drivers/motor/motor_state.hpp
Wyświetl plik

@ -1,10 +1,25 @@
#pragma once #pragma once
#include "pico/stdlib.h" #include "pico/stdlib.h"
#include "calibration.hpp"
namespace motor { namespace motor {
struct MotorPins {
uint positive;
uint negative;
MotorPins() : positive(0), negative(0) {}
MotorPins(uint pos_pin, uint neg_pin) : positive(pos_pin), negative(neg_pin) {}
};
struct EncoderPins {
uint a;
uint b;
EncoderPins() : a(0), b(0) {}
EncoderPins(uint a_pin, uint b_pin) : a(a_pin), b(b_pin) {}
};
class MotorState { class MotorState {
//-------------------------------------------------- //--------------------------------------------------
// Enums // Enums
@ -22,23 +37,24 @@ namespace motor {
public: public:
static constexpr float DEFAULT_FREQUENCY = 25000.0f; // The standard motor update rate static constexpr float DEFAULT_FREQUENCY = 25000.0f; // The standard motor update rate
static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY;
static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor update rate
static constexpr float MIN_FREQUENCY = 10.0f; // Lowest achievable with hardware PWM with good resolution 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 MAX_FREQUENCY = 50000.0f; // Highest nice speed
static constexpr float ZERO_PERCENT = 0.0f; static constexpr float ZERO_PERCENT = 0.0f;
static constexpr float ONEHUNDRED_PERCENT = 1.0f; static constexpr float ONEHUNDRED_PERCENT = 1.0f;
private:
static constexpr float MIN_VALID_DUTY = 1.0f;
//-------------------------------------------------- //--------------------------------------------------
// Variables // Variables
//-------------------------------------------------- //--------------------------------------------------
private: private:
float motor_speed = 0.0f; float motor_speed;
float last_enabled_duty = 0.0f; float motor_scale;
bool enabled = false; bool inverted;
//Calibration calib; float last_enabled_duty;
bool enabled;
float deadzone_percent = 0.0f;
//-------------------------------------------------- //--------------------------------------------------
@ -46,19 +62,16 @@ namespace motor {
//-------------------------------------------------- //--------------------------------------------------
public: public:
MotorState(); MotorState();
//MotorState(CalibrationType default_type); MotorState(float speed_scale, bool inverted);
//MotorState(const Calibration& calibration);
//-------------------------------------------------- //--------------------------------------------------
// Methods // Methods
//-------------------------------------------------- //--------------------------------------------------
public: public:
float enable(); float enable_with_return();
float disable(); float disable_with_return();
bool is_enabled() const; bool is_enabled() const;
private:
float _enable(); // Internal version of enable without convenient initialisation to the middle
public: public:
float get_duty() const; float get_duty() const;
float set_duty_with_return(float duty); float set_duty_with_return(float duty);
@ -66,24 +79,23 @@ namespace motor {
float get_speed() const; float get_speed() const;
float set_speed_with_return(float speed); float set_speed_with_return(float speed);
public: float get_speed_scale() const;
float get_min_speed() const; void set_speed_scale(float speed_scale);
//float get_mid_speed() const;
float get_max_speed() const;
float to_min_with_return(); void invert_direction(bool invert);
//float to_mid_with_return(); bool is_inverted() const;
float to_max_with_return();
//--------------------------------------------------
float stop_with_return();
float to_full_negative_with_return();
float to_full_positive_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 = 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); 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); 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); static float map_float(float in, float in_min, float in_max, float out_min, float out_max);
}; };

18
examples/motor2040/motor2040_single_motor.cpp
Wyświetl plik

@ -22,7 +22,7 @@ constexpr float SWEEP_EXTENT = 90.0f;
// Create a motor on pin 0 and 1 // Create a motor on pin 0 and 1
Motor2 m = Motor2(motor2040::SERVO_1, motor2040::SERVO_2); Motor2 m = Motor2(motor2040::MOTOR_1);
int main() { int main() {
@ -31,21 +31,21 @@ int main() {
// Initialise the motor // Initialise the motor
m.init(); m.init();
// Enable the motor (this puts it at the middle) // Enable the motor
m.enable(); m.enable();
sleep_ms(2000); sleep_ms(2000);
// Go to min // Go at full neative
m.to_min(); m.to_full_negative();
sleep_ms(2000); sleep_ms(2000);
// Go to max // Go at full positive
m.to_max(); m.to_full_positive();
sleep_ms(2000); sleep_ms(2000);
// Go back to mid // Stop the motor
//m.to_mid(); m.stop();
//sleep_ms(2000); sleep_ms(2000);
// Do a sine sweep // Do a sine sweep
for(auto j = 0u; j < SWEEPS; j++) { for(auto j = 0u; j < SWEEPS; j++) {

5
examples/pico_motor_shim/balance/demo.cpp
Wyświetl plik

@ -13,6 +13,7 @@ Press "A" to start and stop the balancer
*/ */
using namespace pimoroni; using namespace pimoroni;
using namespace motor;
static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1
static constexpr float Z_BIAS_CORRECTION = 0.5f; //A magic number that seems to correct the MSA301's Z bias static constexpr float Z_BIAS_CORRECTION = 0.5f; //A magic number that seems to correct the MSA301's Z bias
@ -22,8 +23,8 @@ static constexpr float PROPORTIONAL = 0.03f;
Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0);
Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
I2C i2c(BOARD::BREAKOUT_GARDEN); I2C i2c(BOARD::BREAKOUT_GARDEN);
BreakoutMSA301 msa301(&i2c); BreakoutMSA301 msa301(&i2c);

5
examples/pico_motor_shim/sequence/demo.cpp
Wyświetl plik

@ -11,6 +11,7 @@ Press "A" to start and stop the movement sequence
*/ */
using namespace pimoroni; using namespace pimoroni;
using namespace motor;
static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1
static const uint32_t ACCELERATE_TIME_MS = 2000; static const uint32_t ACCELERATE_TIME_MS = 2000;
@ -20,8 +21,8 @@ static const uint32_t STOP_TIME_MS = 1000;
Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0);
Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
static bool button_toggle = false; static bool button_toggle = false;

9
examples/pico_motor_shim/song/demo.cpp
Wyświetl plik

@ -11,6 +11,7 @@ Plug a motor into connector 1, and press "A" to start the song playing (does not
*/ */
using namespace pimoroni; using namespace pimoroni;
using namespace motor;
// List frequencies (in hz) to play in sequence here. Use zero for when silence or a pause is wanted // List frequencies (in hz) to play in sequence here. Use zero for when silence or a pause is wanted
// Song from PicoExplorer noise.py // Song from PicoExplorer noise.py
@ -39,11 +40,11 @@ static const uint STATIONARY_TOGGLE_US = 2000;
Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0);
#ifdef USE_FAST_DECAY #ifdef USE_FAST_DECAY
Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY);
Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY);
#else #else
Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY);
Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY);
#endif #endif
static bool button_toggle = false; static bool button_toggle = false;

2
libraries/motor2040/motor2040.cmake
Wyświetl plik

@ -3,4 +3,4 @@ add_library(motor2040 INTERFACE)
target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR}) target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR})
# Pull in pico libraries that we need # Pull in pico libraries that we need
target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2) # motor_cluster) target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2 motor_cluster)

73
libraries/motor2040/motor2040.hpp
Wyświetl plik

@ -7,25 +7,38 @@
namespace motor { namespace motor {
namespace motor2040 { namespace motor2040 {
const uint SERVO_1 = 0; const uint MOTOR_1P = 4;
const uint SERVO_2 = 1; const uint MOTOR_1N = 5;
const uint SERVO_3 = 2; const uint MOTOR_2P = 6;
const uint SERVO_4 = 3; const uint MOTOR_2N = 7;
const uint SERVO_5 = 4; const uint MOTOR_3P = 8;
const uint SERVO_6 = 5; const uint MOTOR_3N = 9;
const uint SERVO_7 = 6; const uint MOTOR_4P = 10;
const uint SERVO_8 = 7; const uint MOTOR_4N = 11;
const uint SERVO_9 = 8;
const uint SERVO_10 = 9; const MotorPins MOTOR_1(MOTOR_1P, MOTOR_1N);
const uint SERVO_11 = 10; const MotorPins MOTOR_2(MOTOR_2P, MOTOR_2N);
const uint SERVO_12 = 11; const MotorPins MOTOR_3(MOTOR_3P, MOTOR_3N);
const uint SERVO_13 = 12; const MotorPins MOTOR_4(MOTOR_4P, MOTOR_4N);
const uint SERVO_14 = 13; const uint NUM_MOTORS = 4;
const uint SERVO_15 = 14;
const uint SERVO_16 = 15; const uint ENCODER_1A = 0;
const uint SERVO_17 = 16; const uint ENCODER_1B = 1;
const uint SERVO_18 = 17; const uint ENCODER_2A = 2;
const uint NUM_SERVOS = 18; const uint ENCODER_2B = 3;
const uint ENCODER_3A = 12;
const uint ENCODER_3B = 13;
const uint ENCODER_4A = 14;
const uint ENCODER_4B = 15;
const EncoderPins ENCODER_1(ENCODER_1A, ENCODER_1B);
const EncoderPins ENCODER_2(ENCODER_2A, ENCODER_2B);
const EncoderPins ENCODER_3(ENCODER_3A, ENCODER_3B);
const EncoderPins ENCODER_4(ENCODER_4A, ENCODER_4B);
const uint NUM_ENCODERS = 4;
const uint TX_TRIG = 16;
const uint RX_ECHO = 17;
const uint LED_DATA = 18; const uint LED_DATA = 18;
const uint NUM_LEDS = 1; const uint NUM_LEDS = 1;
@ -41,19 +54,19 @@ namespace motor {
const uint ADC2 = 28; const uint ADC2 = 28;
const uint SHARED_ADC = 29; // The pin used for the board's sensing features const uint SHARED_ADC = 29; // The pin used for the board's sensing features
const uint SENSOR_1_ADDR = 0b000; const uint CURRENT_SENSE_1_ADDR = 0b000;
const uint SENSOR_2_ADDR = 0b001; const uint CURRENT_SENSE_2_ADDR = 0b001;
const uint SENSOR_3_ADDR = 0b010; const uint CURRENT_SENSE_3_ADDR = 0b010;
const uint SENSOR_4_ADDR = 0b011; const uint CURRENT_SENSE_4_ADDR = 0b011;
const uint SENSOR_5_ADDR = 0b100; const uint VOLTAGE_SENSE_ADDR = 0b100;
const uint SENSOR_6_ADDR = 0b101; const uint FAULT_SENSE_ADDR = 0b101;
const uint NUM_SENSORS = 6;
const uint VOLTAGE_SENSE_ADDR = 0b110; const uint SENSOR_1_ADDR = 0b110;
const uint CURRENT_SENSE_ADDR = 0b111; const uint SENSOR_2_ADDR = 0b111;
const uint NUM_SENSORS = 2;
constexpr float SHUNT_RESISTOR = 0.003f; constexpr float SHUNT_RESISTOR = 0.47f;
constexpr float CURRENT_GAIN = 69; constexpr float CURRENT_GAIN = 5;
constexpr float VOLTAGE_GAIN = 3.9f / 13.9f; constexpr float VOLTAGE_GAIN = 3.9f / 13.9f;
constexpr float CURRENT_OFFSET = -0.02f; constexpr float CURRENT_OFFSET = -0.02f;
} }

2
libraries/pico_motor_shim/pico_motor_shim.cmake
Wyświetl plik

@ -3,4 +3,4 @@ add_library(pico_motor_shim INTERFACE)
target_include_directories(pico_motor_shim INTERFACE ${CMAKE_CURRENT_LIST_DIR}) target_include_directories(pico_motor_shim INTERFACE ${CMAKE_CURRENT_LIST_DIR})
# Pull in pico libraries that we need # Pull in pico libraries that we need
target_link_libraries(pico_motor_shim INTERFACE pico_stdlib) target_link_libraries(pico_motor_shim INTERFACE pico_stdlib motor)

19
libraries/pico_motor_shim/pico_motor_shim.hpp
Wyświetl plik

@ -1,12 +1,19 @@
#pragma once #pragma once
#include "pico/stdlib.h" #include "pico/stdlib.h"
namespace pico_motor_shim { #include "motor2.hpp"
const uint8_t BUTTON_A = 2;
const uint MOTOR_1_POS = 6; namespace motor {
const uint MOTOR_1_NEG = 7; namespace pico_motor_shim {
const uint BUTTON_A = 2;
const uint MOTOR_2_POS = 27; const uint MOTOR_1P = 6;
const uint MOTOR_2_NEG = 26; const uint MOTOR_1N = 7;
const uint MOTOR_2P = 27;
const uint MOTOR_2N = 26;
const MotorPins MOTOR_1(MOTOR_1P, MOTOR_1N);
const MotorPins MOTOR_2(MOTOR_2P, MOTOR_2N);
const uint NUM_MOTORS = 2;
}
} }