From ac3edafbf4cd8e5b8fb58e53b87a7d6a0514d543 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 5 Apr 2022 16:53:36 +0100 Subject: [PATCH] Contined motor dev --- common/pimoroni_common.hpp | 22 ++ drivers/motor/motor.cpp | 2 +- drivers/motor/motor.hpp | 7 +- drivers/motor/motor2.cpp | 76 ++-- drivers/motor/motor2.hpp | 41 +- drivers/motor/motor_cluster.cpp | 349 ++++++++++++++---- drivers/motor/motor_cluster.hpp | 73 +++- drivers/motor/motor_state.cpp | 82 ++-- drivers/motor/motor_state.hpp | 65 ++-- drivers/pwm/pwm_cluster.cpp | 63 ++++ drivers/pwm/pwm_cluster.hpp | 7 + examples/motor2040/motor2040_single_motor.cpp | 4 +- libraries/motor2040/motor2040.hpp | 16 +- libraries/pico_motor_shim/pico_motor_shim.hpp | 4 +- 14 files changed, 600 insertions(+), 211 deletions(-) diff --git a/common/pimoroni_common.hpp b/common/pimoroni_common.hpp index 6fc0a903..3a788732 100644 --- a/common/pimoroni_common.hpp +++ b/common/pimoroni_common.hpp @@ -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) {} + }; } \ No newline at end of file diff --git a/drivers/motor/motor.cpp b/drivers/motor/motor.cpp index d1db206e..73c812c4 100644 --- a/drivers/motor/motor.cpp +++ b/drivers/motor/motor.cpp @@ -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) { } diff --git a/drivers/motor/motor.hpp b/drivers/motor/motor.hpp index a2e7bff3..921fbff5 100644 --- a/drivers/motor/motor.hpp +++ b/drivers/motor/motor.hpp @@ -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(); diff --git a/drivers/motor/motor2.cpp b/drivers/motor/motor2.cpp index 2f584796..35ad7f32 100644 --- a/drivers/motor/motor2.cpp +++ b/drivers/motor/motor2.cpp @@ -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); diff --git a/drivers/motor/motor2.hpp b/drivers/motor/motor2.hpp index b4648e64..b81fc39b 100644 --- a/drivers/motor/motor2.hpp +++ b/drivers/motor/motor2.hpp @@ -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); }; diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index 9760cc8e..eaba20b7 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -3,24 +3,25 @@ #include 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 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_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 motors, bool load) { + void MotorCluster::stop(std::initializer_list 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 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 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 motors, bool load) { + void MotorCluster::full_positive(std::initializer_list 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 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 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 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 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; } } diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index b4d66b30..96384149 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -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 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_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 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 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 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 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 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 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 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 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 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 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; + } }; } \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 079038e1..9e9da46b 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -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); } diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index d4b17f3b..8bf94300 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -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); diff --git a/drivers/pwm/pwm_cluster.cpp b/drivers/pwm/pwm_cluster.cpp index 6197bbc9..c691d039 100644 --- a/drivers/pwm/pwm_cluster.cpp +++ b/drivers/pwm/pwm_cluster.cpp @@ -102,6 +102,55 @@ PWMCluster::PWMCluster(PIO pio, uint sm, std::initializer_list 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_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; diff --git a/drivers/pwm/pwm_cluster.hpp b/drivers/pwm/pwm_cluster.hpp index 147e8b4a..e60f2969 100644 --- a/drivers/pwm/pwm_cluster.hpp +++ b/drivers/pwm/pwm_cluster.hpp @@ -4,6 +4,7 @@ #include "hardware/pio.h" #include "hardware/dma.h" #include "hardware/irq.h" +#include "common/pimoroni_common.hpp" #include 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 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_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); diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index e217c8fc..dae8f56a 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -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 diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index b362ec99..1da18c91 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -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; diff --git a/libraries/pico_motor_shim/pico_motor_shim.hpp b/libraries/pico_motor_shim/pico_motor_shim.hpp index c0681138..ab456807 100644 --- a/libraries/pico_motor_shim/pico_motor_shim.hpp +++ b/libraries/pico_motor_shim/pico_motor_shim.hpp @@ -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; } } \ No newline at end of file