kopia lustrzana https://github.com/pimoroni/pimoroni-pico
Contined motor dev
ZodiusInfuser
rodzic
9a9c3832a2
commit
ac3edafbf4
|
|
@ -6,6 +6,12 @@
|
|||
#define PIMORONI_I2C_DEFAULT_INSTANCE i2c0
|
||||
#define PIMORONI_SPI_DEFAULT_INSTANCE spi0
|
||||
|
||||
// Macro to return a value clamped between a minimum and maximum
|
||||
#ifndef CLAMP
|
||||
#define CLAMP(a, mn, mx) ((a)<(mx)?((a)>(mn)?(a):(mn)):(mx))
|
||||
#endif
|
||||
|
||||
|
||||
namespace pimoroni {
|
||||
static const unsigned int PIN_UNUSED = INT_MAX; // Intentionally INT_MAX to avoid overflowing MicroPython's int type
|
||||
|
||||
|
|
@ -74,4 +80,20 @@ namespace pimoroni {
|
|||
162, 163, 165, 167, 169, 170, 172, 174, 176, 178, 179, 181, 183, 185, 187, 189,
|
||||
191, 193, 194, 196, 198, 200, 202, 204, 206, 208, 210, 212, 214, 216, 218, 220,
|
||||
222, 224, 227, 229, 231, 233, 235, 237, 239, 241, 244, 246, 248, 250, 252, 255};
|
||||
|
||||
struct pin_pair {
|
||||
union {
|
||||
uint8_t first;
|
||||
uint8_t a;
|
||||
uint8_t positive;
|
||||
};
|
||||
union {
|
||||
uint8_t second;
|
||||
uint8_t b;
|
||||
uint8_t negative;
|
||||
};
|
||||
|
||||
pin_pair() : first(0), second(0) {}
|
||||
pin_pair(uint8_t first, uint8_t second) : first(first), second(second) {}
|
||||
};
|
||||
}
|
||||
|
|
@ -2,7 +2,7 @@
|
|||
#include "pwm.hpp"
|
||||
|
||||
namespace motor {
|
||||
Motor::Motor(const MotorPins &pins, float freq, DecayMode mode)
|
||||
Motor::Motor(const pin_pair &pins, float freq, DecayMode mode)
|
||||
: pins(pins), pwm_frequency(freq), motor_decay_mode(mode) {
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -2,8 +2,11 @@
|
|||
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
#include "motor_state.hpp"
|
||||
|
||||
using namespace pimoroni;
|
||||
|
||||
namespace motor {
|
||||
|
||||
class Motor {
|
||||
|
|
@ -29,7 +32,7 @@ namespace motor {
|
|||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
MotorPins pins;
|
||||
pin_pair pins;
|
||||
pwm_config pwm_cfg;
|
||||
uint16_t pwm_period;
|
||||
float pwm_frequency = DEFAULT_PWM_FREQUENCY;
|
||||
|
|
@ -42,7 +45,7 @@ namespace motor {
|
|||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Motor(const MotorPins &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE);
|
||||
Motor(const pin_pair &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE);
|
||||
~Motor();
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -3,8 +3,9 @@
|
|||
#include "pwm.hpp"
|
||||
|
||||
namespace motor {
|
||||
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(const pin_pair &pins, MotorState::Direction direction, float speed_scale,
|
||||
float deadzone_percent, float freq, MotorState::DecayMode mode)
|
||||
: motor_pins(pins), state(direction, speed_scale, deadzone_percent), pwm_frequency(freq), motor_decay_mode(mode) {
|
||||
}
|
||||
|
||||
Motor2::~Motor2() {
|
||||
|
|
@ -41,7 +42,7 @@ namespace motor {
|
|||
return success;
|
||||
}
|
||||
|
||||
MotorPins Motor2::pins() const {
|
||||
pin_pair Motor2::pins() const {
|
||||
return motor_pins;
|
||||
}
|
||||
|
||||
|
|
@ -73,22 +74,6 @@ 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;
|
||||
}
|
||||
|
|
@ -140,29 +125,21 @@ namespace motor {
|
|||
return success;
|
||||
}
|
||||
|
||||
MotorState::DecayMode Motor2::decay_mode() {
|
||||
return motor_decay_mode;
|
||||
}
|
||||
|
||||
void Motor2::decay_mode(MotorState::DecayMode mode) {
|
||||
motor_decay_mode = mode;
|
||||
apply_duty(state.get_duty());
|
||||
}
|
||||
|
||||
void Motor2::stop() {
|
||||
apply_duty(state.set_speed_with_return(0.0f));
|
||||
apply_duty(state.stop_with_return());
|
||||
}
|
||||
|
||||
void Motor2::coast() {
|
||||
state.set_duty_with_return(0.0f);
|
||||
disable();
|
||||
}
|
||||
|
||||
void Motor2::to_full_negative() {
|
||||
apply_duty(state.to_full_negative_with_return());
|
||||
void Motor2::full_negative() {
|
||||
apply_duty(state.full_negative_with_return());
|
||||
}
|
||||
|
||||
void Motor2::to_full_positive() {
|
||||
apply_duty(state.to_full_positive_with_return());
|
||||
void Motor2::full_positive() {
|
||||
apply_duty(state.full_positive_with_return());
|
||||
}
|
||||
|
||||
void Motor2::to_percent(float in, float in_min, float in_max) {
|
||||
|
|
@ -173,6 +150,39 @@ namespace motor {
|
|||
apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max));
|
||||
}
|
||||
|
||||
MotorState::Direction Motor2::direction() const {
|
||||
return state.get_direction();
|
||||
}
|
||||
|
||||
void Motor2::direction(MotorState::Direction direction) {
|
||||
state.set_direction(direction);
|
||||
}
|
||||
|
||||
float Motor2::speed_scale() const {
|
||||
return state.get_speed_scale();
|
||||
}
|
||||
|
||||
void Motor2::speed_scale(float speed_scale) {
|
||||
state.set_speed_scale(speed_scale);
|
||||
}
|
||||
|
||||
float Motor2::deadzone_percent() const {
|
||||
return state.get_deadzone_percent();
|
||||
}
|
||||
|
||||
void Motor2::deadzone_percent(float speed_scale) {
|
||||
apply_duty(state.set_deadzone_percent_with_return(speed_scale));
|
||||
}
|
||||
|
||||
MotorState::DecayMode Motor2::decay_mode() {
|
||||
return motor_decay_mode;
|
||||
}
|
||||
|
||||
void Motor2::decay_mode(MotorState::DecayMode mode) {
|
||||
motor_decay_mode = mode;
|
||||
apply_duty(state.get_duty());
|
||||
}
|
||||
|
||||
void Motor2::apply_duty(float duty) {
|
||||
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
|
||||
|
||||
|
|
|
|||
|
|
@ -2,8 +2,11 @@
|
|||
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
#include "motor_state.hpp"
|
||||
|
||||
using namespace pimoroni;
|
||||
|
||||
namespace motor {
|
||||
|
||||
class Motor2 {
|
||||
|
|
@ -11,7 +14,7 @@ namespace motor {
|
|||
// Variables
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
MotorPins motor_pins;
|
||||
pin_pair motor_pins;
|
||||
MotorState state;
|
||||
pwm_config pwm_cfg;
|
||||
uint16_t pwm_period;
|
||||
|
|
@ -23,7 +26,8 @@ namespace motor {
|
|||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Motor2(const MotorPins &pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE);
|
||||
Motor2(const pin_pair &pins, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE);
|
||||
~Motor2();
|
||||
|
||||
|
||||
|
|
@ -34,7 +38,7 @@ namespace motor {
|
|||
bool init();
|
||||
|
||||
// For print access in micropython
|
||||
MotorPins pins() const;
|
||||
pin_pair pins() const;
|
||||
|
||||
void enable();
|
||||
void disable();
|
||||
|
|
@ -46,28 +50,33 @@ namespace motor {
|
|||
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);
|
||||
|
||||
MotorState::DecayMode decay_mode();
|
||||
void decay_mode(MotorState::DecayMode mode);
|
||||
//--------------------------------------------------
|
||||
|
||||
void stop();
|
||||
void coast(); // An alias for disable
|
||||
|
||||
//--------------------------------------------------
|
||||
void to_full_negative();
|
||||
void to_full_positive();
|
||||
void coast();
|
||||
void full_negative();
|
||||
void 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);
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
MotorState::Direction direction() const;
|
||||
void direction(MotorState::Direction direction);
|
||||
|
||||
float speed_scale() const;
|
||||
void speed_scale(float speed_scale);
|
||||
|
||||
float deadzone_percent() const;
|
||||
void deadzone_percent(float deadzone_percent);
|
||||
|
||||
MotorState::DecayMode decay_mode();
|
||||
void decay_mode(MotorState::DecayMode mode);
|
||||
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
void apply_duty(float duty);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -3,24 +3,25 @@
|
|||
#include <cstdio>
|
||||
|
||||
namespace motor {
|
||||
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(speed_scale, inverted, auto_phase);
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, MotorState::Direction direction,
|
||||
float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode,
|
||||
bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_base, (pin_pair_count * 2), seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase);
|
||||
}
|
||||
|
||||
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(speed_scale, inverted, auto_phase);
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, MotorState::Direction direction,
|
||||
float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode,
|
||||
bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_pairs, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase);
|
||||
}
|
||||
|
||||
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(speed_scale, inverted, auto_phase);
|
||||
}
|
||||
|
||||
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(speed_scale, inverted, auto_phase);
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, MotorState::Direction direction,
|
||||
float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode,
|
||||
bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_pairs, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::~MotorCluster() {
|
||||
|
|
@ -38,10 +39,12 @@ namespace motor {
|
|||
pwm_period = period;
|
||||
|
||||
// Update the pwm before setting the new wrap
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
pwms.set_chan_level(motor, 0, false);
|
||||
pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
|
||||
pwms.set_chan_level(motor_positive(motor), 0, false);
|
||||
pwms.set_chan_level(motor_negative(motor), 0, false);
|
||||
pwms.set_chan_offset(motor_positive(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
|
||||
pwms.set_chan_offset(motor_negative(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
|
||||
}
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
|
|
@ -61,15 +64,15 @@ namespace motor {
|
|||
}
|
||||
|
||||
uint8_t MotorCluster::count() const {
|
||||
return pwms.get_chan_count();
|
||||
return pwms.get_chan_pair_count();
|
||||
}
|
||||
|
||||
uint8_t MotorCluster::pin(uint8_t motor) const {
|
||||
return pwms.get_chan_pin(motor);
|
||||
pin_pair MotorCluster::pins(uint8_t motor) const {
|
||||
return pwms.get_chan_pin_pair(motor);
|
||||
}
|
||||
|
||||
void MotorCluster::enable(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].enable_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
|
@ -92,7 +95,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void MotorCluster::enable_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
enable(motor, false);
|
||||
}
|
||||
|
|
@ -101,7 +104,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void MotorCluster::disable(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].disable_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
|
@ -124,7 +127,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void MotorCluster::disable_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
disable(motor, false);
|
||||
}
|
||||
|
|
@ -133,17 +136,17 @@ namespace motor {
|
|||
}
|
||||
|
||||
bool MotorCluster::is_enabled(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].is_enabled();
|
||||
}
|
||||
|
||||
float MotorCluster::duty(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_duty();
|
||||
}
|
||||
|
||||
void MotorCluster::duty(uint8_t motor, float duty, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].set_duty_with_return(duty);
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
|
@ -166,7 +169,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void MotorCluster::all_to_duty(float duty, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->duty(motor, duty, false);
|
||||
}
|
||||
|
|
@ -175,12 +178,12 @@ namespace motor {
|
|||
}
|
||||
|
||||
float MotorCluster::speed(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_speed();
|
||||
}
|
||||
|
||||
void MotorCluster::speed(uint8_t motor, float speed, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].set_speed_with_return(speed);
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
|
@ -203,7 +206,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void MotorCluster::all_to_speed(float speed, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->speed(motor, speed, false);
|
||||
}
|
||||
|
|
@ -212,12 +215,12 @@ namespace motor {
|
|||
}
|
||||
|
||||
float MotorCluster::phase(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return motor_phases[motor];
|
||||
}
|
||||
|
||||
void MotorCluster::phase(uint8_t motor, float phase, bool load) {
|
||||
assert(motor < pwms.get_chan_count());
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
motor_phases[motor] = MIN(MAX(phase, 0.0f), 1.0f);
|
||||
pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwms.get_wrap()), load);
|
||||
}
|
||||
|
|
@ -240,7 +243,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void MotorCluster::all_to_phase(float phase, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->phase(motor, phase, false);
|
||||
}
|
||||
|
|
@ -264,12 +267,13 @@ namespace motor {
|
|||
pwm_frequency = freq;
|
||||
|
||||
// Update the pwm before setting the new wrap
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint motor = 0; motor < motor_count; motor++) {
|
||||
if(states[motor].is_enabled()) {
|
||||
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_positive(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
|
||||
pwms.set_chan_offset(motor_negative(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
|
||||
}
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
|
|
@ -286,77 +290,137 @@ namespace motor {
|
|||
return success;
|
||||
}
|
||||
|
||||
float MotorCluster::speed_scale(uint8_t motor) const {
|
||||
assert(is_assigned(motor));
|
||||
return states[motor].get_speed_scale();
|
||||
}
|
||||
|
||||
void MotorCluster::to_full_negative(uint8_t motor, bool load) {
|
||||
assert(is_assigned(motor));
|
||||
float new_duty = states[motor].to_full_negative_with_return();
|
||||
void MotorCluster::stop(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].stop_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_full_negative(const uint8_t *motors, uint8_t length, bool load) {
|
||||
void MotorCluster::stop(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_full_negative(motors[i], false);
|
||||
this->stop(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_full_negative(std::initializer_list<uint8_t> motors, bool load) {
|
||||
void MotorCluster::stop(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
to_full_negative(motor, false);
|
||||
this->stop(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::stop_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->stop(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::coast(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_duty_with_return(0.0f);
|
||||
float new_duty = states[motor].disable_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
||||
void MotorCluster::coast(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->coast(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::coast(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->coast(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::coast_all(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->coast(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::full_negative(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].full_negative_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
||||
void MotorCluster::full_negative(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->full_negative(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::full_negative(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
this->full_negative(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_full_negative(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_full_negative(motor, false);
|
||||
this->full_negative(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_full_positive(uint8_t motor, bool load) {
|
||||
assert(is_assigned(motor));
|
||||
float new_duty = states[motor].to_full_positive_with_return();
|
||||
void MotorCluster::full_positive(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].full_positive_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_full_positive(const uint8_t *motors, uint8_t length, bool load) {
|
||||
void MotorCluster::full_positive(const uint8_t *motors, uint8_t length, bool load) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
to_full_positive(motors[i], false);
|
||||
this->full_positive(motors[i], false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_full_positive(std::initializer_list<uint8_t> motors, bool load) {
|
||||
void MotorCluster::full_positive(std::initializer_list<uint8_t> motors, bool load) {
|
||||
for(auto motor : motors) {
|
||||
to_full_positive(motor, false);
|
||||
this->full_positive(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_full_positive(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_full_positive(motor, false);
|
||||
this->full_positive(motor, false);
|
||||
}
|
||||
if(load)
|
||||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) {
|
||||
assert(is_assigned(motor));
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].to_percent_with_return(in, in_min, in_max);
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
|
@ -379,7 +443,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void MotorCluster::all_to_percent(float in, float in_min, float in_max, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_percent(motor, in, in_min, in_max, false);
|
||||
}
|
||||
|
|
@ -388,7 +452,7 @@ 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));
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max);
|
||||
apply_duty(motor, new_duty, load);
|
||||
}
|
||||
|
|
@ -411,7 +475,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void MotorCluster::all_to_percent(float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
to_percent(motor, in, in_min, in_max, speed_min, speed_max, false);
|
||||
}
|
||||
|
|
@ -423,18 +487,167 @@ namespace motor {
|
|||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::apply_duty(uint8_t motor, float duty, bool load) {
|
||||
pwms.set_chan_level(motor, MotorState::duty_to_level(duty, pwm_period), load);
|
||||
MotorState::Direction MotorCluster::direction(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_direction();
|
||||
}
|
||||
|
||||
void MotorCluster::create_motor_states(float speed_scale, bool inverted, bool auto_phase) {
|
||||
uint8_t motor_count = pwms.get_chan_count();
|
||||
void MotorCluster::direction(uint8_t motor, MotorState::Direction direction) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_direction(direction);
|
||||
}
|
||||
|
||||
void MotorCluster::direction(const uint8_t *motors, uint8_t length, MotorState::Direction direction) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->direction(motors[i], direction);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::direction(std::initializer_list<uint8_t> motors, MotorState::Direction direction) {
|
||||
for(auto motor : motors) {
|
||||
this->direction(motor, direction);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_directions(MotorState::Direction direction) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->direction(motor, direction);
|
||||
}
|
||||
}
|
||||
|
||||
float MotorCluster::speed_scale(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_speed_scale();
|
||||
}
|
||||
|
||||
void MotorCluster::speed_scale(uint8_t motor, float speed_scale) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_speed_scale(speed_scale);
|
||||
}
|
||||
|
||||
void MotorCluster::speed_scale(const uint8_t *motors, uint8_t length, float speed_scale) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->speed_scale(motors[i], speed_scale);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::speed_scale(std::initializer_list<uint8_t> motors, float speed_scale) {
|
||||
for(auto motor : motors) {
|
||||
this->speed_scale(motor, speed_scale);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_speed_scales(float speed_scale) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->speed_scale(motor, speed_scale);
|
||||
}
|
||||
}
|
||||
|
||||
float MotorCluster::deadzone_percent(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_deadzone_percent();
|
||||
}
|
||||
|
||||
void MotorCluster::deadzone_percent(uint8_t motor, float deadzone_percent) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_deadzone_percent_with_return(deadzone_percent); //TODO
|
||||
}
|
||||
|
||||
void MotorCluster::deadzone_percent(const uint8_t *motors, uint8_t length, float deadzone_percent) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->deadzone_percent(motors[i], deadzone_percent);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::deadzone_percent(std::initializer_list<uint8_t> motors, float deadzone_percent) {
|
||||
for(auto motor : motors) {
|
||||
this->deadzone_percent(motor, deadzone_percent);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_deadzone_percents(float deadzone_percent) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->deadzone_percent(motor, deadzone_percent);
|
||||
}
|
||||
}
|
||||
|
||||
MotorState::DecayMode MotorCluster::decay_mode(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return MotorState::SLOW_DECAY;//TODO states[motor].get_decay_mode();
|
||||
}
|
||||
|
||||
void MotorCluster::decay_mode(uint8_t motor, MotorState::DecayMode mode) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
//TODO states[motor].set_decay_mode(mode);
|
||||
}
|
||||
|
||||
void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, MotorState::DecayMode mode) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->decay_mode(motors[i], mode);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::decay_mode(std::initializer_list<uint8_t> motors, MotorState::DecayMode mode) {
|
||||
for(auto motor : motors) {
|
||||
this->decay_mode(motor, mode);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_decay_modes(MotorState::DecayMode mode) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->decay_mode(motor, mode);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MotorCluster::apply_duty(uint8_t motor, float duty, bool load) {
|
||||
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
|
||||
|
||||
//TODO move this into motor state
|
||||
MotorState::DecayMode decay_mode = MotorState::SLOW_DECAY;
|
||||
switch(decay_mode) {
|
||||
case MotorState::SLOW_DECAY: //aka 'Braking'
|
||||
if(signed_level >= 0) {
|
||||
pwms.set_chan_level(motor_positive(motor), pwm_period, false);
|
||||
pwms.set_chan_level(motor_negative(motor), pwm_period - signed_level, load);
|
||||
}
|
||||
else {
|
||||
pwms.set_chan_level(motor_positive(motor), pwm_period + signed_level, false);
|
||||
pwms.set_chan_level(motor_negative(motor), pwm_period, load);
|
||||
}
|
||||
break;
|
||||
|
||||
case MotorState::FAST_DECAY: //aka 'Coasting'
|
||||
default:
|
||||
if(signed_level >= 0) {
|
||||
pwms.set_chan_level(motor_positive(motor), signed_level, false);
|
||||
pwms.set_chan_level(motor_negative(motor), 0, load);
|
||||
}
|
||||
else {
|
||||
pwms.set_chan_level(motor_positive(motor), 0, false);
|
||||
pwms.set_chan_level(motor_negative(motor), 0 - signed_level, load);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::create_motor_states(MotorState::Direction direction, float speed_scale,
|
||||
float deadzone_percent, MotorState::DecayMode mode, bool auto_phase) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_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(speed_scale, inverted);
|
||||
states[motor] = MotorState(direction, speed_scale, deadzone_percent);
|
||||
motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,10 +24,15 @@ namespace motor {
|
|||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
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(PIO pio, uint sm, uint pin_base, uint pin_pair_count, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
~MotorCluster();
|
||||
|
||||
|
||||
|
|
@ -38,7 +43,7 @@ namespace motor {
|
|||
bool init();
|
||||
|
||||
uint8_t count() const;
|
||||
uint8_t pin(uint8_t motor) const;
|
||||
pin_pair pins(uint8_t motor) const;
|
||||
|
||||
void enable(uint8_t motor, bool load = true);
|
||||
void enable(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
|
|
@ -74,16 +79,24 @@ namespace motor {
|
|||
bool frequency(float freq);
|
||||
|
||||
//--------------------------------------------------
|
||||
float speed_scale(uint8_t motor) const;
|
||||
void stop(uint8_t motor, bool load = true);
|
||||
void stop(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void stop(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void stop_all(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 coast(uint8_t motor, bool load = true);
|
||||
void coast(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void coast(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void coast_all(bool load = true);
|
||||
|
||||
void full_negative(uint8_t motor, bool load = true);
|
||||
void full_negative(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void full_negative(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_to_full_negative(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 full_positive(uint8_t motor, bool load = true);
|
||||
void full_positive(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void 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);
|
||||
|
|
@ -99,9 +112,43 @@ namespace motor {
|
|||
void load();
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
MotorState::Direction direction(uint8_t motor) const;
|
||||
void direction(uint8_t motor, MotorState::Direction direction);
|
||||
void direction(const uint8_t *motors, uint8_t length, MotorState::Direction direction);
|
||||
void direction(std::initializer_list<uint8_t> motors, MotorState::Direction direction);
|
||||
void all_directions(MotorState::Direction direction);
|
||||
|
||||
float speed_scale(uint8_t motor) const;
|
||||
void speed_scale(uint8_t motor, float speed_scale);
|
||||
void speed_scale(const uint8_t *motors, uint8_t length, float speed_scale);
|
||||
void speed_scale(std::initializer_list<uint8_t> motors, float speed_scale);
|
||||
void all_speed_scales(float speed_scale);
|
||||
|
||||
float deadzone_percent(uint8_t motor) const;
|
||||
void deadzone_percent(uint8_t motor, float deadzone_percent);
|
||||
void deadzone_percent(const uint8_t *motors, uint8_t length, float deadzone_percent);
|
||||
void deadzone_percent(std::initializer_list<uint8_t> motors, float deadzone_percent);
|
||||
void all_deadzone_percents(float deadzone_percent);
|
||||
|
||||
MotorState::DecayMode decay_mode(uint8_t motor) const;
|
||||
void decay_mode(uint8_t motor, MotorState::DecayMode mode);
|
||||
void decay_mode(const uint8_t *motors, uint8_t length, MotorState::DecayMode mode);
|
||||
void decay_mode(std::initializer_list<uint8_t> motors, MotorState::DecayMode mode);
|
||||
void all_decay_modes(MotorState::DecayMode mode);
|
||||
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
void apply_duty(uint8_t motor, float duty, bool load);
|
||||
void create_motor_states(float speed_scale, bool inverted, bool auto_phase);
|
||||
void create_motor_states(MotorState::Direction direction, float speed_scale,
|
||||
float deadzone_percent, MotorState::DecayMode mode, bool auto_phase);
|
||||
|
||||
static uint8_t motor_positive(uint8_t motor) {
|
||||
return PWMCluster::channel_from_pair(motor);
|
||||
}
|
||||
static uint8_t motor_negative(uint8_t motor) {
|
||||
return PWMCluster::channel_from_pair(motor) + 1;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -1,24 +1,27 @@
|
|||
#include "motor_state.hpp"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
#include "float.h"
|
||||
|
||||
namespace motor {
|
||||
MotorState::MotorState()
|
||||
: motor_speed(0.0f), motor_scale(DEFAULT_SPEED_SCALE), inverted(false), last_enabled_duty(0.0f), enabled(false) {
|
||||
MotorState::MotorState()
|
||||
: motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false),
|
||||
motor_direction(DEFAULT_DIRECTION), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE) {
|
||||
}
|
||||
|
||||
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) {
|
||||
MotorState::MotorState(Direction direction, float speed_scale, float deadzone_percent)
|
||||
: motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false),
|
||||
motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_deadzone(CLAMP(deadzone_percent, 0.0f, 1.0f)) {
|
||||
}
|
||||
|
||||
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
|
||||
if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) {
|
||||
if(motor_direction == NORMAL)
|
||||
duty = last_enabled_duty;
|
||||
else
|
||||
duty = 0.0f - last_enabled_duty;
|
||||
}
|
||||
return duty;
|
||||
}
|
||||
|
|
@ -38,7 +41,7 @@ namespace motor {
|
|||
|
||||
float MotorState::set_duty_with_return(float duty) {
|
||||
// Clamp the duty between the hard limits
|
||||
last_enabled_duty = MIN(MAX(duty, -1.0f), 1.0f);
|
||||
last_enabled_duty = CLAMP(duty, -1.0f, 1.0f);
|
||||
motor_speed = last_enabled_duty * motor_scale;
|
||||
|
||||
return enable_with_return();
|
||||
|
|
@ -50,42 +53,21 @@ namespace motor {
|
|||
|
||||
float MotorState::set_speed_with_return(float speed) {
|
||||
// Clamp the speed between the hard limits
|
||||
motor_speed = MIN(MAX(speed, -motor_scale), motor_scale);
|
||||
motor_speed = CLAMP(speed, -motor_scale, motor_scale);
|
||||
last_enabled_duty = motor_speed / motor_scale;
|
||||
|
||||
return enable_with_return();
|
||||
}
|
||||
|
||||
float MotorState::get_speed_scale() const {
|
||||
return motor_scale;
|
||||
}
|
||||
|
||||
void MotorState::set_speed_scale(float speed_scale) {
|
||||
motor_scale = MAX(speed_scale, FLT_EPSILON);
|
||||
motor_speed = last_enabled_duty * motor_scale;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
bool MotorState::is_inverted() const {
|
||||
return inverted;
|
||||
}
|
||||
|
||||
float MotorState::stop_with_return() {
|
||||
return set_duty_with_return(0.0f);
|
||||
}
|
||||
|
||||
float MotorState::to_full_negative_with_return() {
|
||||
float MotorState::full_negative_with_return() {
|
||||
return set_duty_with_return(-1.0f);
|
||||
}
|
||||
|
||||
float MotorState::to_full_positive_with_return() {
|
||||
float MotorState::full_positive_with_return() {
|
||||
return set_duty_with_return(1.0f);
|
||||
}
|
||||
|
||||
|
|
@ -99,6 +81,40 @@ namespace motor {
|
|||
return set_speed_with_return(speed);
|
||||
}
|
||||
|
||||
MotorState::Direction MotorState::get_direction() const {
|
||||
return motor_direction;
|
||||
}
|
||||
|
||||
void MotorState::set_direction(Direction direction) {
|
||||
if(direction != motor_direction) {
|
||||
motor_direction = direction;
|
||||
motor_speed = 0.0f - motor_speed;
|
||||
last_enabled_duty = 0.0f - last_enabled_duty;
|
||||
}
|
||||
}
|
||||
|
||||
float MotorState::get_speed_scale() const {
|
||||
return motor_scale;
|
||||
}
|
||||
|
||||
void MotorState::set_speed_scale(float speed_scale) {
|
||||
motor_scale = MAX(speed_scale, FLT_EPSILON);
|
||||
motor_speed = last_enabled_duty * motor_scale;
|
||||
}
|
||||
|
||||
|
||||
float MotorState::get_deadzone_percent() const {
|
||||
return motor_scale;
|
||||
}
|
||||
|
||||
float MotorState::set_deadzone_percent_with_return(float deadzone_percent) {
|
||||
motor_deadzone = CLAMP(deadzone_percent, 0.0f, 1.0f);
|
||||
if(enabled)
|
||||
return enable_with_return();
|
||||
else
|
||||
return disable_with_return();
|
||||
}
|
||||
|
||||
int32_t MotorState::duty_to_level(float duty, uint32_t resolution) {
|
||||
return (int32_t)(duty * (float)resolution);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,22 +4,6 @@
|
|||
|
||||
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
|
||||
|
|
@ -30,17 +14,25 @@ namespace motor {
|
|||
SLOW_DECAY = 1, //aka 'Braking'
|
||||
};
|
||||
|
||||
enum Direction {
|
||||
NORMAL = 0,
|
||||
REVERSED = 1,
|
||||
};
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constants
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
static const Direction DEFAULT_DIRECTION = NORMAL; // The standard motor direction
|
||||
static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor speed scale
|
||||
static constexpr float DEFAULT_DEADZONE = 0.1f; // The standard motor deadzone
|
||||
|
||||
static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; // The standard motor decay behaviour
|
||||
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;
|
||||
static constexpr float MAX_FREQUENCY = 50000.0f;
|
||||
|
||||
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;
|
||||
|
||||
|
|
@ -50,11 +42,13 @@ namespace motor {
|
|||
//--------------------------------------------------
|
||||
private:
|
||||
float motor_speed;
|
||||
float motor_scale;
|
||||
bool inverted;
|
||||
float last_enabled_duty;
|
||||
bool enabled;
|
||||
float deadzone_percent = 0.0f;
|
||||
|
||||
// Customisation variables
|
||||
Direction motor_direction;
|
||||
float motor_scale;
|
||||
float motor_deadzone;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
|
|
@ -62,7 +56,7 @@ namespace motor {
|
|||
//--------------------------------------------------
|
||||
public:
|
||||
MotorState();
|
||||
MotorState(float speed_scale, bool inverted);
|
||||
MotorState(Direction direction, float speed_scale, float deadzone_percent);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
|
|
@ -72,27 +66,32 @@ namespace motor {
|
|||
float enable_with_return();
|
||||
float disable_with_return();
|
||||
bool is_enabled() const;
|
||||
public:
|
||||
|
||||
float get_duty() const;
|
||||
float set_duty_with_return(float duty);
|
||||
|
||||
float get_speed() const;
|
||||
float set_speed_with_return(float speed);
|
||||
|
||||
float get_speed_scale() const;
|
||||
void set_speed_scale(float speed_scale);
|
||||
|
||||
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 full_negative_with_return();
|
||||
float 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);
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
Direction get_direction() const;
|
||||
void set_direction(Direction direction);
|
||||
|
||||
float get_speed_scale() const;
|
||||
void set_speed_scale(float speed_scale);
|
||||
|
||||
float get_deadzone_percent() const;
|
||||
float set_deadzone_percent_with_return(float deadzone_percent);
|
||||
|
||||
//--------------------------------------------------
|
||||
static int32_t duty_to_level(float duty, uint32_t resolution);
|
||||
|
||||
|
|
|
|||
|
|
@ -102,6 +102,55 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Se
|
|||
constructor_common(seq_buffer, dat_buffer);
|
||||
}
|
||||
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Sequence *seq_buffer, TransitionData *dat_buffer)
|
||||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(0x00000000)
|
||||
, channel_count(0)
|
||||
, channels(nullptr)
|
||||
, wrap_level(0) {
|
||||
|
||||
// Create the pin mask and channel mapping
|
||||
for(uint i = 0; i < length; i++) {
|
||||
pin_pair pair = pin_pairs[i];
|
||||
if((pair.first < NUM_BANK0_GPIOS) && (pair.second < NUM_BANK0_GPIOS)) {
|
||||
pin_mask |= (1u << pair.first);
|
||||
channel_to_pin_map[channel_count] = pair.first;
|
||||
channel_count++;
|
||||
|
||||
pin_mask |= (1u << pair.second);
|
||||
channel_to_pin_map[channel_count] = pair.second;
|
||||
channel_count++;
|
||||
}
|
||||
}
|
||||
|
||||
constructor_common(seq_buffer, dat_buffer);
|
||||
}
|
||||
|
||||
PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Sequence *seq_buffer, TransitionData *dat_buffer)
|
||||
: pio(pio)
|
||||
, sm(sm)
|
||||
, pin_mask(0x00000000)
|
||||
, channel_count(0)
|
||||
, channels(nullptr)
|
||||
, wrap_level(0) {
|
||||
|
||||
// Create the pin mask and channel mapping
|
||||
for(auto pair : pin_pairs) {
|
||||
if((pair.first < NUM_BANK0_GPIOS) && (pair.second < NUM_BANK0_GPIOS)) {
|
||||
pin_mask |= (1u << pair.first);
|
||||
channel_to_pin_map[channel_count] = pair.first;
|
||||
channel_count++;
|
||||
|
||||
pin_mask |= (1u << pair.second);
|
||||
channel_to_pin_map[channel_count] = pair.second;
|
||||
channel_count++;
|
||||
}
|
||||
}
|
||||
|
||||
constructor_common(seq_buffer, dat_buffer);
|
||||
}
|
||||
|
||||
void PWMCluster::constructor_common(Sequence *seq_buffer, TransitionData *dat_buffer) {
|
||||
// Initialise all the channels this PWM will control
|
||||
if(channel_count > 0) {
|
||||
|
|
@ -332,11 +381,25 @@ uint8_t PWMCluster::get_chan_count() const {
|
|||
return channel_count;
|
||||
}
|
||||
|
||||
uint8_t PWMCluster::get_chan_pair_count() const {
|
||||
return (channel_count / 2);
|
||||
}
|
||||
|
||||
uint8_t PWMCluster::get_chan_pin(uint8_t channel) const {
|
||||
assert(channel < channel_count);
|
||||
return channel_to_pin_map[channel];
|
||||
}
|
||||
|
||||
pin_pair PWMCluster::get_chan_pin_pair(uint8_t channel_pair) const {
|
||||
assert(channel_pair < get_chan_pair_count());
|
||||
uint8_t channel_base = channel_from_pair(channel_pair);
|
||||
return pin_pair(channel_to_pin_map[channel_base], channel_to_pin_map[channel_base + 1]);
|
||||
}
|
||||
|
||||
uint8_t PWMCluster::channel_from_pair(uint8_t channel_pair) {
|
||||
return (channel_pair * 2);
|
||||
}
|
||||
|
||||
uint32_t PWMCluster::get_chan_level(uint8_t channel) const {
|
||||
assert(channel < channel_count);
|
||||
return channels[channel].level;
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
#include "hardware/pio.h"
|
||||
#include "hardware/dma.h"
|
||||
#include "hardware/irq.h"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
#include <initializer_list>
|
||||
|
||||
namespace pimoroni {
|
||||
|
|
@ -137,6 +138,9 @@ namespace pimoroni {
|
|||
PWMCluster(PIO pio, uint sm, uint pin_base, uint pin_count, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
|
||||
PWMCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
|
||||
PWMCluster(PIO pio, uint sm, std::initializer_list<uint8_t> pins, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
|
||||
|
||||
PWMCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
|
||||
PWMCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Sequence *seq_buffer = nullptr, TransitionData *dat_buffer = nullptr);
|
||||
~PWMCluster();
|
||||
|
||||
private:
|
||||
|
|
@ -150,7 +154,10 @@ namespace pimoroni {
|
|||
bool init();
|
||||
|
||||
uint8_t get_chan_count() const;
|
||||
uint8_t get_chan_pair_count() const;
|
||||
uint8_t get_chan_pin(uint8_t channel) const;
|
||||
pin_pair get_chan_pin_pair(uint8_t channel_pair) const;
|
||||
static uint8_t channel_from_pair(uint8_t channel_pair);
|
||||
|
||||
uint32_t get_chan_level(uint8_t channel) const;
|
||||
void set_chan_level(uint8_t channel, uint32_t level, bool load = true);
|
||||
|
|
|
|||
|
|
@ -36,11 +36,11 @@ int main() {
|
|||
sleep_ms(2000);
|
||||
|
||||
// Go at full neative
|
||||
m.to_full_negative();
|
||||
m.full_negative();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Go at full positive
|
||||
m.to_full_positive();
|
||||
m.full_positive();
|
||||
sleep_ms(2000);
|
||||
|
||||
// Stop the motor
|
||||
|
|
|
|||
|
|
@ -16,10 +16,10 @@ namespace motor {
|
|||
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 pin_pair MOTOR_1(MOTOR_1P, MOTOR_1N);
|
||||
const pin_pair MOTOR_2(MOTOR_2P, MOTOR_2N);
|
||||
const pin_pair MOTOR_3(MOTOR_3P, MOTOR_3N);
|
||||
const pin_pair MOTOR_4(MOTOR_4P, MOTOR_4N);
|
||||
const uint NUM_MOTORS = 4;
|
||||
|
||||
const uint ENCODER_1A = 0;
|
||||
|
|
@ -31,10 +31,10 @@ namespace motor {
|
|||
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 pin_pair ENCODER_1(ENCODER_1A, ENCODER_1B);
|
||||
const pin_pair ENCODER_2(ENCODER_2A, ENCODER_2B);
|
||||
const pin_pair ENCODER_3(ENCODER_3A, ENCODER_3B);
|
||||
const pin_pair ENCODER_4(ENCODER_4A, ENCODER_4B);
|
||||
const uint NUM_ENCODERS = 4;
|
||||
|
||||
const uint TX_TRIG = 16;
|
||||
|
|
|
|||
|
|
@ -12,8 +12,8 @@ namespace motor {
|
|||
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 pin_pair MOTOR_1(MOTOR_1P, MOTOR_1N);
|
||||
const pin_pair MOTOR_2(MOTOR_2P, MOTOR_2N);
|
||||
const uint NUM_MOTORS = 2;
|
||||
}
|
||||
}
|
||||
Ładowanie…
Reference in New Issue