kopia lustrzana https://github.com/pimoroni/pimoroni-pico
Various motor work
ZodiusInfuser
rodzic
e59bdc34c4
commit
9a9c3832a2
|
|
@ -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;
|
||||
}
|
||||
};
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -3,6 +3,7 @@ add_library(${DRIVER_NAME} INTERFACE)
|
|||
|
||||
target_sources(${DRIVER_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor.cpp
|
||||
${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp
|
||||
)
|
||||
|
||||
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
|
|
|||
|
|
@ -1,14 +1,14 @@
|
|||
#include "motor.hpp"
|
||||
#include "pwm.hpp"
|
||||
|
||||
namespace pimoroni {
|
||||
Motor::Motor(uint pin_pos, uint pin_neg, float freq, DecayMode mode)
|
||||
: pin_pos(pin_pos), pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) {
|
||||
namespace motor {
|
||||
Motor::Motor(const MotorPins &pins, float freq, DecayMode mode)
|
||||
: pins(pins), pwm_frequency(freq), motor_decay_mode(mode) {
|
||||
}
|
||||
|
||||
Motor::~Motor() {
|
||||
gpio_set_function(pin_pos, GPIO_FUNC_NULL);
|
||||
gpio_set_function(pin_neg, GPIO_FUNC_NULL);
|
||||
gpio_set_function(pins.positive, GPIO_FUNC_NULL);
|
||||
gpio_set_function(pins.negative, GPIO_FUNC_NULL);
|
||||
}
|
||||
|
||||
bool Motor::init() {
|
||||
|
|
@ -26,11 +26,11 @@ namespace pimoroni {
|
|||
//Apply the divider
|
||||
pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f);
|
||||
|
||||
pwm_init(pwm_gpio_to_slice_num(pin_pos), &pwm_cfg, true);
|
||||
gpio_set_function(pin_pos, GPIO_FUNC_PWM);
|
||||
pwm_init(pwm_gpio_to_slice_num(pins.positive), &pwm_cfg, true);
|
||||
gpio_set_function(pins.positive, GPIO_FUNC_PWM);
|
||||
|
||||
pwm_init(pwm_gpio_to_slice_num(pin_neg), &pwm_cfg, true);
|
||||
gpio_set_function(pin_neg, GPIO_FUNC_PWM);
|
||||
pwm_init(pwm_gpio_to_slice_num(pins.negative), &pwm_cfg, true);
|
||||
gpio_set_function(pins.negative, GPIO_FUNC_PWM);
|
||||
update_pwm();
|
||||
|
||||
success = true;
|
||||
|
|
@ -66,8 +66,8 @@ namespace pimoroni {
|
|||
pwm_period = period;
|
||||
pwm_frequency = freq;
|
||||
|
||||
uint pos_num = pwm_gpio_to_slice_num(pin_pos);
|
||||
uint neg_num = pwm_gpio_to_slice_num(pin_neg);
|
||||
uint pos_num = pwm_gpio_to_slice_num(pins.positive);
|
||||
uint neg_num = pwm_gpio_to_slice_num(pins.negative);
|
||||
|
||||
//Apply the new divider
|
||||
uint8_t div = div16 >> 4;
|
||||
|
|
@ -112,8 +112,8 @@ namespace pimoroni {
|
|||
|
||||
void Motor::disable() {
|
||||
motor_speed = 0.0f;
|
||||
pwm_set_gpio_level(pin_pos, 0);
|
||||
pwm_set_gpio_level(pin_neg, 0);
|
||||
pwm_set_gpio_level(pins.positive, 0);
|
||||
pwm_set_gpio_level(pins.negative, 0);
|
||||
}
|
||||
|
||||
void Motor::update_pwm() {
|
||||
|
|
@ -122,24 +122,24 @@ namespace pimoroni {
|
|||
switch(motor_decay_mode) {
|
||||
case SLOW_DECAY: //aka 'Braking'
|
||||
if(signed_duty_cycle >= 0) {
|
||||
pwm_set_gpio_level(pin_pos, pwm_period);
|
||||
pwm_set_gpio_level(pin_neg, pwm_period - signed_duty_cycle);
|
||||
pwm_set_gpio_level(pins.positive, pwm_period);
|
||||
pwm_set_gpio_level(pins.negative, pwm_period - signed_duty_cycle);
|
||||
}
|
||||
else {
|
||||
pwm_set_gpio_level(pin_pos, pwm_period + signed_duty_cycle);
|
||||
pwm_set_gpio_level(pin_neg, pwm_period);
|
||||
pwm_set_gpio_level(pins.positive, pwm_period + signed_duty_cycle);
|
||||
pwm_set_gpio_level(pins.negative, pwm_period);
|
||||
}
|
||||
break;
|
||||
|
||||
case FAST_DECAY: //aka 'Coasting'
|
||||
default:
|
||||
if(signed_duty_cycle >= 0) {
|
||||
pwm_set_gpio_level(pin_pos, signed_duty_cycle);
|
||||
pwm_set_gpio_level(pin_neg, 0);
|
||||
pwm_set_gpio_level(pins.positive, signed_duty_cycle);
|
||||
pwm_set_gpio_level(pins.negative, 0);
|
||||
}
|
||||
else {
|
||||
pwm_set_gpio_level(pin_pos, 0);
|
||||
pwm_set_gpio_level(pin_neg, 0 - signed_duty_cycle);
|
||||
pwm_set_gpio_level(pins.positive, 0);
|
||||
pwm_set_gpio_level(pins.negative, 0 - signed_duty_cycle);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,8 +2,9 @@
|
|||
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "motor_state.hpp"
|
||||
|
||||
namespace pimoroni {
|
||||
namespace motor {
|
||||
|
||||
class Motor {
|
||||
//--------------------------------------------------
|
||||
|
|
@ -28,8 +29,7 @@ namespace pimoroni {
|
|||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
uint pin_pos;
|
||||
uint pin_neg;
|
||||
MotorPins pins;
|
||||
pwm_config pwm_cfg;
|
||||
uint16_t pwm_period;
|
||||
float pwm_frequency = DEFAULT_PWM_FREQUENCY;
|
||||
|
|
@ -42,7 +42,7 @@ namespace pimoroni {
|
|||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
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();
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -3,17 +3,13 @@
|
|||
#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(const MotorPins &pins, float speed_scale, float freq, MotorState::DecayMode 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() {
|
||||
gpio_set_function(motor_pin_pos, GPIO_FUNC_NULL);
|
||||
gpio_set_function(motor_pin_neg, GPIO_FUNC_NULL);
|
||||
gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL);
|
||||
gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL);
|
||||
}
|
||||
|
||||
bool Motor2::init() {
|
||||
|
|
@ -31,43 +27,43 @@ namespace motor {
|
|||
// 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_pins.positive), &pwm_cfg, true);
|
||||
gpio_set_function(motor_pins.positive, 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_init(pwm_gpio_to_slice_num(motor_pins.negative), &pwm_cfg, true);
|
||||
gpio_set_function(motor_pins.negative, GPIO_FUNC_PWM);
|
||||
|
||||
pwm_set_gpio_level(motor_pin_pos, 0);
|
||||
pwm_set_gpio_level(motor_pin_neg, 0);
|
||||
pwm_set_gpio_level(motor_pins.positive, 0);
|
||||
pwm_set_gpio_level(motor_pins.negative, 0);
|
||||
|
||||
success = true;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
uint Motor2::pin() const {
|
||||
return motor_pin_pos;
|
||||
MotorPins Motor2::pins() const {
|
||||
return motor_pins;
|
||||
}
|
||||
|
||||
void Motor2::enable() {
|
||||
apply_duty(state.enable());
|
||||
apply_duty(state.enable_with_return());
|
||||
}
|
||||
|
||||
void Motor2::disable() {
|
||||
apply_duty(state.disable());
|
||||
apply_duty(state.disable_with_return());
|
||||
}
|
||||
|
||||
bool Motor2::is_enabled() const {
|
||||
return state.is_enabled();
|
||||
}
|
||||
|
||||
// float Motor2::duty() const {
|
||||
// return state.get_duty();
|
||||
// }
|
||||
float Motor2::duty() const {
|
||||
return state.get_duty();
|
||||
}
|
||||
|
||||
// void Motor2::duty(float duty) {
|
||||
// apply_duty(state.set_duty_with_return(duty));
|
||||
// }
|
||||
void Motor2::duty(float duty) {
|
||||
apply_duty(state.set_duty_with_return(duty));
|
||||
}
|
||||
|
||||
float Motor2::speed() const {
|
||||
return state.get_speed();
|
||||
|
|
@ -77,6 +73,22 @@ namespace motor {
|
|||
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 {
|
||||
return pwm_frequency;
|
||||
}
|
||||
|
|
@ -97,12 +109,15 @@ namespace motor {
|
|||
pwm_period = period;
|
||||
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
|
||||
uint8_t div = div16 >> 4;
|
||||
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(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%)
|
||||
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(state.is_enabled() && !pre_update_pwm) {
|
||||
|
|
@ -140,28 +157,12 @@ namespace motor {
|
|||
disable();
|
||||
}
|
||||
|
||||
float Motor2::min_speed() const {
|
||||
return state.get_min_speed();
|
||||
void Motor2::to_full_negative() {
|
||||
apply_duty(state.to_full_negative_with_return());
|
||||
}
|
||||
|
||||
// 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_full_positive() {
|
||||
apply_duty(state.to_full_positive_with_return());
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
// 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);
|
||||
pwm_set_gpio_level(motor_pins.positive, pwm_period);
|
||||
pwm_set_gpio_level(motor_pins.negative, 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);
|
||||
pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level);
|
||||
pwm_set_gpio_level(motor_pins.negative, 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);
|
||||
pwm_set_gpio_level(motor_pins.positive, signed_level);
|
||||
pwm_set_gpio_level(motor_pins.negative, 0);
|
||||
}
|
||||
else {
|
||||
pwm_set_gpio_level(motor_pin_pos, 0);
|
||||
pwm_set_gpio_level(motor_pin_neg, 0 - signed_level);
|
||||
pwm_set_gpio_level(motor_pins.positive, 0);
|
||||
pwm_set_gpio_level(motor_pins.negative, 0 - signed_level);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,8 +11,7 @@ namespace motor {
|
|||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
uint motor_pin_pos;
|
||||
uint motor_pin_neg;
|
||||
MotorPins motor_pins;
|
||||
MotorState state;
|
||||
pwm_config pwm_cfg;
|
||||
uint16_t pwm_period;
|
||||
|
|
@ -24,8 +23,7 @@ namespace motor {
|
|||
// 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(const MotorPins &pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE);
|
||||
~Motor2();
|
||||
|
||||
|
||||
|
|
@ -36,18 +34,24 @@ namespace motor {
|
|||
bool init();
|
||||
|
||||
// For print access in micropython
|
||||
uint pin() const;
|
||||
MotorPins pins() const;
|
||||
|
||||
void enable();
|
||||
void disable();
|
||||
bool is_enabled() const;
|
||||
|
||||
//float duty() const;
|
||||
//void duty(float duty);
|
||||
float duty() const;
|
||||
void duty(float duty);
|
||||
|
||||
float speed() const;
|
||||
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;
|
||||
bool frequency(float freq);
|
||||
|
||||
|
|
@ -58,19 +62,11 @@ namespace motor {
|
|||
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_full_negative();
|
||||
void to_full_positive();
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -3,44 +3,24 @@
|
|||
#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)
|
||||
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) {
|
||||
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) {
|
||||
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) {
|
||||
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) {
|
||||
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);
|
||||
create_motor_states(speed_scale, inverted, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::~MotorCluster() {
|
||||
|
|
@ -90,8 +70,8 @@ namespace 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);
|
||||
float new_duty = states[motor].enable_with_return();
|
||||
apply_duty(motor, new_duty, 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) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
float new_pulse = states[motor].disable();
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
float new_duty = states[motor].disable_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
||||
void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) {
|
||||
|
|
@ -157,38 +137,38 @@ namespace motor {
|
|||
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());
|
||||
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());
|
||||
float new_pulse = states[motor].set_pulse_with_return(pulse);
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
float new_duty = states[motor].set_duty_with_return(duty);
|
||||
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);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->pulse(motors[i], pulse, false);
|
||||
this->duty(motors[i], duty, false);
|
||||
}
|
||||
if(load)
|
||||
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) {
|
||||
this->pulse(motor, pulse, false);
|
||||
this->duty(motor, duty, false);
|
||||
}
|
||||
if(load)
|
||||
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();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->pulse(motor, pulse, false);
|
||||
this->duty(motor, duty, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
|
|
@ -201,8 +181,8 @@ namespace motor {
|
|||
|
||||
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);
|
||||
float new_duty = states[motor].set_speed_with_return(speed);
|
||||
apply_duty(motor, new_duty, 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();
|
||||
for(uint motor = 0; motor < motor_count; motor++) {
|
||||
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);
|
||||
}
|
||||
|
|
@ -306,112 +286,70 @@ namespace motor {
|
|||
return success;
|
||||
}
|
||||
|
||||
float MotorCluster::min_speed(uint8_t motor) const {
|
||||
float MotorCluster::speed_scale(uint8_t motor) const {
|
||||
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));
|
||||
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 {
|
||||
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) {
|
||||
void MotorCluster::to_full_negative(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);
|
||||
to_full_negative(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
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) {
|
||||
to_min(motor, false);
|
||||
to_full_negative(motor, false);
|
||||
}
|
||||
if(load)
|
||||
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();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_min(motor, false);
|
||||
to_full_negative(motor, false);
|
||||
}
|
||||
if(load)
|
||||
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));
|
||||
float new_pulse = states[motor].to_mid_with_return();
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
float new_duty = states[motor].to_full_positive_with_return();
|
||||
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);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_mid(motors[i], false);
|
||||
to_full_positive(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
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) {
|
||||
to_mid(motor, false);
|
||||
to_full_positive(motor, false);
|
||||
}
|
||||
if(load)
|
||||
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();
|
||||
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);
|
||||
to_full_positive(motor, false);
|
||||
}
|
||||
if(load)
|
||||
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) {
|
||||
assert(is_assigned(motor));
|
||||
float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max);
|
||||
apply_pulse(motor, new_pulse, load);
|
||||
float new_duty = states[motor].to_percent_with_return(in, in_min, in_max);
|
||||
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) {
|
||||
|
|
@ -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) {
|
||||
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);
|
||||
float new_duty = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max);
|
||||
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) {
|
||||
|
|
@ -481,45 +419,22 @@ namespace motor {
|
|||
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::apply_duty(uint8_t motor, float duty, bool 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();
|
||||
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);
|
||||
states[motor] = MotorState(speed_scale, inverted);
|
||||
motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,15 +24,10 @@ namespace motor {
|
|||
// 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(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, 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, 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, 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();
|
||||
|
||||
|
||||
|
|
@ -79,24 +74,17 @@ namespace motor {
|
|||
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;
|
||||
float speed_scale(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_full_negative(uint8_t motor, bool load = true);
|
||||
void to_full_negative(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void to_full_negative(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_to_full_negative(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_full_positive(uint8_t motor, bool load = true);
|
||||
void to_full_positive(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void to_full_positive(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_to_full_positive(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);
|
||||
|
|
@ -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 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);
|
||||
void create_motor_states(float speed_scale, bool inverted, bool auto_phase);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -1,31 +1,33 @@
|
|||
#include "motor_state.hpp"
|
||||
#include "float.h"
|
||||
|
||||
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)
|
||||
// : calib(default_type) {
|
||||
//}
|
||||
|
||||
//MotorState::MotorState(const Calibration& calibration)
|
||||
// : calib(calibration) {
|
||||
//}
|
||||
|
||||
float MotorState::enable() {
|
||||
return _enable();
|
||||
MotorState::MotorState(float speed_scale, bool inverted)
|
||||
: motor_speed(0.0f), motor_scale(MAX(speed_scale, FLT_EPSILON)), inverted(inverted), last_enabled_duty(0.0f), enabled(false) {
|
||||
}
|
||||
|
||||
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;
|
||||
return 0.0f; // A zero duty
|
||||
}
|
||||
|
||||
float MotorState::_enable() {
|
||||
enabled = true;
|
||||
return last_enabled_duty;
|
||||
}
|
||||
|
||||
bool MotorState::is_enabled() const {
|
||||
return enabled;
|
||||
}
|
||||
|
|
@ -35,14 +37,11 @@ namespace motor {
|
|||
}
|
||||
|
||||
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();
|
||||
// Clamp the duty between the hard limits
|
||||
last_enabled_duty = MIN(MAX(duty, -1.0f), 1.0f);
|
||||
motor_speed = last_enabled_duty * motor_scale;
|
||||
|
||||
return enable_with_return();
|
||||
}
|
||||
|
||||
float MotorState::get_speed() const {
|
||||
|
|
@ -50,58 +49,48 @@ namespace motor {
|
|||
}
|
||||
|
||||
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();
|
||||
// Clamp the speed between the hard limits
|
||||
motor_speed = MIN(MAX(speed, -motor_scale), motor_scale);
|
||||
last_enabled_duty = motor_speed / motor_scale;
|
||||
|
||||
return enable_with_return();
|
||||
}
|
||||
|
||||
float MotorState::get_min_speed() const {
|
||||
float speed = 0.0f;
|
||||
//TODO
|
||||
//if(calib.size() > 0) {
|
||||
// speed = calib.first().speed;
|
||||
//}
|
||||
return speed;
|
||||
float MotorState::get_speed_scale() const {
|
||||
return motor_scale;
|
||||
}
|
||||
|
||||
// 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;
|
||||
void MotorState::set_speed_scale(float speed_scale) {
|
||||
motor_scale = MAX(speed_scale, FLT_EPSILON);
|
||||
motor_speed = last_enabled_duty * motor_scale;
|
||||
}
|
||||
|
||||
float MotorState::to_min_with_return() {
|
||||
return set_speed_with_return(get_min_speed());
|
||||
void MotorState::invert_direction(bool invert) {
|
||||
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() {
|
||||
// return set_speed_with_return(get_mid_speed());
|
||||
// }
|
||||
bool MotorState::is_inverted() const {
|
||||
return inverted;
|
||||
}
|
||||
|
||||
float MotorState::to_max_with_return() {
|
||||
return set_speed_with_return(get_max_speed());
|
||||
float MotorState::stop_with_return() {
|
||||
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 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);
|
||||
}
|
||||
|
||||
|
|
@ -110,14 +99,6 @@ namespace motor {
|
|||
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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,10 +1,25 @@
|
|||
#pragma once
|
||||
|
||||
#include "pico/stdlib.h"
|
||||
#include "calibration.hpp"
|
||||
|
||||
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 {
|
||||
//--------------------------------------------------
|
||||
// Enums
|
||||
|
|
@ -22,23 +37,24 @@ namespace motor {
|
|||
public:
|
||||
static constexpr float DEFAULT_FREQUENCY = 25000.0f; // The standard motor update rate
|
||||
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 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;
|
||||
float motor_speed;
|
||||
float motor_scale;
|
||||
bool inverted;
|
||||
float last_enabled_duty;
|
||||
bool enabled;
|
||||
float deadzone_percent = 0.0f;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
|
|
@ -46,19 +62,16 @@ namespace motor {
|
|||
//--------------------------------------------------
|
||||
public:
|
||||
MotorState();
|
||||
//MotorState(CalibrationType default_type);
|
||||
//MotorState(const Calibration& calibration);
|
||||
MotorState(float speed_scale, bool inverted);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Methods
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
float enable();
|
||||
float disable();
|
||||
float enable_with_return();
|
||||
float disable_with_return();
|
||||
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);
|
||||
|
|
@ -66,24 +79,23 @@ namespace motor {
|
|||
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 get_speed_scale() const;
|
||||
void set_speed_scale(float speed_scale);
|
||||
|
||||
float to_min_with_return();
|
||||
//float to_mid_with_return();
|
||||
float to_max_with_return();
|
||||
void invert_direction(bool invert);
|
||||
bool is_inverted() const;
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
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, 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);
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ constexpr float SWEEP_EXTENT = 90.0f;
|
|||
|
||||
|
||||
// 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() {
|
||||
|
|
@ -31,21 +31,21 @@ int main() {
|
|||
// Initialise the motor
|
||||
m.init();
|
||||
|
||||
// Enable the motor (this puts it at the middle)
|
||||
// Enable the motor
|
||||
m.enable();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Go to min
|
||||
m.to_min();
|
||||
// Go at full neative
|
||||
m.to_full_negative();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Go to max
|
||||
m.to_max();
|
||||
// Go at full positive
|
||||
m.to_full_positive();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Go back to mid
|
||||
//m.to_mid();
|
||||
//sleep_ms(2000);
|
||||
// Stop the motor
|
||||
m.stop();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Do a sine sweep
|
||||
for(auto j = 0u; j < SWEEPS; j++) {
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@ Press "A" to start and stop the balancer
|
|||
*/
|
||||
|
||||
using namespace pimoroni;
|
||||
using namespace motor;
|
||||
|
||||
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
|
||||
|
|
@ -22,8 +23,8 @@ static constexpr float PROPORTIONAL = 0.03f;
|
|||
|
||||
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_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_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);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
|
||||
|
||||
I2C i2c(BOARD::BREAKOUT_GARDEN);
|
||||
BreakoutMSA301 msa301(&i2c);
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@ Press "A" to start and stop the movement sequence
|
|||
*/
|
||||
|
||||
using namespace pimoroni;
|
||||
using namespace motor;
|
||||
|
||||
static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1
|
||||
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);
|
||||
|
||||
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_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_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);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE);
|
||||
|
||||
static bool button_toggle = false;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 motor;
|
||||
|
||||
// List frequencies (in hz) to play in sequence here. Use zero for when silence or a pause is wanted
|
||||
// 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);
|
||||
#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_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_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, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY);
|
||||
#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_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_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, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY);
|
||||
#endif
|
||||
|
||||
static bool button_toggle = false;
|
||||
|
|
|
|||
|
|
@ -3,4 +3,4 @@ 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)
|
||||
target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2 motor_cluster)
|
||||
|
|
@ -7,25 +7,38 @@
|
|||
|
||||
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 MOTOR_1P = 4;
|
||||
const uint MOTOR_1N = 5;
|
||||
const uint MOTOR_2P = 6;
|
||||
const uint MOTOR_2N = 7;
|
||||
const uint MOTOR_3P = 8;
|
||||
const uint MOTOR_3N = 9;
|
||||
const uint MOTOR_4P = 10;
|
||||
const uint MOTOR_4N = 11;
|
||||
|
||||
const MotorPins MOTOR_1(MOTOR_1P, MOTOR_1N);
|
||||
const MotorPins MOTOR_2(MOTOR_2P, MOTOR_2N);
|
||||
const MotorPins MOTOR_3(MOTOR_3P, MOTOR_3N);
|
||||
const MotorPins MOTOR_4(MOTOR_4P, MOTOR_4N);
|
||||
const uint NUM_MOTORS = 4;
|
||||
|
||||
const uint ENCODER_1A = 0;
|
||||
const uint ENCODER_1B = 1;
|
||||
const uint ENCODER_2A = 2;
|
||||
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 NUM_LEDS = 1;
|
||||
|
|
@ -41,19 +54,19 @@ namespace motor {
|
|||
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 CURRENT_SENSE_1_ADDR = 0b000;
|
||||
const uint CURRENT_SENSE_2_ADDR = 0b001;
|
||||
const uint CURRENT_SENSE_3_ADDR = 0b010;
|
||||
const uint CURRENT_SENSE_4_ADDR = 0b011;
|
||||
const uint VOLTAGE_SENSE_ADDR = 0b100;
|
||||
const uint FAULT_SENSE_ADDR = 0b101;
|
||||
|
||||
const uint VOLTAGE_SENSE_ADDR = 0b110;
|
||||
const uint CURRENT_SENSE_ADDR = 0b111;
|
||||
const uint SENSOR_1_ADDR = 0b110;
|
||||
const uint SENSOR_2_ADDR = 0b111;
|
||||
const uint NUM_SENSORS = 2;
|
||||
|
||||
constexpr float SHUNT_RESISTOR = 0.003f;
|
||||
constexpr float CURRENT_GAIN = 69;
|
||||
constexpr float SHUNT_RESISTOR = 0.47f;
|
||||
constexpr float CURRENT_GAIN = 5;
|
||||
constexpr float VOLTAGE_GAIN = 3.9f / 13.9f;
|
||||
constexpr float CURRENT_OFFSET = -0.02f;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,4 +3,4 @@ add_library(pico_motor_shim INTERFACE)
|
|||
target_include_directories(pico_motor_shim INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
# 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)
|
||||
|
|
@ -1,12 +1,19 @@
|
|||
#pragma once
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
namespace pico_motor_shim {
|
||||
const uint8_t BUTTON_A = 2;
|
||||
#include "motor2.hpp"
|
||||
|
||||
const uint MOTOR_1_POS = 6;
|
||||
const uint MOTOR_1_NEG = 7;
|
||||
namespace motor {
|
||||
namespace pico_motor_shim {
|
||||
const uint BUTTON_A = 2;
|
||||
|
||||
const uint MOTOR_2_POS = 27;
|
||||
const uint MOTOR_2_NEG = 26;
|
||||
const uint MOTOR_1P = 6;
|
||||
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;
|
||||
}
|
||||
}
|
||||
Ładowanie…
Reference in New Issue