diff --git a/drivers/motor/calibration.cpp b/drivers/motor/calibration.cpp deleted file mode 100644 index 64fbd50d..00000000 --- a/drivers/motor/calibration.cpp +++ /dev/null @@ -1,338 +0,0 @@ -#include "calibration.hpp" - -namespace motor { - Calibration::Pair::Pair() - : duty(0.0f), speed(0.0f) { - } - - Calibration::Pair::Pair(float duty, float speed) - : duty(duty), speed(speed) { - } - - Calibration::Calibration() - : calibration(nullptr), calibration_size(0), limit_lower(true), limit_upper(true) { - } - - Calibration::Calibration(CalibrationType default_type) - : Calibration() { - apply_default_pairs(default_type); - } - - Calibration::Calibration(const Calibration &other) - : calibration(nullptr), calibration_size(0), limit_lower(other.limit_lower), limit_upper(other.limit_upper) { - uint size = other.size(); - apply_blank_pairs(size); - for(uint i = 0; i < size; i++) { - calibration[i] = other.calibration[i]; - } - } - - Calibration::~Calibration() { - if(calibration != nullptr) { - delete[] calibration; - calibration = nullptr; - } - } - - Calibration &Calibration::operator=(const Calibration &other) { - uint size = other.size(); - apply_blank_pairs(size); - for(uint i = 0; i < size; i++) { - calibration[i] = other.calibration[i]; - } - limit_lower = other.limit_lower; - limit_upper = other.limit_upper; - - return *this; - } - - Calibration::Pair &Calibration::operator[](uint8_t index) { - assert(index < calibration_size); - return calibration[index]; - } - - const Calibration::Pair &Calibration::operator[](uint8_t index) const { - assert(index < calibration_size); - return calibration[index]; - } - - void Calibration::apply_blank_pairs(uint size) { - if(calibration != nullptr) { - delete[] calibration; - } - - if(size > 0) { - calibration = new Pair[size]; - calibration_size = size; - } - else { - calibration = nullptr; - calibration_size = 0; - } - } - - void Calibration::apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed) { - apply_blank_pairs(2); - calibration[0] = Pair(min_duty, min_speed); - calibration[1] = Pair(max_duty, max_speed); - } - - void Calibration::apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed) { - apply_blank_pairs(3); - calibration[0] = Pair(min_duty, min_speed); - calibration[1] = Pair(mid_duty, mid_speed); - calibration[2] = Pair(max_duty, max_speed); - } - - void Calibration::apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed) { - apply_blank_pairs(size); - if(size > 0) { - float size_minus_one = (float)(size - 1); - for(uint i = 0; i < size; i++) { - float duty = Calibration::map_float((float)i, 0.0f, size_minus_one, min_duty, max_duty); - float speed = Calibration::map_float((float)i, 0.0f, size_minus_one, min_speed, max_speed); - calibration[i] = Pair(duty, speed); - } - } - } - - void Calibration::apply_default_pairs(CalibrationType default_type) { - switch(default_type) { - default: - case ANGULAR: - apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE, - -90.0f, 0.0f, +90.0f); - break; - case LINEAR: - apply_two_pairs(DEFAULT_MIN_PULSE, DEFAULT_MAX_PULSE, - 0.0f, 1.0f); - break; - case CONTINUOUS: - apply_three_pairs(DEFAULT_MIN_PULSE, DEFAULT_MID_PULSE, DEFAULT_MAX_PULSE, - -1.0f, 0.0f, +1.0f); - break; - } - } - - uint Calibration::size() const { - return calibration_size; - } - - Calibration::Pair &Calibration::pair(uint8_t index) { - assert(index < calibration_size); - return calibration[index]; - } - - const Calibration::Pair &Calibration::pair(uint8_t index) const { - assert(index < calibration_size); - return calibration[index]; - } - - float Calibration::duty(uint8_t index) const { - return pair(index).duty; - } - - void Calibration::duty(uint8_t index, float duty) { - pair(index).duty = duty; - } - - float Calibration::speed(uint8_t index) const { - return pair(index).speed; - } - - void Calibration::speed(uint8_t index, float speed) { - pair(index).speed = speed; - } - - Calibration::Pair &Calibration::first() { - assert(calibration_size > 0); - return calibration[0]; - } - - const Calibration::Pair &Calibration::first() const { - assert(calibration_size > 0); - return calibration[0]; - } - - float Calibration::first_duty() const { - return first().duty; - } - - void Calibration::first_duty(float duty) { - first().duty = duty; - } - - float Calibration::first_speed() const { - return first().speed; - } - - void Calibration::first_speed(float speed) { - first().speed = speed; - } - - Calibration::Pair &Calibration::last() { - assert(calibration_size > 0); - return calibration[calibration_size - 1]; - } - - const Calibration::Pair &Calibration::last() const { - assert(calibration_size > 0); - return calibration[calibration_size - 1]; - } - - float Calibration::last_duty() const { - return last().duty; - } - - void Calibration::last_duty(float duty) { - last().duty = duty; - } - - float Calibration::last_speed() const { - return last().speed; - } - - void Calibration::last_speed(float speed) { - last().speed = speed; - } - - bool Calibration::has_lower_limit() const { - return limit_lower; - } - - bool Calibration::has_upper_limit() const { - return limit_upper; - } - - void Calibration::limit_to_calibration(bool lower, bool upper) { - limit_lower = lower; - limit_upper = upper; - } - - bool Calibration::speed_to_duty(float speed, float &duty_out, float &speed_out) const { - bool success = false; - if(calibration_size >= 2) { - uint8_t last = calibration_size - 1; - - speed_out = speed; - - // Is the speed below the bottom most calibration pair? - if(speed < calibration[0].speed) { - // Should the speed be limited to the calibration or projected below it? - if(limit_lower) { - duty_out = calibration[0].duty; - speed_out = calibration[0].speed; - } - else { - duty_out = map_float(speed, calibration[0].speed, calibration[1].speed, - calibration[0].duty, calibration[1].duty); - } - } - // Is the speed above the top most calibration pair? - else if(speed > calibration[last].speed) { - // Should the speed be limited to the calibration or projected above it? - if(limit_upper) { - duty_out = calibration[last].duty; - speed_out = calibration[last].speed; - } - else { - duty_out = map_float(speed, calibration[last - 1].speed, calibration[last].speed, - calibration[last - 1].duty, calibration[last].duty); - } - } - else { - // The speed must between two calibration pairs, so iterate through them to find which ones - for(uint8_t i = 0; i < last; i++) { - if(speed <= calibration[i + 1].speed) { - duty_out = map_float(speed, calibration[i].speed, calibration[i + 1].speed, - calibration[i].duty, calibration[i + 1].duty); - break; // No need to continue checking so break out of the loop - } - } - } - - // Clamp the duty between the hard limits - if(duty_out < LOWER_HARD_LIMIT || duty_out > UPPER_HARD_LIMIT) { - duty_out = MIN(MAX(duty_out, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT); - - // Is the duty below the bottom most calibration pair? - if(duty_out < calibration[0].duty) { - speed_out = map_float(duty_out, calibration[0].duty, calibration[1].duty, - calibration[0].speed, calibration[1].speed); - } - // Is the duty above the top most calibration pair? - else if(duty_out > calibration[last].duty) { - speed_out = map_float(duty_out, calibration[last - 1].duty, calibration[last].duty, - calibration[last - 1].speed, calibration[last].speed); - } - else { - // The duty must between two calibration pairs, so iterate through them to find which ones - for(uint8_t i = 0; i < last; i++) { - if(duty_out <= calibration[i + 1].duty) { - speed_out = map_float(duty_out, calibration[i].duty, calibration[i + 1].duty, - calibration[i].speed, calibration[i + 1].speed); - break; // No need to continue checking so break out of the loop - } - } - } - } - - success = true; - } - - return success; - } - - bool Calibration::duty_to_speed(float duty, float &speed_out, float &duty_out) const { - bool success = false; - if(calibration_size >= 2) { - uint8_t last = calibration_size - 1; - - // Clamp the duty between the hard limits - duty_out = MIN(MAX(duty, LOWER_HARD_LIMIT), UPPER_HARD_LIMIT); - - // Is the duty below the bottom most calibration pair? - if(duty_out < calibration[0].duty) { - // Should the duty be limited to the calibration or projected below it? - if(limit_lower) { - speed_out = calibration[0].speed; - duty_out = calibration[0].duty; - } - else { - speed_out = map_float(duty, calibration[0].duty, calibration[1].duty, - calibration[0].speed, calibration[1].speed); - } - } - // Is the duty above the top most calibration pair? - else if(duty > calibration[last].duty) { - // Should the duty be limited to the calibration or projected above it? - if(limit_upper) { - speed_out = calibration[last].speed; - duty_out = calibration[last].duty; - } - else { - speed_out = map_float(duty, calibration[last - 1].duty, calibration[last].duty, - calibration[last - 1].speed, calibration[last].speed); - } - } - else { - // The duty must between two calibration pairs, so iterate through them to find which ones - for(uint8_t i = 0; i < last; i++) { - if(duty <= calibration[i + 1].duty) { - speed_out = map_float(duty, calibration[i].duty, calibration[i + 1].duty, - calibration[i].speed, calibration[i + 1].speed); - break; // No need to continue checking so break out of the loop - } - } - } - - success = true; - } - - return success; - } - - float Calibration::map_float(float in, float in_min, float in_max, float out_min, float out_max) { - return (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min; - } -}; \ No newline at end of file diff --git a/drivers/motor/calibration.hpp b/drivers/motor/calibration.hpp deleted file mode 100644 index 447773d2..00000000 --- a/drivers/motor/calibration.hpp +++ /dev/null @@ -1,119 +0,0 @@ -#pragma once - -#include "pico/stdlib.h" - -namespace motor { - - enum CalibrationType { - ANGULAR = 0, - LINEAR, - CONTINUOUS - }; - - class Calibration { - //-------------------------------------------------- - // Constants - //-------------------------------------------------- - public: - static constexpr float DEFAULT_MIN_PULSE = 500.0f; // in microseconds - static constexpr float DEFAULT_MID_PULSE = 1500.0f; // in microseconds - static constexpr float DEFAULT_MAX_PULSE = 2500.0f; // in microseconds - - private: - static constexpr float LOWER_HARD_LIMIT = 400.0f; // The minimum microsecond duty to send - static constexpr float UPPER_HARD_LIMIT = 2600.0f; // The maximum microsecond duty to send - - - //-------------------------------------------------- - // Substructures - //-------------------------------------------------- - public: - struct Pair { - //-------------------------------------------------- - // Constructors/Destructor - //-------------------------------------------------- - Pair(); - Pair(float duty, float speed); - - - //-------------------------------------------------- - // Variables - //-------------------------------------------------- - float duty; - float speed; - }; - - - //-------------------------------------------------- - // Constructors/Destructor - //-------------------------------------------------- - public: - Calibration(); - Calibration(CalibrationType default_type); - Calibration(const Calibration &other); - virtual ~Calibration(); - - - //-------------------------------------------------- - // Operators - //-------------------------------------------------- - public: - Calibration &operator=(const Calibration &other); - Pair &operator[](uint8_t index); - const Pair &operator[](uint8_t index) const; - - - //-------------------------------------------------- - // Methods - //-------------------------------------------------- - public: - void apply_blank_pairs(uint size); - void apply_two_pairs(float min_duty, float max_duty, float min_speed, float max_speed); - void apply_three_pairs(float min_duty, float mid_duty, float max_duty, float min_speed, float mid_speed, float max_speed); - void apply_uniform_pairs(uint size, float min_duty, float max_duty, float min_speed, float max_speed); - void apply_default_pairs(CalibrationType default_type); - - uint size() const; - - Pair &pair(uint8_t index); // Ensure the pairs are assigned in ascending speed order - const Pair &pair(uint8_t index) const; // Ensure the pairs are assigned in ascending speed order - float duty(uint8_t index) const; - void duty(uint8_t index, float duty); - float speed(uint8_t index) const; - void speed(uint8_t index, float speed); - - Pair &first(); - const Pair &first() const; - float first_duty() const; - void first_duty(float duty); - float first_speed() const; - void first_speed(float speed); - - Pair &last(); - const Pair &last() const; - float last_duty() const; - void last_duty(float duty); - float last_speed() const; - void last_speed(float speed); - - bool has_lower_limit() const; - bool has_upper_limit() const; - void limit_to_calibration(bool lower, bool upper); - - bool speed_to_duty(float speed, float &duty_out, float &speed_out) const; - bool duty_to_speed(float duty, float &speed_out, float &duty_out) const; - - //static float map_float(float in, float in_min, float in_max, float out_min, float out_max); - - - //-------------------------------------------------- - // Variables - //-------------------------------------------------- - private: - Pair* calibration; - uint calibration_size; - bool limit_lower; - bool limit_upper; - }; - -} \ No newline at end of file diff --git a/drivers/motor/motor.cmake b/drivers/motor/motor.cmake index 7f498ab6..38001cf8 100644 --- a/drivers/motor/motor.cmake +++ b/drivers/motor/motor.cmake @@ -3,6 +3,7 @@ add_library(${DRIVER_NAME} INTERFACE) target_sources(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}/motor.cpp + ${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp ) target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) diff --git a/drivers/motor/motor.cpp b/drivers/motor/motor.cpp index 50dbe95b..d1db206e 100644 --- a/drivers/motor/motor.cpp +++ b/drivers/motor/motor.cpp @@ -1,14 +1,14 @@ #include "motor.hpp" #include "pwm.hpp" -namespace pimoroni { - Motor::Motor(uint pin_pos, uint pin_neg, float freq, DecayMode mode) - : pin_pos(pin_pos), pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) { +namespace motor { + Motor::Motor(const MotorPins &pins, float freq, DecayMode mode) + : pins(pins), pwm_frequency(freq), motor_decay_mode(mode) { } Motor::~Motor() { - gpio_set_function(pin_pos, GPIO_FUNC_NULL); - gpio_set_function(pin_neg, GPIO_FUNC_NULL); + gpio_set_function(pins.positive, GPIO_FUNC_NULL); + gpio_set_function(pins.negative, GPIO_FUNC_NULL); } bool Motor::init() { @@ -26,11 +26,11 @@ namespace pimoroni { //Apply the divider pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); - pwm_init(pwm_gpio_to_slice_num(pin_pos), &pwm_cfg, true); - gpio_set_function(pin_pos, GPIO_FUNC_PWM); + pwm_init(pwm_gpio_to_slice_num(pins.positive), &pwm_cfg, true); + gpio_set_function(pins.positive, GPIO_FUNC_PWM); - pwm_init(pwm_gpio_to_slice_num(pin_neg), &pwm_cfg, true); - gpio_set_function(pin_neg, GPIO_FUNC_PWM); + pwm_init(pwm_gpio_to_slice_num(pins.negative), &pwm_cfg, true); + gpio_set_function(pins.negative, GPIO_FUNC_PWM); update_pwm(); success = true; @@ -66,8 +66,8 @@ namespace pimoroni { pwm_period = period; pwm_frequency = freq; - uint pos_num = pwm_gpio_to_slice_num(pin_pos); - uint neg_num = pwm_gpio_to_slice_num(pin_neg); + uint pos_num = pwm_gpio_to_slice_num(pins.positive); + uint neg_num = pwm_gpio_to_slice_num(pins.negative); //Apply the new divider uint8_t div = div16 >> 4; @@ -112,8 +112,8 @@ namespace pimoroni { void Motor::disable() { motor_speed = 0.0f; - pwm_set_gpio_level(pin_pos, 0); - pwm_set_gpio_level(pin_neg, 0); + pwm_set_gpio_level(pins.positive, 0); + pwm_set_gpio_level(pins.negative, 0); } void Motor::update_pwm() { @@ -122,24 +122,24 @@ namespace pimoroni { switch(motor_decay_mode) { case SLOW_DECAY: //aka 'Braking' if(signed_duty_cycle >= 0) { - pwm_set_gpio_level(pin_pos, pwm_period); - pwm_set_gpio_level(pin_neg, pwm_period - signed_duty_cycle); + pwm_set_gpio_level(pins.positive, pwm_period); + pwm_set_gpio_level(pins.negative, pwm_period - signed_duty_cycle); } else { - pwm_set_gpio_level(pin_pos, pwm_period + signed_duty_cycle); - pwm_set_gpio_level(pin_neg, pwm_period); + pwm_set_gpio_level(pins.positive, pwm_period + signed_duty_cycle); + pwm_set_gpio_level(pins.negative, pwm_period); } break; case FAST_DECAY: //aka 'Coasting' default: if(signed_duty_cycle >= 0) { - pwm_set_gpio_level(pin_pos, signed_duty_cycle); - pwm_set_gpio_level(pin_neg, 0); + pwm_set_gpio_level(pins.positive, signed_duty_cycle); + pwm_set_gpio_level(pins.negative, 0); } else { - pwm_set_gpio_level(pin_pos, 0); - pwm_set_gpio_level(pin_neg, 0 - signed_duty_cycle); + pwm_set_gpio_level(pins.positive, 0); + pwm_set_gpio_level(pins.negative, 0 - signed_duty_cycle); } break; } diff --git a/drivers/motor/motor.hpp b/drivers/motor/motor.hpp index eb3d6922..a2e7bff3 100644 --- a/drivers/motor/motor.hpp +++ b/drivers/motor/motor.hpp @@ -2,8 +2,9 @@ #include "pico/stdlib.h" #include "hardware/pwm.h" +#include "motor_state.hpp" -namespace pimoroni { +namespace motor { class Motor { //-------------------------------------------------- @@ -28,8 +29,7 @@ namespace pimoroni { // Variables //-------------------------------------------------- private: - uint pin_pos; - uint pin_neg; + MotorPins pins; pwm_config pwm_cfg; uint16_t pwm_period; float pwm_frequency = DEFAULT_PWM_FREQUENCY; @@ -42,7 +42,7 @@ namespace pimoroni { // Constructors/Destructor //-------------------------------------------------- public: - Motor(uint pin_pos, uint pin_neg, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); + Motor(const MotorPins &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); ~Motor(); diff --git a/drivers/motor/motor2.cpp b/drivers/motor/motor2.cpp index 441e4957..2f584796 100644 --- a/drivers/motor/motor2.cpp +++ b/drivers/motor/motor2.cpp @@ -3,17 +3,13 @@ #include "pwm.hpp" namespace motor { - Motor2::Motor2(uint pin_pos, uint pin_neg, float freq, MotorState::DecayMode mode) - : motor_pin_pos(pin_pos), motor_pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) { + Motor2::Motor2(const MotorPins &pins, float speed_scale, float freq, MotorState::DecayMode mode) + : motor_pins(pins), state(speed_scale, false), pwm_frequency(freq), motor_decay_mode(mode) { } - // Motor2::Motor2(uint pin, /*const Calibration& calibration,*/ float freq) - // : motor_pin_pos(pin), /*state(calibration),*/ pwm_frequency(freq) { - // } - Motor2::~Motor2() { - gpio_set_function(motor_pin_pos, GPIO_FUNC_NULL); - gpio_set_function(motor_pin_neg, GPIO_FUNC_NULL); + gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL); + gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL); } bool Motor2::init() { @@ -31,43 +27,43 @@ namespace motor { // Apply the divider pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); // There's no 'pwm_config_set_clkdiv_int_frac' for some reason... - pwm_init(pwm_gpio_to_slice_num(motor_pin_pos), &pwm_cfg, true); - gpio_set_function(motor_pin_pos, GPIO_FUNC_PWM); + pwm_init(pwm_gpio_to_slice_num(motor_pins.positive), &pwm_cfg, true); + gpio_set_function(motor_pins.positive, GPIO_FUNC_PWM); - pwm_init(pwm_gpio_to_slice_num(motor_pin_neg), &pwm_cfg, true); - gpio_set_function(motor_pin_neg, GPIO_FUNC_PWM); + pwm_init(pwm_gpio_to_slice_num(motor_pins.negative), &pwm_cfg, true); + gpio_set_function(motor_pins.negative, GPIO_FUNC_PWM); - pwm_set_gpio_level(motor_pin_pos, 0); - pwm_set_gpio_level(motor_pin_neg, 0); + pwm_set_gpio_level(motor_pins.positive, 0); + pwm_set_gpio_level(motor_pins.negative, 0); success = true; } return success; } - uint Motor2::pin() const { - return motor_pin_pos; + MotorPins Motor2::pins() const { + return motor_pins; } void Motor2::enable() { - apply_duty(state.enable()); + apply_duty(state.enable_with_return()); } void Motor2::disable() { - apply_duty(state.disable()); + apply_duty(state.disable_with_return()); } bool Motor2::is_enabled() const { return state.is_enabled(); } - // float Motor2::duty() const { - // return state.get_duty(); - // } + float Motor2::duty() const { + return state.get_duty(); + } - // void Motor2::duty(float duty) { - // apply_duty(state.set_duty_with_return(duty)); - // } + void Motor2::duty(float duty) { + apply_duty(state.set_duty_with_return(duty)); + } float Motor2::speed() const { return state.get_speed(); @@ -77,6 +73,22 @@ namespace motor { apply_duty(state.set_speed_with_return(speed)); } + float Motor2::speed_scale() const { + return state.get_speed_scale(); + } + + void Motor2::speed_scale(float speed_scale) { + state.set_speed_scale(speed_scale); + } + + void Motor2::invert_direction(bool invert) { + state.invert_direction(invert); + } + + bool Motor2::is_inverted() const { + return state.is_inverted(); + } + float Motor2::frequency() const { return pwm_frequency; } @@ -97,12 +109,15 @@ namespace motor { pwm_period = period; pwm_frequency = freq; - uint pin_num = pwm_gpio_to_slice_num(motor_pin_pos); + uint pos_pin_num = pwm_gpio_to_slice_num(motor_pins.positive); + uint neg_pin_num = pwm_gpio_to_slice_num(motor_pins.negative); // Apply the new divider uint8_t div = div16 >> 4; uint8_t mod = div16 % 16; - pwm_set_clkdiv_int_frac(pin_num, div, mod); + pwm_set_clkdiv_int_frac(pos_pin_num, div, mod); + if(neg_pin_num != pos_pin_num) + pwm_set_clkdiv_int_frac(neg_pin_num, div, mod); // If the the period is larger, update the pwm before setting the new wraps if(state.is_enabled() && pre_update_pwm) { @@ -110,7 +125,9 @@ namespace motor { } // Set the new wrap (should be 1 less than the period to get full 0 to 100%) - pwm_set_wrap(pin_num, pwm_period - 1); + pwm_set_wrap(pos_pin_num, pwm_period - 1); + if(neg_pin_num != pos_pin_num) + pwm_set_wrap(neg_pin_num, pwm_period - 1); // If the the period is smaller, update the pwm after setting the new wraps if(state.is_enabled() && !pre_update_pwm) { @@ -140,28 +157,12 @@ namespace motor { disable(); } - float Motor2::min_speed() const { - return state.get_min_speed(); + void Motor2::to_full_negative() { + apply_duty(state.to_full_negative_with_return()); } - // float Motor2::mid_speed() const { - // return state.get_mid_speed(); - // } - - float Motor2::max_speed() const { - return state.get_max_speed(); - } - - void Motor2::to_min() { - apply_duty(state.to_min_with_return()); - } - - // void Motor2::to_mid() { - // apply_duty(state.to_mid_with_return()); - // } - - void Motor2::to_max() { - apply_duty(state.to_max_with_return()); + void Motor2::to_full_positive() { + apply_duty(state.to_full_positive_with_return()); } void Motor2::to_percent(float in, float in_min, float in_max) { @@ -172,38 +173,30 @@ namespace motor { apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max)); } - // Calibration& Motor2::calibration() { - // return state.calibration(); - // } - - // const Calibration& Motor2::calibration() const { - // return state.calibration(); - // } - void Motor2::apply_duty(float duty) { int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); switch(motor_decay_mode) { case MotorState::SLOW_DECAY: //aka 'Braking' if(signed_level >= 0) { - pwm_set_gpio_level(motor_pin_pos, pwm_period); - pwm_set_gpio_level(motor_pin_neg, pwm_period - signed_level); + pwm_set_gpio_level(motor_pins.positive, pwm_period); + pwm_set_gpio_level(motor_pins.negative, pwm_period - signed_level); } else { - pwm_set_gpio_level(motor_pin_pos, pwm_period + signed_level); - pwm_set_gpio_level(motor_pin_neg, pwm_period); + pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level); + pwm_set_gpio_level(motor_pins.negative, pwm_period); } break; case MotorState::FAST_DECAY: //aka 'Coasting' default: if(signed_level >= 0) { - pwm_set_gpio_level(motor_pin_pos, signed_level); - pwm_set_gpio_level(motor_pin_neg, 0); + pwm_set_gpio_level(motor_pins.positive, signed_level); + pwm_set_gpio_level(motor_pins.negative, 0); } else { - pwm_set_gpio_level(motor_pin_pos, 0); - pwm_set_gpio_level(motor_pin_neg, 0 - signed_level); + pwm_set_gpio_level(motor_pins.positive, 0); + pwm_set_gpio_level(motor_pins.negative, 0 - signed_level); } break; } diff --git a/drivers/motor/motor2.hpp b/drivers/motor/motor2.hpp index 43310c14..b4648e64 100644 --- a/drivers/motor/motor2.hpp +++ b/drivers/motor/motor2.hpp @@ -11,8 +11,7 @@ namespace motor { // Variables //-------------------------------------------------- private: - uint motor_pin_pos; - uint motor_pin_neg; + MotorPins motor_pins; MotorState state; pwm_config pwm_cfg; uint16_t pwm_period; @@ -24,8 +23,7 @@ namespace motor { // Constructors/Destructor //-------------------------------------------------- public: - Motor2(uint pin_pos, uint pin_neg, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE); - //Motor2(uint pin, /*const Calibration& calibration,*/ float freq = MotorState::DEFAULT_FREQUENCY); + Motor2(const MotorPins &pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE); ~Motor2(); @@ -36,18 +34,24 @@ namespace motor { bool init(); // For print access in micropython - uint pin() const; + MotorPins pins() const; void enable(); void disable(); bool is_enabled() const; - //float duty() const; - //void duty(float duty); + float duty() const; + void duty(float duty); float speed() const; void speed(float speed); + float speed_scale() const; + void speed_scale(float speed_scale); + + void invert_direction(bool invert); + bool is_inverted() const; + float frequency() const; bool frequency(float freq); @@ -58,19 +62,11 @@ namespace motor { void coast(); // An alias for disable //-------------------------------------------------- - float min_speed() const; - //float mid_speed() const; - float max_speed() const; - - void to_min(); - //void to_mid(); - void to_max(); + void to_full_negative(); + void to_full_positive(); void to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT); void to_percent(float in, float in_min, float in_max, float speed_min, float speed_max); - //Calibration& calibration(); - //const Calibration& calibration() const; - //-------------------------------------------------- private: void apply_duty(float duty); diff --git a/drivers/motor/motor_cluster.cpp b/drivers/motor/motor_cluster.cpp index ad97751d..9760cc8e 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -3,44 +3,24 @@ #include namespace motor { - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(default_type, auto_phase); + create_motor_states(speed_scale, inverted, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(default_type, auto_phase); + create_motor_states(speed_scale, inverted, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(default_type, auto_phase); + create_motor_states(speed_scale, inverted, auto_phase); } - MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pins, CalibrationType default_type, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) + MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pins, float speed_scale, bool inverted, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(default_type, auto_phase); - } - - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pin_mask, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(calibration, auto_phase); - } - - MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pin_base, pin_count, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(calibration, auto_phase); - } - - MotorCluster::MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pins, length, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(calibration, auto_phase); - } - - MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pins, const Calibration& calibration, float freq, bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer) - : pwms(pio, sm, pins, seq_buffer, dat_buffer), pwm_frequency(freq) { - create_motor_states(calibration, auto_phase); + create_motor_states(speed_scale, inverted, auto_phase); } MotorCluster::~MotorCluster() { @@ -90,8 +70,8 @@ namespace motor { void MotorCluster::enable(uint8_t motor, bool load) { assert(motor < pwms.get_chan_count()); - float new_pulse = states[motor].enable(); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].enable_with_return(); + apply_duty(motor, new_duty, load); } void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) { @@ -122,8 +102,8 @@ namespace motor { void MotorCluster::disable(uint8_t motor, bool load) { assert(motor < pwms.get_chan_count()); - float new_pulse = states[motor].disable(); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].disable_with_return(); + apply_duty(motor, new_duty, load); } void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) { @@ -157,38 +137,38 @@ namespace motor { return states[motor].is_enabled(); } - float MotorCluster::pulse(uint8_t motor) const { + float MotorCluster::duty(uint8_t motor) const { assert(motor < pwms.get_chan_count()); - return states[motor].get_pulse(); + return states[motor].get_duty(); } - void MotorCluster::pulse(uint8_t motor, float pulse, bool load) { + void MotorCluster::duty(uint8_t motor, float duty, bool load) { assert(motor < pwms.get_chan_count()); - float new_pulse = states[motor].set_pulse_with_return(pulse); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].set_duty_with_return(duty); + apply_duty(motor, new_duty, load); } - void MotorCluster::pulse(const uint8_t *motors, uint8_t length, float pulse, bool load) { + void MotorCluster::duty(const uint8_t *motors, uint8_t length, float duty, bool load) { assert(motors != nullptr); for(uint8_t i = 0; i < length; i++) { - this->pulse(motors[i], pulse, false); + this->duty(motors[i], duty, false); } if(load) pwms.load_pwm(); } - void MotorCluster::pulse(std::initializer_list motors, float pulse, bool load) { + void MotorCluster::duty(std::initializer_list motors, float duty, bool load) { for(auto motor : motors) { - this->pulse(motor, pulse, false); + this->duty(motor, duty, false); } if(load) pwms.load_pwm(); } - void MotorCluster::all_to_pulse(float pulse, bool load) { + void MotorCluster::all_to_duty(float duty, bool load) { uint8_t motor_count = pwms.get_chan_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - this->pulse(motor, pulse, false); + this->duty(motor, duty, false); } if(load) pwms.load_pwm(); @@ -201,8 +181,8 @@ namespace motor { void MotorCluster::speed(uint8_t motor, float speed, bool load) { assert(motor < pwms.get_chan_count()); - float new_pulse = states[motor].set_speed_with_return(speed); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].set_speed_with_return(speed); + apply_duty(motor, new_duty, load); } void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) { @@ -287,7 +267,7 @@ namespace motor { uint8_t motor_count = pwms.get_chan_count(); for(uint motor = 0; motor < motor_count; motor++) { if(states[motor].is_enabled()) { - apply_pulse(motor, states[motor].get_pulse(), false); + apply_duty(motor, states[motor].get_duty(), false); } pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwm_period), false); } @@ -306,112 +286,70 @@ namespace motor { return success; } - float MotorCluster::min_speed(uint8_t motor) const { + float MotorCluster::speed_scale(uint8_t motor) const { assert(is_assigned(motor)); - return states[motor].get_min_speed(); + return states[motor].get_speed_scale(); } - float MotorCluster::mid_speed(uint8_t motor) const { + void MotorCluster::to_full_negative(uint8_t motor, bool load) { assert(is_assigned(motor)); - return states[motor].get_mid_speed(); + float new_duty = states[motor].to_full_negative_with_return(); + apply_duty(motor, new_duty, load); } - float MotorCluster::max_speed(uint8_t motor) const { - assert(is_assigned(motor)); - return states[motor].get_max_speed(); - } - - void MotorCluster::to_min(uint8_t motor, bool load) { - assert(is_assigned(motor)); - float new_pulse = states[motor].to_min_with_return(); - apply_pulse(motor, new_pulse, load); - } - - void MotorCluster::to_min(const uint8_t *motors, uint8_t length, bool load) { + void MotorCluster::to_full_negative(const uint8_t *motors, uint8_t length, bool load) { assert(motors != nullptr); for(uint8_t i = 0; i < length; i++) { - to_min(motors[i], false); + to_full_negative(motors[i], false); } if(load) pwms.load_pwm(); } - void MotorCluster::to_min(std::initializer_list motors, bool load) { + void MotorCluster::to_full_negative(std::initializer_list motors, bool load) { for(auto motor : motors) { - to_min(motor, false); + to_full_negative(motor, false); } if(load) pwms.load_pwm(); } - void MotorCluster::all_to_min(bool load) { + void MotorCluster::all_to_full_negative(bool load) { uint8_t motor_count = pwms.get_chan_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - to_min(motor, false); + to_full_negative(motor, false); } if(load) pwms.load_pwm(); } - void MotorCluster::to_mid(uint8_t motor, bool load) { + void MotorCluster::to_full_positive(uint8_t motor, bool load) { assert(is_assigned(motor)); - float new_pulse = states[motor].to_mid_with_return(); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].to_full_positive_with_return(); + apply_duty(motor, new_duty, load); } - void MotorCluster::to_mid(const uint8_t *motors, uint8_t length, bool load) { + void MotorCluster::to_full_positive(const uint8_t *motors, uint8_t length, bool load) { assert(motors != nullptr); for(uint8_t i = 0; i < length; i++) { - to_mid(motors[i], false); + to_full_positive(motors[i], false); } if(load) pwms.load_pwm(); } - void MotorCluster::to_mid(std::initializer_list motors, bool load) { + void MotorCluster::to_full_positive(std::initializer_list motors, bool load) { for(auto motor : motors) { - to_mid(motor, false); + to_full_positive(motor, false); } if(load) pwms.load_pwm(); } - void MotorCluster::all_to_mid(bool load) { + void MotorCluster::all_to_full_positive(bool load) { uint8_t motor_count = pwms.get_chan_count(); for(uint8_t motor = 0; motor < motor_count; motor++) { - to_mid(motor, false); - } - if(load) - pwms.load_pwm(); - } - - void MotorCluster::to_max(uint8_t motor, bool load) { - assert(is_assigned(motor)); - float new_pulse = states[motor].to_max_with_return(); - apply_pulse(motor, new_pulse, load); - } - - void MotorCluster::to_max(const uint8_t *motors, uint8_t length, bool load) { - assert(motors != nullptr); - for(uint8_t i = 0; i < length; i++) { - to_max(motors[i], false); - } - if(load) - pwms.load_pwm(); - } - - void MotorCluster::to_max(std::initializer_list motors, bool load) { - for(auto motor : motors) { - to_max(motor, false); - } - if(load) - pwms.load_pwm(); - } - - void MotorCluster::all_to_max(bool load) { - uint8_t motor_count = pwms.get_chan_count(); - for(uint8_t motor = 0; motor < motor_count; motor++) { - to_max(motor, false); + to_full_positive(motor, false); } if(load) pwms.load_pwm(); @@ -419,8 +357,8 @@ namespace motor { void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) { assert(is_assigned(motor)); - float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].to_percent_with_return(in, in_min, in_max); + apply_duty(motor, new_duty, load); } void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) { @@ -451,8 +389,8 @@ namespace motor { void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) { assert(is_assigned(motor)); - float new_pulse = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max); - apply_pulse(motor, new_pulse, load); + float new_duty = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max); + apply_duty(motor, new_duty, load); } void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) { @@ -481,45 +419,22 @@ namespace motor { pwms.load_pwm(); } - Calibration& MotorCluster::calibration(uint8_t motor) { - assert(is_assigned(motor)); - return states[motor].calibration(); - } - - const Calibration& MotorCluster::calibration(uint8_t motor) const { - assert(is_assigned(motor)); - return states[motor].calibration(); - } - void MotorCluster::load() { pwms.load_pwm(); } - void MotorCluster::apply_pulse(uint8_t motor, float pulse, bool load) { - pwms.set_chan_level(motor, MotorState::pulse_to_level(pulse, pwm_period, pwm_frequency), load); + void MotorCluster::apply_duty(uint8_t motor, float duty, bool load) { + pwms.set_chan_level(motor, MotorState::duty_to_level(duty, pwm_period), load); } - void MotorCluster::create_motor_states(CalibrationType default_type, bool auto_phase) { + void MotorCluster::create_motor_states(float speed_scale, bool inverted, bool auto_phase) { uint8_t motor_count = pwms.get_chan_count(); if(motor_count > 0) { states = new MotorState[motor_count]; motor_phases = new float[motor_count]; for(uint motor = 0; motor < motor_count; motor++) { - states[motor] = MotorState(default_type); - motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; - } - } - } - - void MotorCluster::create_motor_states(const Calibration& calibration, bool auto_phase) { - uint8_t motor_count = pwms.get_chan_count(); - if(motor_count > 0) { - states = new MotorState[motor_count]; - motor_phases = new float[motor_count]; - - for(uint motor = 0; motor < motor_count; motor++) { - states[motor] = MotorState(calibration); + states[motor] = MotorState(speed_scale, inverted); motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; } } diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index 29c275b3..b4d66b30 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -24,15 +24,10 @@ namespace motor { // Constructors/Destructor //-------------------------------------------------- public: - MotorCluster(PIO pio, uint sm, uint pin_mask, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, std::initializer_list pins, CalibrationType default_type = ANGULAR, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - - MotorCluster(PIO pio, uint sm, uint pin_mask, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); - MotorCluster(PIO pio, uint sm, std::initializer_list pins, const Calibration& calibration, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, uint pin_mask, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_count, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, const uint8_t *pins, uint32_t length, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); + MotorCluster(PIO pio, uint sm, std::initializer_list pins, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, bool inverted = false, float freq = MotorState::DEFAULT_FREQUENCY, bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr); ~MotorCluster(); @@ -79,24 +74,17 @@ namespace motor { bool frequency(float freq); //-------------------------------------------------- - float min_speed(uint8_t motor) const; - float mid_speed(uint8_t motor) const; - float max_speed(uint8_t motor) const; + float speed_scale(uint8_t motor) const; - void to_min(uint8_t motor, bool load = true); - void to_min(const uint8_t *motors, uint8_t length, bool load = true); - void to_min(std::initializer_list motors, bool load = true); - void all_to_min(bool load = true); + void to_full_negative(uint8_t motor, bool load = true); + void to_full_negative(const uint8_t *motors, uint8_t length, bool load = true); + void to_full_negative(std::initializer_list motors, bool load = true); + void all_to_full_negative(bool load = true); - void to_mid(uint8_t motor, bool load = true); - void to_mid(const uint8_t *motors, uint8_t length, bool load = true); - void to_mid(std::initializer_list motors, bool load = true); - void all_to_mid(bool load = true); - - void to_max(uint8_t motor, bool load = true); - void to_max(const uint8_t *motors, uint8_t length, bool load = true); - void to_max(std::initializer_list motors, bool load = true); - void all_to_max(bool load = true); + void to_full_positive(uint8_t motor, bool load = true); + void to_full_positive(const uint8_t *motors, uint8_t length, bool load = true); + void to_full_positive(std::initializer_list motors, bool load = true); + void all_to_full_positive(bool load = true); void to_percent(uint8_t motor, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true); void to_percent(const uint8_t *motors, uint8_t length, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true); @@ -108,16 +96,12 @@ namespace motor { void to_percent(std::initializer_list motors, float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true); void all_to_percent(float in, float in_min, float in_max, float speed_min, float speed_max, bool load = true); - Calibration& calibration(uint8_t motor); - const Calibration& calibration(uint8_t motor) const; - void load(); //-------------------------------------------------- private: void apply_duty(uint8_t motor, float duty, bool load); - void create_motor_states(CalibrationType default_type, bool auto_phase); - void create_motor_states(const Calibration& calibration, bool auto_phase); + void create_motor_states(float speed_scale, bool inverted, bool auto_phase); }; } \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index 1a9f7d7e..079038e1 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -1,31 +1,33 @@ #include "motor_state.hpp" +#include "float.h" namespace motor { - MotorState::MotorState() { + MotorState::MotorState() + : motor_speed(0.0f), motor_scale(DEFAULT_SPEED_SCALE), inverted(false), last_enabled_duty(0.0f), enabled(false) { } - //MotorState::MotorState(CalibrationType default_type) - // : calib(default_type) { - //} - - //MotorState::MotorState(const Calibration& calibration) - // : calib(calibration) { - //} - - float MotorState::enable() { - return _enable(); + MotorState::MotorState(float speed_scale, bool inverted) + : motor_speed(0.0f), motor_scale(MAX(speed_scale, FLT_EPSILON)), inverted(inverted), last_enabled_duty(0.0f), enabled(false) { } - float MotorState::disable() { + float MotorState::enable_with_return() { + enabled = true; + + float duty = 0.0f; + if((duty <= 0.0f - deadzone_percent) || (duty >= deadzone_percent)) { + if(inverted) + duty = 0.0f - last_enabled_duty; + else + duty = last_enabled_duty; + } + return duty; + } + + float MotorState::disable_with_return() { enabled = false; return 0.0f; // A zero duty } - float MotorState::_enable() { - enabled = true; - return last_enabled_duty; - } - bool MotorState::is_enabled() const { return enabled; } @@ -35,14 +37,11 @@ namespace motor { } float MotorState::set_duty_with_return(float duty) { - //TODO - // float speed_out, duty_out; - // if(calib.duty_to_speed(duty, speed_out, duty_out)) { - // motor_speed = speed_out; - // last_enabled_duty = duty_out; - // return _enable(); - // } - return disable(); + // Clamp the duty between the hard limits + last_enabled_duty = MIN(MAX(duty, -1.0f), 1.0f); + motor_speed = last_enabled_duty * motor_scale; + + return enable_with_return(); } float MotorState::get_speed() const { @@ -50,58 +49,48 @@ namespace motor { } float MotorState::set_speed_with_return(float speed) { - //TODO - // float duty_out, speed_out; - // if(calib.speed_to_duty(speed, duty_out, speed_out)) { - // last_enabled_duty = duty_out; - // motor_speed = speed_out; - // return _enable(); - // } - return disable(); + // Clamp the speed between the hard limits + motor_speed = MIN(MAX(speed, -motor_scale), motor_scale); + last_enabled_duty = motor_speed / motor_scale; + + return enable_with_return(); } - float MotorState::get_min_speed() const { - float speed = 0.0f; - //TODO - //if(calib.size() > 0) { - // speed = calib.first().speed; - //} - return speed; + float MotorState::get_speed_scale() const { + return motor_scale; } - // float MotorState::get_mid_speed() const { - // float speed = 0.0f; - // if(calib.size() > 0) { - // const Calibration::Pair &first = calib.first(); - // const Calibration::Pair &last = calib.last(); - // speed = (first.speed + last.speed) / 2.0f; - // } - // return speed; - // } - - float MotorState::get_max_speed() const { - float speed = 0.0f; - //TODO - //if(calib.size() > 0) { - // speed = calib.last().speed; - //} - return speed; + void MotorState::set_speed_scale(float speed_scale) { + motor_scale = MAX(speed_scale, FLT_EPSILON); + motor_speed = last_enabled_duty * motor_scale; } - float MotorState::to_min_with_return() { - return set_speed_with_return(get_min_speed()); + void MotorState::invert_direction(bool invert) { + if(invert != inverted) { + inverted = invert; + motor_speed = 0.0f - motor_speed; + last_enabled_duty = 0.0f - last_enabled_duty; + } } - // float MotorState::to_mid_with_return() { - // return set_speed_with_return(get_mid_speed()); - // } + bool MotorState::is_inverted() const { + return inverted; + } - float MotorState::to_max_with_return() { - return set_speed_with_return(get_max_speed()); + float MotorState::stop_with_return() { + return set_duty_with_return(0.0f); + } + + float MotorState::to_full_negative_with_return() { + return set_duty_with_return(-1.0f); + } + + float MotorState::to_full_positive_with_return() { + return set_duty_with_return(1.0f); } float MotorState::to_percent_with_return(float in, float in_min, float in_max) { - float speed = MotorState::map_float(in, in_min, in_max, get_min_speed(), get_max_speed()); + float speed = MotorState::map_float(in, in_min, in_max, 0.0f - get_speed_scale(), get_speed_scale()); return set_speed_with_return(speed); } @@ -110,14 +99,6 @@ namespace motor { return set_speed_with_return(speed); } - // Calibration& MotorState::calibration() { - // return calib; - // } - - // const Calibration& MotorState::calibration() const { - // return calib; - // } - int32_t MotorState::duty_to_level(float duty, uint32_t resolution) { return (int32_t)(duty * (float)resolution); } diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index 31107fc8..d4b17f3b 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -1,10 +1,25 @@ #pragma once #include "pico/stdlib.h" -#include "calibration.hpp" namespace motor { + struct MotorPins { + uint positive; + uint negative; + + MotorPins() : positive(0), negative(0) {} + MotorPins(uint pos_pin, uint neg_pin) : positive(pos_pin), negative(neg_pin) {} + }; + + struct EncoderPins { + uint a; + uint b; + + EncoderPins() : a(0), b(0) {} + EncoderPins(uint a_pin, uint b_pin) : a(a_pin), b(b_pin) {} + }; + class MotorState { //-------------------------------------------------- // Enums @@ -22,23 +37,24 @@ namespace motor { public: static constexpr float DEFAULT_FREQUENCY = 25000.0f; // The standard motor update rate static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY; + static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor update rate + static constexpr float MIN_FREQUENCY = 10.0f; // Lowest achievable with hardware PWM with good resolution static constexpr float MAX_FREQUENCY = 50000.0f; // Highest nice speed static constexpr float ZERO_PERCENT = 0.0f; static constexpr float ONEHUNDRED_PERCENT = 1.0f; - private: - static constexpr float MIN_VALID_DUTY = 1.0f; - //-------------------------------------------------- // Variables //-------------------------------------------------- private: - float motor_speed = 0.0f; - float last_enabled_duty = 0.0f; - bool enabled = false; - //Calibration calib; + float motor_speed; + float motor_scale; + bool inverted; + float last_enabled_duty; + bool enabled; + float deadzone_percent = 0.0f; //-------------------------------------------------- @@ -46,19 +62,16 @@ namespace motor { //-------------------------------------------------- public: MotorState(); - //MotorState(CalibrationType default_type); - //MotorState(const Calibration& calibration); + MotorState(float speed_scale, bool inverted); //-------------------------------------------------- // Methods //-------------------------------------------------- public: - float enable(); - float disable(); + float enable_with_return(); + float disable_with_return(); bool is_enabled() const; - private: - float _enable(); // Internal version of enable without convenient initialisation to the middle public: float get_duty() const; float set_duty_with_return(float duty); @@ -66,24 +79,23 @@ namespace motor { float get_speed() const; float set_speed_with_return(float speed); - public: - float get_min_speed() const; - //float get_mid_speed() const; - float get_max_speed() const; + float get_speed_scale() const; + void set_speed_scale(float speed_scale); - float to_min_with_return(); - //float to_mid_with_return(); - float to_max_with_return(); + void invert_direction(bool invert); + bool is_inverted() const; + + //-------------------------------------------------- + + float stop_with_return(); + float to_full_negative_with_return(); + float to_full_positive_with_return(); float to_percent_with_return(float in, float in_min = ZERO_PERCENT, float in_max = ONEHUNDRED_PERCENT); float to_percent_with_return(float in, float in_min, float in_max, float speed_min, float speed_max); - //Calibration& calibration(); - //const Calibration& calibration() const; - //-------------------------------------------------- static int32_t duty_to_level(float duty, uint32_t resolution); - private: static float map_float(float in, float in_min, float in_max, float out_min, float out_max); }; diff --git a/examples/motor2040/motor2040_single_motor.cpp b/examples/motor2040/motor2040_single_motor.cpp index be32938e..e217c8fc 100644 --- a/examples/motor2040/motor2040_single_motor.cpp +++ b/examples/motor2040/motor2040_single_motor.cpp @@ -22,7 +22,7 @@ constexpr float SWEEP_EXTENT = 90.0f; // Create a motor on pin 0 and 1 -Motor2 m = Motor2(motor2040::SERVO_1, motor2040::SERVO_2); +Motor2 m = Motor2(motor2040::MOTOR_1); int main() { @@ -31,21 +31,21 @@ int main() { // Initialise the motor m.init(); - // Enable the motor (this puts it at the middle) + // Enable the motor m.enable(); sleep_ms(2000); - // Go to min - m.to_min(); + // Go at full neative + m.to_full_negative(); sleep_ms(2000); - // Go to max - m.to_max(); + // Go at full positive + m.to_full_positive(); sleep_ms(2000); - // Go back to mid - //m.to_mid(); - //sleep_ms(2000); + // Stop the motor + m.stop(); + sleep_ms(2000); // Do a sine sweep for(auto j = 0u; j < SWEEPS; j++) { diff --git a/examples/pico_motor_shim/balance/demo.cpp b/examples/pico_motor_shim/balance/demo.cpp index 8c964d72..95e98aa7 100644 --- a/examples/pico_motor_shim/balance/demo.cpp +++ b/examples/pico_motor_shim/balance/demo.cpp @@ -13,6 +13,7 @@ Press "A" to start and stop the balancer */ using namespace pimoroni; +using namespace motor; static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 static constexpr float Z_BIAS_CORRECTION = 0.5f; //A magic number that seems to correct the MSA301's Z bias @@ -22,8 +23,8 @@ static constexpr float PROPORTIONAL = 0.03f; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); -Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); -Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); I2C i2c(BOARD::BREAKOUT_GARDEN); BreakoutMSA301 msa301(&i2c); diff --git a/examples/pico_motor_shim/sequence/demo.cpp b/examples/pico_motor_shim/sequence/demo.cpp index 0b9767a7..88a4a830 100644 --- a/examples/pico_motor_shim/sequence/demo.cpp +++ b/examples/pico_motor_shim/sequence/demo.cpp @@ -11,6 +11,7 @@ Press "A" to start and stop the movement sequence */ using namespace pimoroni; +using namespace motor; static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 static const uint32_t ACCELERATE_TIME_MS = 2000; @@ -20,8 +21,8 @@ static const uint32_t STOP_TIME_MS = 1000; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); -Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); -Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_1(pico_motor_shim::MOTOR_1);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_2(pico_motor_shim::MOTOR_2);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); static bool button_toggle = false; diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/song/demo.cpp index 75d920db..b6404863 100644 --- a/examples/pico_motor_shim/song/demo.cpp +++ b/examples/pico_motor_shim/song/demo.cpp @@ -11,6 +11,7 @@ Plug a motor into connector 1, and press "A" to start the song playing (does not */ using namespace pimoroni; +using namespace motor; // List frequencies (in hz) to play in sequence here. Use zero for when silence or a pause is wanted // Song from PicoExplorer noise.py @@ -39,11 +40,11 @@ static const uint STATIONARY_TOGGLE_US = 2000; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); #ifdef USE_FAST_DECAY - Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); #else - Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); #endif static bool button_toggle = false; diff --git a/libraries/motor2040/motor2040.cmake b/libraries/motor2040/motor2040.cmake index 96aa50fa..60f67543 100644 --- a/libraries/motor2040/motor2040.cmake +++ b/libraries/motor2040/motor2040.cmake @@ -3,4 +3,4 @@ add_library(motor2040 INTERFACE) target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR}) # Pull in pico libraries that we need -target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2) # motor_cluster) \ No newline at end of file +target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2 motor_cluster) \ No newline at end of file diff --git a/libraries/motor2040/motor2040.hpp b/libraries/motor2040/motor2040.hpp index ee93be86..b362ec99 100644 --- a/libraries/motor2040/motor2040.hpp +++ b/libraries/motor2040/motor2040.hpp @@ -7,25 +7,38 @@ namespace motor { namespace motor2040 { - const uint SERVO_1 = 0; - const uint SERVO_2 = 1; - const uint SERVO_3 = 2; - const uint SERVO_4 = 3; - const uint SERVO_5 = 4; - const uint SERVO_6 = 5; - const uint SERVO_7 = 6; - const uint SERVO_8 = 7; - const uint SERVO_9 = 8; - const uint SERVO_10 = 9; - const uint SERVO_11 = 10; - const uint SERVO_12 = 11; - const uint SERVO_13 = 12; - const uint SERVO_14 = 13; - const uint SERVO_15 = 14; - const uint SERVO_16 = 15; - const uint SERVO_17 = 16; - const uint SERVO_18 = 17; - const uint NUM_SERVOS = 18; + const uint MOTOR_1P = 4; + const uint MOTOR_1N = 5; + const uint MOTOR_2P = 6; + const uint MOTOR_2N = 7; + const uint MOTOR_3P = 8; + const uint MOTOR_3N = 9; + const uint MOTOR_4P = 10; + const uint MOTOR_4N = 11; + + const MotorPins MOTOR_1(MOTOR_1P, MOTOR_1N); + const MotorPins MOTOR_2(MOTOR_2P, MOTOR_2N); + const MotorPins MOTOR_3(MOTOR_3P, MOTOR_3N); + const MotorPins MOTOR_4(MOTOR_4P, MOTOR_4N); + const uint NUM_MOTORS = 4; + + const uint ENCODER_1A = 0; + const uint ENCODER_1B = 1; + const uint ENCODER_2A = 2; + const uint ENCODER_2B = 3; + const uint ENCODER_3A = 12; + const uint ENCODER_3B = 13; + const uint ENCODER_4A = 14; + const uint ENCODER_4B = 15; + + const EncoderPins ENCODER_1(ENCODER_1A, ENCODER_1B); + const EncoderPins ENCODER_2(ENCODER_2A, ENCODER_2B); + const EncoderPins ENCODER_3(ENCODER_3A, ENCODER_3B); + const EncoderPins ENCODER_4(ENCODER_4A, ENCODER_4B); + const uint NUM_ENCODERS = 4; + + const uint TX_TRIG = 16; + const uint RX_ECHO = 17; const uint LED_DATA = 18; const uint NUM_LEDS = 1; @@ -41,19 +54,19 @@ namespace motor { const uint ADC2 = 28; const uint SHARED_ADC = 29; // The pin used for the board's sensing features - const uint SENSOR_1_ADDR = 0b000; - const uint SENSOR_2_ADDR = 0b001; - const uint SENSOR_3_ADDR = 0b010; - const uint SENSOR_4_ADDR = 0b011; - const uint SENSOR_5_ADDR = 0b100; - const uint SENSOR_6_ADDR = 0b101; - const uint NUM_SENSORS = 6; + const uint CURRENT_SENSE_1_ADDR = 0b000; + const uint CURRENT_SENSE_2_ADDR = 0b001; + const uint CURRENT_SENSE_3_ADDR = 0b010; + const uint CURRENT_SENSE_4_ADDR = 0b011; + const uint VOLTAGE_SENSE_ADDR = 0b100; + const uint FAULT_SENSE_ADDR = 0b101; - const uint VOLTAGE_SENSE_ADDR = 0b110; - const uint CURRENT_SENSE_ADDR = 0b111; + const uint SENSOR_1_ADDR = 0b110; + const uint SENSOR_2_ADDR = 0b111; + const uint NUM_SENSORS = 2; - constexpr float SHUNT_RESISTOR = 0.003f; - constexpr float CURRENT_GAIN = 69; + constexpr float SHUNT_RESISTOR = 0.47f; + constexpr float CURRENT_GAIN = 5; constexpr float VOLTAGE_GAIN = 3.9f / 13.9f; constexpr float CURRENT_OFFSET = -0.02f; } diff --git a/libraries/pico_motor_shim/pico_motor_shim.cmake b/libraries/pico_motor_shim/pico_motor_shim.cmake index d28b6c2f..19b08935 100644 --- a/libraries/pico_motor_shim/pico_motor_shim.cmake +++ b/libraries/pico_motor_shim/pico_motor_shim.cmake @@ -3,4 +3,4 @@ add_library(pico_motor_shim INTERFACE) target_include_directories(pico_motor_shim INTERFACE ${CMAKE_CURRENT_LIST_DIR}) # Pull in pico libraries that we need -target_link_libraries(pico_motor_shim INTERFACE pico_stdlib) \ No newline at end of file +target_link_libraries(pico_motor_shim INTERFACE pico_stdlib motor) \ No newline at end of file diff --git a/libraries/pico_motor_shim/pico_motor_shim.hpp b/libraries/pico_motor_shim/pico_motor_shim.hpp index e93d6bf5..c0681138 100644 --- a/libraries/pico_motor_shim/pico_motor_shim.hpp +++ b/libraries/pico_motor_shim/pico_motor_shim.hpp @@ -1,12 +1,19 @@ #pragma once #include "pico/stdlib.h" -namespace pico_motor_shim { - const uint8_t BUTTON_A = 2; +#include "motor2.hpp" - const uint MOTOR_1_POS = 6; - const uint MOTOR_1_NEG = 7; +namespace motor { + namespace pico_motor_shim { + const uint BUTTON_A = 2; - const uint MOTOR_2_POS = 27; - const uint MOTOR_2_NEG = 26; + const uint MOTOR_1P = 6; + const uint MOTOR_1N = 7; + const uint MOTOR_2P = 27; + const uint MOTOR_2N = 26; + + const MotorPins MOTOR_1(MOTOR_1P, MOTOR_1N); + const MotorPins MOTOR_2(MOTOR_2P, MOTOR_2N); + const uint NUM_MOTORS = 2; + } } \ No newline at end of file