From f3c0a305f2770fbb298e9cc923dce70d8eb225a4 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Sat, 9 Apr 2022 01:41:42 +0100 Subject: [PATCH] Improvements to MotorState --- drivers/motor/motor2.cpp | 119 ++++++++++++---------- drivers/motor/motor2.hpp | 24 ++--- drivers/motor/motor_cluster.cpp | 150 +++++++++++++++------------- drivers/motor/motor_cluster.hpp | 54 +++++----- drivers/motor/motor_state.cpp | 53 +++++----- drivers/motor/motor_state.hpp | 35 +++---- micropython/modules/motor/motor.cpp | 14 +-- 7 files changed, 232 insertions(+), 217 deletions(-) diff --git a/drivers/motor/motor2.cpp b/drivers/motor/motor2.cpp index 35ad7f32..1d7a86d3 100644 --- a/drivers/motor/motor2.cpp +++ b/drivers/motor/motor2.cpp @@ -1,11 +1,11 @@ #include "motor2.hpp" #include "hardware/clocks.h" #include "pwm.hpp" +#include "math.h" namespace motor { - 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(const pin_pair &pins, Direction direction, float speed_scale, float deadzone, float freq, DecayMode mode) + : motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_decay_mode(mode) { } Motor2::~Motor2() { @@ -47,11 +47,11 @@ namespace motor { } void Motor2::enable() { - apply_duty(state.enable_with_return()); + apply_duty(state.enable_with_return(), motor_decay_mode); } void Motor2::disable() { - apply_duty(state.disable_with_return()); + apply_duty(state.disable_with_return(), motor_decay_mode); } bool Motor2::is_enabled() const { @@ -63,7 +63,7 @@ namespace motor { } void Motor2::duty(float duty) { - apply_duty(state.set_duty_with_return(duty)); + apply_duty(state.set_duty_with_return(duty), motor_decay_mode); } float Motor2::speed() const { @@ -71,7 +71,7 @@ namespace motor { } void Motor2::speed(float speed) { - apply_duty(state.set_speed_with_return(speed)); + apply_duty(state.set_speed_with_return(speed), motor_decay_mode); } float Motor2::frequency() const { @@ -106,7 +106,7 @@ namespace motor { // If the the period is larger, update the pwm before setting the new wraps if(state.is_enabled() && pre_update_pwm) { - apply_duty(state.get_duty()); + apply_duty(state.get_deadzoned_duty(), motor_decay_mode); } // Set the new wrap (should be 1 less than the period to get full 0 to 100%) @@ -116,7 +116,7 @@ namespace motor { // If the the period is smaller, update the pwm after setting the new wraps if(state.is_enabled() && !pre_update_pwm) { - apply_duty(state.get_duty()); + apply_duty(state.get_deadzoned_duty(), motor_decay_mode); } success = true; @@ -125,36 +125,50 @@ namespace motor { return success; } + DecayMode Motor2::decay_mode() { + return motor_decay_mode; + } + + void Motor2::decay_mode(DecayMode mode) { + motor_decay_mode = mode; + if(state.is_enabled()) { + apply_duty(state.get_deadzoned_duty(), motor_decay_mode); + } + } + void Motor2::stop() { - apply_duty(state.stop_with_return()); + apply_duty(state.stop_with_return(), motor_decay_mode); } void Motor2::coast() { - state.set_duty_with_return(0.0f); - disable(); + apply_duty(state.stop_with_return(), FAST_DECAY); + } + + void Motor2::brake() { + apply_duty(state.stop_with_return(), SLOW_DECAY); } void Motor2::full_negative() { - apply_duty(state.full_negative_with_return()); + apply_duty(state.full_negative_with_return(), motor_decay_mode); } void Motor2::full_positive() { - apply_duty(state.full_positive_with_return()); + apply_duty(state.full_positive_with_return(), motor_decay_mode); } void Motor2::to_percent(float in, float in_min, float in_max) { - apply_duty(state.to_percent_with_return(in, in_min, in_max)); + apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_decay_mode); } void Motor2::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) { - apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max)); + apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max), motor_decay_mode); } - MotorState::Direction Motor2::direction() const { + Direction Motor2::direction() const { return state.get_direction(); } - void Motor2::direction(MotorState::Direction direction) { + void Motor2::direction(Direction direction) { state.set_direction(direction); } @@ -166,49 +180,46 @@ namespace motor { state.set_speed_scale(speed_scale); } - float Motor2::deadzone_percent() const { - return state.get_deadzone_percent(); + float Motor2::deadzone() const { + return state.get_deadzone(); } - void Motor2::deadzone_percent(float speed_scale) { - apply_duty(state.set_deadzone_percent_with_return(speed_scale)); + void Motor2::deadzone(float deadzone) { + apply_duty(state.set_deadzone_with_return(deadzone), motor_decay_mode); } - MotorState::DecayMode Motor2::decay_mode() { - return motor_decay_mode; - } + void Motor2::apply_duty(float duty, DecayMode mode) { + if(isfinite(duty)) { + int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); - void Motor2::decay_mode(MotorState::DecayMode mode) { - motor_decay_mode = mode; - apply_duty(state.get_duty()); - } + switch(mode) { + case SLOW_DECAY: //aka 'Braking' + if(signed_level >= 0) { + pwm_set_gpio_level(motor_pins.positive, pwm_period); + pwm_set_gpio_level(motor_pins.negative, pwm_period - signed_level); + } + else { + pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level); + pwm_set_gpio_level(motor_pins.negative, pwm_period); + } + break; - void Motor2::apply_duty(float duty) { - int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); - - switch(motor_decay_mode) { - case MotorState::SLOW_DECAY: //aka 'Braking' - if(signed_level >= 0) { - pwm_set_gpio_level(motor_pins.positive, pwm_period); - pwm_set_gpio_level(motor_pins.negative, pwm_period - signed_level); + case FAST_DECAY: //aka 'Coasting' + default: + if(signed_level >= 0) { + pwm_set_gpio_level(motor_pins.positive, signed_level); + pwm_set_gpio_level(motor_pins.negative, 0); + } + else { + pwm_set_gpio_level(motor_pins.positive, 0); + pwm_set_gpio_level(motor_pins.negative, 0 - signed_level); + } + break; } - else { - pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level); - pwm_set_gpio_level(motor_pins.negative, pwm_period); - } - break; - - case MotorState::FAST_DECAY: //aka 'Coasting' - default: - if(signed_level >= 0) { - pwm_set_gpio_level(motor_pins.positive, signed_level); - pwm_set_gpio_level(motor_pins.negative, 0); - } - else { - pwm_set_gpio_level(motor_pins.positive, 0); - pwm_set_gpio_level(motor_pins.negative, 0 - signed_level); - } - break; + } + else { + pwm_set_gpio_level(motor_pins.positive, 0); + pwm_set_gpio_level(motor_pins.negative, 0); } } }; \ No newline at end of file diff --git a/drivers/motor/motor2.hpp b/drivers/motor/motor2.hpp index b81fc39b..e3a21122 100644 --- a/drivers/motor/motor2.hpp +++ b/drivers/motor/motor2.hpp @@ -19,15 +19,14 @@ namespace motor { pwm_config pwm_cfg; uint16_t pwm_period; float pwm_frequency; - MotorState::DecayMode motor_decay_mode; - + DecayMode motor_decay_mode; //-------------------------------------------------- // Constructors/Destructor //-------------------------------------------------- public: - 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(const pin_pair &pins, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE); ~Motor2(); @@ -53,10 +52,14 @@ namespace motor { float frequency() const; bool frequency(float freq); + DecayMode decay_mode(); + void decay_mode(DecayMode mode); + //-------------------------------------------------- void stop(); void coast(); + void brake(); //TODO void full_negative(); void full_positive(); void to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT); @@ -64,21 +67,18 @@ namespace motor { //-------------------------------------------------- - MotorState::Direction direction() const; - void direction(MotorState::Direction direction); + Direction direction() const; + void direction(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); + float deadzone() const; + void deadzone(float deadzone); //-------------------------------------------------- private: - void apply_duty(float duty); + void apply_duty(float duty, DecayMode mode); }; } \ No newline at end of file diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index eaba20b7..0e820d25 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -1,32 +1,34 @@ #include "motor_cluster.hpp" #include "pwm.hpp" #include +#include "math.h" namespace motor { - 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, + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction, + float speed_scale, float deadzone, float freq, 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); + create_motor_states(direction, speed_scale, deadzone, mode, 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, + MotorCluster::MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction, + float speed_scale, float deadzone, float freq, 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); + create_motor_states(direction, speed_scale, deadzone, mode, 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, + MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Direction direction, + float speed_scale, float deadzone, float freq, 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); + create_motor_states(direction, speed_scale, deadzone, mode, auto_phase); } MotorCluster::~MotorCluster() { delete[] states; delete[] motor_phases; + delete[] motor_modes; } bool MotorCluster::init() { @@ -74,7 +76,7 @@ namespace motor { void MotorCluster::enable(uint8_t motor, bool load) { assert(motor < pwms.get_chan_pair_count()); float new_duty = states[motor].enable_with_return(); - apply_duty(motor, new_duty, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) { @@ -106,7 +108,7 @@ namespace motor { void MotorCluster::disable(uint8_t motor, bool load) { assert(motor < pwms.get_chan_pair_count()); float new_duty = states[motor].disable_with_return(); - apply_duty(motor, new_duty, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) { @@ -148,7 +150,7 @@ namespace motor { void MotorCluster::duty(uint8_t motor, float duty, bool load) { assert(motor < pwms.get_chan_pair_count()); float new_duty = states[motor].set_duty_with_return(duty); - apply_duty(motor, new_duty, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::duty(const uint8_t *motors, uint8_t length, float duty, bool load) { @@ -185,7 +187,7 @@ namespace motor { void MotorCluster::speed(uint8_t motor, float speed, bool load) { assert(motor < pwms.get_chan_pair_count()); float new_duty = states[motor].set_speed_with_return(speed); - apply_duty(motor, new_duty, load); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) { @@ -270,7 +272,7 @@ namespace motor { 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); + apply_duty(motor, states[motor].get_deadzoned_duty(), motor_modes[motor], 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); @@ -293,7 +295,7 @@ namespace motor { 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); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::stop(const uint8_t *motors, uint8_t length, bool load) { @@ -326,7 +328,7 @@ namespace motor { 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); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::coast(const uint8_t *motors, uint8_t length, bool load) { @@ -358,7 +360,7 @@ namespace motor { 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); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::full_negative(const uint8_t *motors, uint8_t length, bool load) { @@ -378,7 +380,7 @@ namespace motor { pwms.load_pwm(); } - void MotorCluster::all_to_full_negative(bool load) { + void MotorCluster::all_full_negative(bool load) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->full_negative(motor, false); @@ -390,7 +392,7 @@ namespace motor { 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); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::full_positive(const uint8_t *motors, uint8_t length, bool load) { @@ -410,7 +412,7 @@ namespace motor { pwms.load_pwm(); } - void MotorCluster::all_to_full_positive(bool load) { + void MotorCluster::all_full_positive(bool load) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->full_positive(motor, false); @@ -422,7 +424,7 @@ namespace motor { void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) { 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); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) { @@ -454,7 +456,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(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); + apply_duty(motor, new_duty, motor_modes[motor], load); } void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) { @@ -487,30 +489,30 @@ namespace motor { pwms.load_pwm(); } - MotorState::Direction MotorCluster::direction(uint8_t motor) const { + Direction MotorCluster::direction(uint8_t motor) const { assert(motor < pwms.get_chan_pair_count()); return states[motor].get_direction(); } - void MotorCluster::direction(uint8_t motor, MotorState::Direction direction) { + void MotorCluster::direction(uint8_t motor, 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) { + void MotorCluster::direction(const uint8_t *motors, uint8_t length, 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) { + void MotorCluster::direction(std::initializer_list motors, Direction direction) { for(auto motor : motors) { this->direction(motor, direction); } } - void MotorCluster::all_directions(MotorState::Direction direction) { + void MotorCluster::all_directions(Direction direction) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { this->direction(motor, direction); @@ -547,60 +549,60 @@ namespace motor { } } - float MotorCluster::deadzone_percent(uint8_t motor) const { + float MotorCluster::deadzone(uint8_t motor) const { assert(motor < pwms.get_chan_pair_count()); - return states[motor].get_deadzone_percent(); + return states[motor].get_deadzone(); } - void MotorCluster::deadzone_percent(uint8_t motor, float deadzone_percent) { + void MotorCluster::deadzone(uint8_t motor, float deadzone) { assert(motor < pwms.get_chan_pair_count()); - states[motor].set_deadzone_percent_with_return(deadzone_percent); //TODO + states[motor].set_deadzone_with_return(deadzone); //TODO } - void MotorCluster::deadzone_percent(const uint8_t *motors, uint8_t length, float deadzone_percent) { + void MotorCluster::deadzone(const uint8_t *motors, uint8_t length, float deadzone) { assert(motors != nullptr); for(uint8_t i = 0; i < length; i++) { - this->deadzone_percent(motors[i], deadzone_percent); + this->deadzone(motors[i], deadzone); } } - void MotorCluster::deadzone_percent(std::initializer_list motors, float deadzone_percent) { + void MotorCluster::deadzone(std::initializer_list motors, float deadzone) { for(auto motor : motors) { - this->deadzone_percent(motor, deadzone_percent); + this->deadzone(motor, deadzone); } } - void MotorCluster::all_deadzone_percents(float deadzone_percent) { + void MotorCluster::all_deadzones(float deadzone) { uint8_t motor_count = pwms.get_chan_pair_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - this->deadzone_percent(motor, deadzone_percent); + this->deadzone(motor, deadzone); } } - MotorState::DecayMode MotorCluster::decay_mode(uint8_t motor) const { + 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(); + return SLOW_DECAY;//TODO states[motor].get_decay_mode(); } - void MotorCluster::decay_mode(uint8_t motor, MotorState::DecayMode mode) { + void MotorCluster::decay_mode(uint8_t motor, 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) { + void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, 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) { + void MotorCluster::decay_mode(std::initializer_list motors, DecayMode mode) { for(auto motor : motors) { this->decay_mode(motor, mode); } } - void MotorCluster::all_decay_modes(MotorState::DecayMode mode) { + void MotorCluster::all_to_decay_mode(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); @@ -608,47 +610,53 @@ namespace motor { } - void MotorCluster::apply_duty(uint8_t motor, float duty, bool load) { - int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); + void MotorCluster::apply_duty(uint8_t motor, float duty, DecayMode mode, bool load) { + if(isfinite(duty)) { + 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; + switch(mode) { + case 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); + case 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; } - else { - pwms.set_chan_level(motor_positive(motor), 0, false); - pwms.set_chan_level(motor_negative(motor), 0 - signed_level, load); - } - break; + } + else { + pwms.set_chan_level(motor_positive(motor), 0, false); + pwms.set_chan_level(motor_negative(motor), 0, load); } } - void MotorCluster::create_motor_states(MotorState::Direction direction, float speed_scale, - float deadzone_percent, MotorState::DecayMode mode, bool auto_phase) { + void MotorCluster::create_motor_states(Direction direction, float speed_scale, + float deadzone, 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]; + motor_modes = new DecayMode[motor_count]; for(uint motor = 0; motor < motor_count; motor++) { - states[motor] = MotorState(direction, speed_scale, deadzone_percent); + states[motor] = MotorState(direction, speed_scale, deadzone); motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; + motor_modes[motor] = mode; } } } diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index 96384149..435bb683 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -18,20 +18,21 @@ namespace motor { float pwm_frequency; MotorState* states; float* motor_phases; + DecayMode* motor_modes; //-------------------------------------------------- // Constructors/Destructor //-------------------------------------------------- public: - 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, + MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, 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, + MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, 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, + MotorCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); ~MotorCluster(); @@ -78,6 +79,12 @@ namespace motor { float frequency() const; bool frequency(float freq); + DecayMode decay_mode(uint8_t motor) const; + void decay_mode(uint8_t motor, DecayMode mode); + void decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode); + void decay_mode(std::initializer_list motors, DecayMode mode); + void all_to_decay_mode(DecayMode mode); + //-------------------------------------------------- void stop(uint8_t motor, bool load = true); void stop(const uint8_t *motors, uint8_t length, bool load = true); @@ -92,12 +99,12 @@ namespace motor { 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 all_full_negative(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 all_full_positive(bool load = true); void to_percent(uint8_t motor, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true); void to_percent(const uint8_t *motors, uint8_t length, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true); @@ -113,11 +120,11 @@ namespace motor { //-------------------------------------------------- - 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); + Direction direction(uint8_t motor) const; + void direction(uint8_t motor, Direction direction); + void direction(const uint8_t *motors, uint8_t length, Direction direction); + void direction(std::initializer_list motors, Direction direction); + void all_directions(Direction direction); float speed_scale(uint8_t motor) const; void speed_scale(uint8_t motor, float speed_scale); @@ -125,23 +132,16 @@ namespace motor { 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); + float deadzone(uint8_t motor) const; + void deadzone(uint8_t motor, float deadzone); + void deadzone(const uint8_t *motors, uint8_t length, float deadzone); + void deadzone(std::initializer_list motors, float deadzone); + void all_deadzones(float deadzone); //-------------------------------------------------- private: - void apply_duty(uint8_t motor, float duty, bool load); - void create_motor_states(MotorState::Direction direction, float speed_scale, - float deadzone_percent, MotorState::DecayMode mode, bool auto_phase); + void apply_duty(uint8_t motor, float duty, DecayMode mode, bool load); + void create_motor_states(Direction direction, float speed_scale, float deadzone, DecayMode mode, bool auto_phase); static uint8_t motor_positive(uint8_t motor) { return PWMCluster::channel_from_pair(motor); diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 9e9da46b..00e381b5 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -1,34 +1,27 @@ #include "motor_state.hpp" #include "common/pimoroni_common.hpp" #include "float.h" +#include "math.h" namespace motor { 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) { + motor_direction(NORMAL), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE){ } - MotorState::MotorState(Direction direction, float speed_scale, float deadzone_percent) + MotorState::MotorState(Direction direction, float speed_scale, float deadzone) : 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)) { + motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) { } float MotorState::enable_with_return() { enabled = true; - - float duty = 0.0f; - 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; + return get_deadzoned_duty(); } float MotorState::disable_with_return() { enabled = false; - return 0.0f; // A zero duty + return NAN; } bool MotorState::is_enabled() const { @@ -39,6 +32,14 @@ namespace motor { return last_enabled_duty; } + float MotorState::get_deadzoned_duty() const { + float duty = 0.0f; + if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) { + duty = last_enabled_duty; + } + return duty; + } + float MotorState::set_duty_with_return(float duty) { // Clamp the duty between the hard limits last_enabled_duty = CLAMP(duty, -1.0f, 1.0f); @@ -48,10 +49,14 @@ namespace motor { } float MotorState::get_speed() const { - return motor_speed; + return (motor_direction == NORMAL) ? motor_speed : 0.0f - motor_speed; } float MotorState::set_speed_with_return(float speed) { + // Invert provided speed if the motor direction is reversed + if(motor_direction == REVERSED) + speed = 0.0f - speed; + // Clamp the speed between the hard limits motor_speed = CLAMP(speed, -motor_scale, motor_scale); last_enabled_duty = motor_speed / motor_scale; @@ -81,16 +86,12 @@ namespace motor { return set_speed_with_return(speed); } - MotorState::Direction MotorState::get_direction() const { + 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; - } + motor_direction = direction; } float MotorState::get_speed_scale() const { @@ -103,16 +104,16 @@ namespace motor { } - float MotorState::get_deadzone_percent() const { - return motor_scale; + float MotorState::get_deadzone() const { + return motor_deadzone; } - float MotorState::set_deadzone_percent_with_return(float deadzone_percent) { - motor_deadzone = CLAMP(deadzone_percent, 0.0f, 1.0f); + float MotorState::set_deadzone_with_return(float deadzone) { + motor_deadzone = CLAMP(deadzone, 0.0f, 1.0f); if(enabled) - return enable_with_return(); + return get_deadzoned_duty(); else - return disable_with_return(); + return NAN; } int32_t MotorState::duty_to_level(float duty, uint32_t resolution) { diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index 8bf94300..990ecbd3 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -4,30 +4,24 @@ namespace motor { + enum Direction { + NORMAL = 0, + REVERSED = 1, + }; + + enum DecayMode { + FAST_DECAY = 0, //aka 'Coasting' + SLOW_DECAY = 1, //aka 'Braking' + }; + class MotorState { - //-------------------------------------------------- - // Enums - //-------------------------------------------------- - public: - enum DecayMode { - FAST_DECAY = 0, //aka 'Coasting' - 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 constexpr float MIN_FREQUENCY = 10.0f; @@ -56,7 +50,7 @@ namespace motor { //-------------------------------------------------- public: MotorState(); - MotorState(Direction direction, float speed_scale, float deadzone_percent); + MotorState(Direction direction, float speed_scale, float deadzone); //-------------------------------------------------- @@ -68,6 +62,7 @@ namespace motor { bool is_enabled() const; float get_duty() const; + float get_deadzoned_duty() const; float set_duty_with_return(float duty); float get_speed() const; @@ -89,8 +84,8 @@ namespace motor { 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); + float get_deadzone() const; + float set_deadzone_with_return(float deadzone); //-------------------------------------------------- static int32_t duty_to_level(float duty, uint32_t resolution); diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 88bceecd..1351f793 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -47,8 +47,8 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_print_str(print, ", direction = REVERSED"); mp_print_str(print, ", speed_scale = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed_scale()), PRINT_REPR); - mp_print_str(print, ", deadzone_percent = "); - mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone_percent()), PRINT_REPR); + mp_print_str(print, ", deadzone = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone()), PRINT_REPR); if(self->motor->decay_mode() == MotorState::SLOW_DECAY) mp_print_str(print, ", decay_mode = SLOW_DECAY"); else @@ -465,7 +465,7 @@ extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - return mp_obj_new_float(self->motor->deadzone_percent()); + return mp_obj_new_float(self->motor->deadzone()); } else { enum { ARG_self, ARG_deadzone_percent }; @@ -480,9 +480,9 @@ extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args, _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); - float deadzone_percent = mp_obj_get_float(args[ARG_deadzone_percent].u_obj); + float deadzone = mp_obj_get_float(args[ARG_deadzone_percent].u_obj); - self->motor->deadzone_percent(deadzone_percent); + self->motor->deadzone(deadzone); return mp_const_none; } } @@ -1605,7 +1605,7 @@ extern mp_obj_t MotorCluster_all_to_full_negative(size_t n_args, const mp_obj_t if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - self->cluster->all_to_full_negative(args[ARG_load].u_bool); + self->cluster->all_full_negative(args[ARG_load].u_bool); } return mp_const_none; } @@ -1693,7 +1693,7 @@ extern mp_obj_t MotorCluster_all_to_full_positive(size_t n_args, const mp_obj_t if(motor_count == 0) mp_raise_ValueError("this cluster does not have any motors"); else { - self->cluster->all_to_full_positive(args[ARG_load].u_bool); + self->cluster->all_full_positive(args[ARG_load].u_bool); } return mp_const_none; }