From 0efe210c4c9237d2dc5eb5d3d2ea50f69b4c800c Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Mon, 25 Apr 2022 16:16:02 +0100 Subject: [PATCH] Added in PH_EN motor driver, and zeropoint support --- common/pimoroni_common.hpp | 7 + drivers/encoder/encoder.cpp | 2 +- drivers/encoder/encoder.hpp | 7 +- drivers/motor/motor.cpp | 426 +++++++++++------- drivers/motor/motor.hpp | 84 +++- drivers/motor/motor_cluster.cpp | 48 +- drivers/motor/motor_cluster.hpp | 20 +- drivers/motor/motor_state.cpp | 43 +- drivers/motor/motor_state.hpp | 17 +- examples/pico_explorer_encoder/demo.cpp | 2 +- examples/pico_motor_shim/song/demo.cpp | 8 +- .../examples/motor2040/motor_profiler.py | 2 +- micropython/modules/encoder/encoder.cpp | 12 +- micropython/modules/motor/motor.c | 1 + micropython/modules/motor/motor.cpp | 88 +++- micropython/modules/motor/motor.h | 1 + 16 files changed, 528 insertions(+), 240 deletions(-) diff --git a/common/pimoroni_common.hpp b/common/pimoroni_common.hpp index b7b14aef..03b64f1f 100644 --- a/common/pimoroni_common.hpp +++ b/common/pimoroni_common.hpp @@ -59,6 +59,11 @@ namespace pimoroni { ACTIVE_HIGH = 1 }; + enum Direction { + NORMAL_DIR = 0, + REVERSED_DIR = 1, + }; + inline uint32_t millis() { return to_ms_since_boot(get_absolute_time()); } @@ -86,11 +91,13 @@ namespace pimoroni { uint8_t first; uint8_t a; uint8_t positive; + uint8_t phase; }; union { uint8_t second; uint8_t b; uint8_t negative; + uint8_t enable; }; pin_pair() : first(0), second(0) {} diff --git a/drivers/encoder/encoder.cpp b/drivers/encoder/encoder.cpp index 427589c2..7fca1472 100644 --- a/drivers/encoder/encoder.cpp +++ b/drivers/encoder/encoder.cpp @@ -330,7 +330,7 @@ void Encoder::process_steps() { microstep_time = time_received; } - bool up = (enc_direction == NORMAL); + bool up = (enc_direction == NORMAL_DIR); // Determine what step occurred switch(LAST_STATE(states)) { diff --git a/drivers/encoder/encoder.hpp b/drivers/encoder/encoder.hpp index 37f4a8dc..45441ef3 100644 --- a/drivers/encoder/encoder.hpp +++ b/drivers/encoder/encoder.hpp @@ -7,11 +7,6 @@ using namespace pimoroni; namespace encoder { - enum Direction { - NORMAL = 0, - REVERSED = 1, - }; - class Encoder { //-------------------------------------------------- // Constants @@ -145,7 +140,7 @@ namespace encoder { // Constructors/Destructor //-------------------------------------------------- public: - Encoder(PIO pio, uint sm, const pin_pair &pins, uint common_pin = PIN_UNUSED, Direction direction = NORMAL, + Encoder(PIO pio, uint sm, const pin_pair &pins, uint common_pin = PIN_UNUSED, Direction direction = NORMAL_DIR, float counts_per_rev = DEFAULT_COUNTS_PER_REV, bool count_microsteps = DEFAULT_COUNT_MICROSTEPS, uint16_t freq_divider = DEFAULT_FREQ_DIVIDER); ~Encoder(); diff --git a/drivers/motor/motor.cpp b/drivers/motor/motor.cpp index d08fcc29..fae17d5b 100644 --- a/drivers/motor/motor.cpp +++ b/drivers/motor/motor.cpp @@ -4,189 +4,64 @@ #include "math.h" namespace motor { - Motor::Motor(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_mode(mode) { + Motor::Driver::Driver(const pin_pair &pins) : motor_pins(pins), pwm_period(1) { } - Motor::~Motor() { - gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL); - gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL); + Motor::Driver::~Driver() { + gpio_set_function(motor_pins.first, GPIO_FUNC_NULL); + gpio_set_function(motor_pins.second, GPIO_FUNC_NULL); } - bool Motor::init() { - bool success = false; - - uint16_t period; uint16_t div16; - if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) { - pwm_period = period; - - pwm_cfg = pwm_get_default_config(); - - // Set the new wrap (should be 1 less than the period to get full 0 to 100%) - pwm_config_set_wrap(&pwm_cfg, pwm_period - 1); - - // 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_pins.positive), &pwm_cfg, true); - pwm_init(pwm_gpio_to_slice_num(motor_pins.negative), &pwm_cfg, true); - - gpio_set_function(motor_pins.positive, GPIO_FUNC_PWM); - gpio_set_function(motor_pins.negative, GPIO_FUNC_PWM); - - pwm_set_gpio_level(motor_pins.positive, 0); - pwm_set_gpio_level(motor_pins.negative, 0); - - success = true; - } - return success; - } - - pin_pair Motor::pins() const { + const pin_pair& Motor::Driver::pins() const { return motor_pins; } - void Motor::enable() { - apply_duty(state.enable_with_return(), motor_mode); + void Motor::DualPWMDriver::init(pwm_config *pwm_cfg, uint16_t period) { + pwm_period = period; + + // Set up the positive and negative pins + pwm_init(pwm_gpio_to_slice_num(motor_pins.positive), pwm_cfg, true); + pwm_init(pwm_gpio_to_slice_num(motor_pins.negative), pwm_cfg, true); + + gpio_set_function(motor_pins.positive, GPIO_FUNC_PWM); + gpio_set_function(motor_pins.negative, GPIO_FUNC_PWM); + + pwm_set_gpio_level(motor_pins.positive, 0); + pwm_set_gpio_level(motor_pins.negative, 0); } - void Motor::disable() { - apply_duty(state.disable_with_return(), motor_mode); - } + void Motor::DualPWMDriver::update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode) { + // Record if the new period will be larger or smaller. + // This is used to apply new pwm speeds either before or after the wrap is applied, + // to avoid momentary blips in PWM output on SLOW_DECAY + bool pre_update_pwm = (period > pwm_period); + pwm_period = period; - bool Motor::is_enabled() const { - return state.is_enabled(); - } + uint pos_pin_slice = pwm_gpio_to_slice_num(motor_pins.positive); + uint neg_pin_slice = pwm_gpio_to_slice_num(motor_pins.negative); - float Motor::duty() const { - return state.get_duty(); - } + // Apply the new divider + pwm_set_clkdiv_int_frac(pos_pin_slice, div, mod); + if((neg_pin_slice != pos_pin_slice)) + pwm_set_clkdiv_int_frac(neg_pin_slice, div, mod); - void Motor::duty(float duty) { - apply_duty(state.set_duty_with_return(duty), motor_mode); - } - - float Motor::speed() const { - return state.get_speed(); - } - - void Motor::speed(float speed) { - apply_duty(state.set_speed_with_return(speed), motor_mode); - } - - float Motor::frequency() const { - return pwm_frequency; - } - - bool Motor::frequency(float freq) { - bool success = false; - - if((freq >= MotorState::MIN_FREQUENCY) && (freq <= MotorState::MAX_FREQUENCY)) { - // Calculate a suitable pwm wrap period for this frequency - uint16_t period; uint16_t div16; - if(pimoroni::calculate_pwm_factors(freq, period, div16)) { - - // Record if the new period will be larger or smaller. - // This is used to apply new pwm speeds either before or after the wrap is applied, - // to avoid momentary blips in PWM output on SLOW_DECAY - bool pre_update_pwm = (period > pwm_period); - - pwm_period = period; - pwm_frequency = freq; - - 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(pos_pin_num, div, mod); - if(neg_pin_num != pos_pin_num) - pwm_set_clkdiv_int_frac(neg_pin_num, div, mod); - - // If the period is larger, update the pwm before setting the new wraps - if(pre_update_pwm) { - apply_duty(state.get_deadzoned_duty(), motor_mode); - } - - // Set the new wrap (should be 1 less than the period to get full 0 to 100%) - 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 period is smaller, update the pwm after setting the new wraps - if(!pre_update_pwm) { - apply_duty(state.get_deadzoned_duty(), motor_mode); - } - - success = true; - } + // If the period is larger, update the pwm before setting the new wraps + if(pre_update_pwm) { + apply_duty(duty, mode); + } + + // Set the new wrap (should be 1 less than the period to get full 0 to 100%) + pwm_set_wrap(pos_pin_slice, pwm_period - 1); + if(neg_pin_slice != pos_pin_slice) + pwm_set_wrap(neg_pin_slice, pwm_period - 1); + + // If the period is smaller, update the pwm after setting the new wraps + if(!pre_update_pwm) { + apply_duty(duty, mode); } - return success; } - void Motor::stop() { - apply_duty(state.stop_with_return(), motor_mode); - } - - void Motor::coast() { - apply_duty(state.stop_with_return(), FAST_DECAY); - } - - void Motor::brake() { - apply_duty(state.stop_with_return(), SLOW_DECAY); - } - - void Motor::full_negative() { - apply_duty(state.full_negative_with_return(), motor_mode); - } - - void Motor::full_positive() { - apply_duty(state.full_positive_with_return(), motor_mode); - } - - void Motor::to_percent(float in, float in_min, float in_max) { - apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_mode); - } - - void Motor::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), motor_mode); - } - - Direction Motor::direction() const { - return state.get_direction(); - } - - void Motor::direction(Direction direction) { - state.set_direction(direction); - } - - float Motor::speed_scale() const { - return state.get_speed_scale(); - } - - void Motor::speed_scale(float speed_scale) { - state.set_speed_scale(speed_scale); - } - - float Motor::deadzone() const { - return state.get_deadzone(); - } - - void Motor::deadzone(float deadzone) { - apply_duty(state.set_deadzone_with_return(deadzone), motor_mode); - } - - DecayMode Motor::decay_mode() { - return motor_mode; - } - - void Motor::decay_mode(DecayMode mode) { - motor_mode = mode; - apply_duty(state.get_deadzoned_duty(), motor_mode); - } - - void Motor::apply_duty(float duty, DecayMode mode) { + void Motor::DualPWMDriver::apply_duty(float duty, DecayMode mode) { if(isfinite(duty)) { int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); @@ -220,4 +95,219 @@ namespace motor { pwm_set_gpio_level(motor_pins.negative, 0); } } + + void Motor::PhEnDriver::init(pwm_config *pwm_cfg, uint16_t period) { + pwm_period = period; + + // Set up the phase and enable pins + gpio_init(motor_pins.phase); + pwm_init(pwm_gpio_to_slice_num(motor_pins.enable), pwm_cfg, true); + + gpio_set_dir(motor_pins.phase, true); + gpio_set_function(motor_pins.enable, GPIO_FUNC_PWM); + + gpio_put(motor_pins.phase, false); + pwm_set_gpio_level(motor_pins.enable, 0); + } + + + void Motor::PhEnDriver::update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode) { + // Record if the new period will be larger or smaller. + // This is used to apply new pwm speeds either before or after the wrap is applied, + // to avoid momentary blips in PWM output on SLOW_DECAY + bool pre_update_pwm = (period > pwm_period); + pwm_period = period; + + uint en_pin_slice = pwm_gpio_to_slice_num(motor_pins.enable); + + // Apply the new divider + pwm_set_clkdiv_int_frac(en_pin_slice, div, mod); + + // If the period is larger, update the pwm before setting the new wraps + if(pre_update_pwm) { + apply_duty(duty, mode); + } + + // Set the new wrap (should be 1 less than the period to get full 0 to 100%) + pwm_set_wrap(en_pin_slice, pwm_period - 1); + + // If the period is smaller, update the pwm after setting the new wraps + if(!pre_update_pwm) { + apply_duty(duty, mode); + } + } + + void Motor::PhEnDriver::apply_duty(float duty, DecayMode mode) { + if(isfinite(duty)) { + int32_t signed_level = MotorState::duty_to_level(duty, pwm_period); + + if(signed_level >= 0) { + gpio_put(motor_pins.phase, true); + pwm_set_gpio_level(motor_pins.enable, signed_level); + } + else { + gpio_put(motor_pins.phase, false); + pwm_set_gpio_level(motor_pins.enable, 0 - signed_level); + } + } + else { + gpio_put(motor_pins.phase, false); + pwm_set_gpio_level(motor_pins.enable, 0); + } + } + + Motor::Motor(const pin_pair &pins, Direction direction, float speed_scale, float zeropoint, float deadzone, float freq, DecayMode mode, bool ph_en_driver) + : driver(ph_en_driver ? (Driver*)new PhEnDriver(pins) : (Driver*)new DualPWMDriver(pins)) + , state(direction, speed_scale, zeropoint, deadzone), pwm_frequency(freq), motor_mode(mode) { + } + + Motor::~Motor() { + delete driver; + } + + bool Motor::init() { + bool success = false; + + uint16_t period; uint16_t div16; + if(pimoroni::calculate_pwm_factors(pwm_frequency, period, div16)) { + pwm_cfg = pwm_get_default_config(); + + // Set the new wrap (should be 1 less than the period to get full 0 to 100%) + pwm_config_set_wrap(&pwm_cfg, period - 1); + + // 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... + + // Set up the pin driver + driver->init(&pwm_cfg, period); + + success = true; + } + return success; + } + + pin_pair Motor::pins() const { + return driver->pins(); + } + + void Motor::enable() { + driver->apply_duty(state.enable_with_return(), motor_mode); + } + + void Motor::disable() { + driver->apply_duty(state.disable_with_return(), motor_mode); + } + + bool Motor::is_enabled() const { + return state.is_enabled(); + } + + float Motor::duty() const { + return state.get_duty(); + } + + void Motor::duty(float duty) { + driver->apply_duty(state.set_duty_with_return(duty), motor_mode); + } + + float Motor::speed() const { + return state.get_speed(); + } + + void Motor::speed(float speed) { + driver->apply_duty(state.set_speed_with_return(speed), motor_mode); + } + + float Motor::frequency() const { + return pwm_frequency; + } + + bool Motor::frequency(float freq) { + bool success = false; + + if((freq >= MotorState::MIN_FREQUENCY) && (freq <= MotorState::MAX_FREQUENCY)) { + // Calculate a suitable pwm wrap period for this frequency + uint16_t period; uint16_t div16; + if(pimoroni::calculate_pwm_factors(freq, period, div16)) { + + pwm_frequency = freq; + + uint8_t div = div16 >> 4; + uint8_t mod = div16 % 16; + driver->update_frequency(div, mod, period, state.get_deadzoned_duty(), motor_mode); + + success = true; + } + } + return success; + } + + void Motor::stop() { + driver->apply_duty(state.stop_with_return(), motor_mode); + } + + void Motor::coast() { + driver->apply_duty(state.stop_with_return(), FAST_DECAY); + } + + void Motor::brake() { + driver->apply_duty(state.stop_with_return(), SLOW_DECAY); + } + + void Motor::full_negative() { + driver->apply_duty(state.full_negative_with_return(), motor_mode); + } + + void Motor::full_positive() { + driver->apply_duty(state.full_positive_with_return(), motor_mode); + } + + void Motor::to_percent(float in, float in_min, float in_max) { + driver->apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_mode); + } + + void Motor::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) { + driver->apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max), motor_mode); + } + + Direction Motor::direction() const { + return state.get_direction(); + } + + void Motor::direction(Direction direction) { + state.set_direction(direction); + } + + float Motor::speed_scale() const { + return state.get_speed_scale(); + } + + void Motor::speed_scale(float speed_scale) { + state.set_speed_scale(speed_scale); + } + + float Motor::zeropoint() const { + return state.get_zeropoint(); + } + + void Motor::zeropoint(float zeropoint) { + state.set_zeropoint(zeropoint); + } + + float Motor::deadzone() const { + return state.get_deadzone(); + } + + void Motor::deadzone(float deadzone) { + driver->apply_duty(state.set_deadzone_with_return(deadzone), motor_mode); + } + + DecayMode Motor::decay_mode() { + return motor_mode; + } + + void Motor::decay_mode(DecayMode mode) { + motor_mode = mode; + driver->apply_duty(state.get_deadzoned_duty(), motor_mode); + } }; \ No newline at end of file diff --git a/drivers/motor/motor.hpp b/drivers/motor/motor.hpp index 45638097..d75f337f 100644 --- a/drivers/motor/motor.hpp +++ b/drivers/motor/motor.hpp @@ -10,23 +10,92 @@ using namespace pimoroni; namespace motor { class Motor { + //-------------------------------------------------- + // Subclasses + //-------------------------------------------------- + private: + class Driver { + //-------------------------------------------------- + // Variables + //-------------------------------------------------- + protected: + pin_pair motor_pins; + uint16_t pwm_period; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + Driver(const pin_pair &pins); + virtual ~Driver(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + const pin_pair& pins() const; + virtual void init(pwm_config *pwm_cfg, uint16_t period) = 0; + virtual void update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode) = 0; + virtual void apply_duty(float duty, DecayMode mode) = 0; + }; + + class DualPWMDriver : public Driver { + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + DualPWMDriver(pin_pair pins) : Driver(pins) {} + virtual ~DualPWMDriver() {} + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual void init(pwm_config *pwm_cfg, uint16_t period); + virtual void update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode); + virtual void apply_duty(float duty, DecayMode mode); + }; + + class PhEnDriver : public Driver { + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + PhEnDriver(pin_pair pins) : Driver(pins) {} + virtual ~PhEnDriver() {} + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + virtual void init(pwm_config *pwm_cfg, uint16_t period); + virtual void update_frequency(uint8_t div, uint8_t mod, uint16_t period, float duty, DecayMode mode); + virtual void apply_duty(float duty, DecayMode mode); + }; + + //-------------------------------------------------- // Variables //-------------------------------------------------- private: - pin_pair motor_pins; + Driver *driver; MotorState state; pwm_config pwm_cfg; - uint16_t pwm_period; float pwm_frequency; DecayMode motor_mode; + //-------------------------------------------------- // Constructors/Destructor //-------------------------------------------------- public: - Motor(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); + Motor(const pin_pair &pins, Direction direction = NORMAL_DIR, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, float zeropoint = MotorState::DEFAULT_ZEROPOINT, + float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE, + bool ph_en_driver = false); ~Motor(); @@ -70,15 +139,14 @@ namespace motor { float speed_scale() const; void speed_scale(float speed_scale); + float zeropoint() const; + void zeropoint(float zeropoint); + float deadzone() const; void deadzone(float deadzone); DecayMode decay_mode(); void decay_mode(DecayMode mode); - - //-------------------------------------------------- - private: - 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 4b77f48c..736d16d7 100644 --- a/drivers/motor/motor_cluster.cpp +++ b/drivers/motor/motor_cluster.cpp @@ -8,24 +8,24 @@ namespace motor { MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction, - float speed_scale, float deadzone, float freq, DecayMode mode, + float speed_scale, float zeropoint, 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, mode, auto_phase); + create_motor_states(direction, speed_scale, zeropoint, deadzone, mode, auto_phase); } 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, + float speed_scale, float zeropoint, 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, mode, auto_phase); + create_motor_states(direction, speed_scale, zeropoint, deadzone, mode, auto_phase); } MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Direction direction, - float speed_scale, float deadzone, float freq, DecayMode mode, + float speed_scale, float zeropoint, 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, mode, auto_phase); + create_motor_states(direction, speed_scale, zeropoint, deadzone, mode, auto_phase); } MotorCluster::~MotorCluster() { @@ -581,6 +581,36 @@ namespace motor { } } + float MotorCluster::zeropoint(uint8_t motor) const { + assert(motor < pwms.get_chan_pair_count()); + return states[motor].get_zeropoint(); + } + + void MotorCluster::zeropoint(uint8_t motor, float zeropoint, bool load) { + assert(motor < pwms.get_chan_pair_count()); + states[motor].set_zeropoint(zeropoint); + } + + void MotorCluster::zeropoint(const uint8_t *motors, uint8_t length, float zeropoint, bool load) { + assert(motors != nullptr); + for(uint8_t i = 0; i < length; i++) { + this->zeropoint(motors[i], zeropoint); + } + } + + void MotorCluster::zeropoint(std::initializer_list motors, float zeropoint, bool load) { + for(auto motor : motors) { + this->zeropoint(motor, zeropoint); + } + } + + void MotorCluster::all_to_zeropoint(float zeropoint, bool load) { + uint8_t motor_count = pwms.get_chan_pair_count(); + for(uint8_t motor = 0; motor < motor_count; motor++) { + this->zeropoint(motor, zeropoint); + } + } + float MotorCluster::deadzone(uint8_t motor) const { assert(motor < pwms.get_chan_pair_count()); return states[motor].get_deadzone(); @@ -679,7 +709,7 @@ namespace motor { } } - void MotorCluster::create_motor_states(Direction direction, float speed_scale, + void MotorCluster::create_motor_states(Direction direction, float speed_scale, float zeropoint, float deadzone, DecayMode mode, bool auto_phase) { uint8_t motor_count = pwms.get_chan_pair_count(); if(motor_count > 0) { @@ -687,10 +717,10 @@ namespace motor { configs = new motor_config[motor_count]; for(uint motor = 0; motor < motor_count; motor++) { - states[motor] = MotorState(direction, speed_scale, deadzone); + states[motor] = MotorState(direction, speed_scale, zeropoint, deadzone); configs[motor].phase = (auto_phase) ? (float)motor / (float)motor_count : 0.0f; configs[motor].mode = mode; } } } -}; \ No newline at end of file +} diff --git a/drivers/motor/motor_cluster.hpp b/drivers/motor/motor_cluster.hpp index d2468b21..785d9c15 100644 --- a/drivers/motor/motor_cluster.hpp +++ b/drivers/motor/motor_cluster.hpp @@ -34,14 +34,14 @@ namespace motor { // Constructors/Destructor //-------------------------------------------------- public: - 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, + MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction = NORMAL_DIR, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float zeropoint = MotorState::DEFAULT_ZEROPOINT, 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, 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, + MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction = NORMAL_DIR, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float zeropoint = MotorState::DEFAULT_ZEROPOINT, 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, 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, + MotorCluster(PIO pio, uint sm, std::initializer_list pin_pairs, Direction direction = NORMAL_DIR, float speed_scale = MotorState::DEFAULT_SPEED_SCALE, + float zeropoint = MotorState::DEFAULT_ZEROPOINT, 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(); @@ -140,6 +140,12 @@ namespace motor { void speed_scale(std::initializer_list motors, float speed_scale); void all_to_speed_scale(float speed_scale); + float zeropoint(uint8_t motor) const; + void zeropoint(uint8_t motor, float zeropoint, bool load = true); + void zeropoint(const uint8_t *motors, uint8_t length, float zeropoint, bool load = true); + void zeropoint(std::initializer_list motors, float zeropoint, bool load = true); + void all_to_zeropoint(float zeropoint, bool load = true); + float deadzone(uint8_t motor) const; void deadzone(uint8_t motor, float deadzone, bool load = true); void deadzone(const uint8_t *motors, uint8_t length, float deadzone, bool load = true); @@ -155,7 +161,7 @@ namespace motor { //-------------------------------------------------- private: 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); + void create_motor_states(Direction direction, float speed_scale, float zeropoint, float deadzone, DecayMode mode, bool auto_phase); }; } \ No newline at end of file diff --git a/drivers/motor/motor_state.cpp b/drivers/motor/motor_state.cpp index ce66db91..4eeca27e 100644 --- a/drivers/motor/motor_state.cpp +++ b/drivers/motor/motor_state.cpp @@ -6,12 +6,12 @@ namespace motor { MotorState::MotorState() : motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false), - motor_direction(NORMAL), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE){ + motor_direction(NORMAL_DIR), motor_scale(DEFAULT_SPEED_SCALE), motor_zeropoint(DEFAULT_ZEROPOINT), motor_deadzone(DEFAULT_DEADZONE){ } - MotorState::MotorState(Direction direction, float speed_scale, float deadzone) + MotorState::MotorState(Direction direction, float speed_scale, float zeropoint, 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, 0.0f, 1.0f)) { + motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_zeropoint(CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON)), motor_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) { } float MotorState::enable_with_return() { @@ -47,23 +47,38 @@ namespace motor { float MotorState::set_duty_with_return(float duty) { // Clamp the duty between the hard limits last_enabled_duty = CLAMP(duty, -1.0f, 1.0f); - motor_speed = last_enabled_duty * motor_scale; + + if(last_enabled_duty > motor_zeropoint) + motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale); + else if(last_enabled_duty < -motor_zeropoint) + motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale); + else + motor_speed = 0.0f; + + //motor_speed = last_enabled_duty * motor_scale; return enable_with_return(); } float MotorState::get_speed() const { - return (motor_direction == NORMAL) ? motor_speed : 0.0f - motor_speed; + return (motor_direction == NORMAL_DIR) ? 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) + if(motor_direction == REVERSED_DIR) speed = 0.0f - speed; // Clamp the speed between the hard limits motor_speed = CLAMP(speed, 0.0f - motor_scale, motor_scale); - last_enabled_duty = motor_speed / motor_scale; + + if(motor_speed > 0.0f) + last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f); + else if(motor_speed < 0.0f) + last_enabled_duty = map_float(motor_speed / motor_scale, 0.0f, 1.0f, motor_zeropoint, 1.0f); + else + last_enabled_duty = 0.0f; + //last_enabled_duty = motor_speed / motor_scale; return enable_with_return(); } @@ -107,6 +122,20 @@ namespace motor { motor_speed = last_enabled_duty * motor_scale; } + float MotorState::get_zeropoint() const { + return motor_scale; + } + + void MotorState::set_zeropoint(float zeropoint) { + motor_zeropoint = CLAMP(zeropoint, 0.0f, 1.0f - FLT_EPSILON); + + if(last_enabled_duty > motor_zeropoint) + motor_speed = map_float(last_enabled_duty, motor_zeropoint, 1.0f, 0.0f, motor_scale); + else if(last_enabled_duty < -motor_zeropoint) + motor_speed = map_float(last_enabled_duty, -motor_zeropoint, -1.0f, 0.0f, -motor_scale); + else + motor_speed = 0.0f; + } float MotorState::get_deadzone() const { return motor_deadzone; diff --git a/drivers/motor/motor_state.hpp b/drivers/motor/motor_state.hpp index 990ecbd3..1a7f9cba 100644 --- a/drivers/motor/motor_state.hpp +++ b/drivers/motor/motor_state.hpp @@ -1,14 +1,12 @@ #pragma once #include "pico/stdlib.h" +#include "common/pimoroni_common.hpp" + +using namespace pimoroni; namespace motor { - enum Direction { - NORMAL = 0, - REVERSED = 1, - }; - enum DecayMode { FAST_DECAY = 0, //aka 'Coasting' SLOW_DECAY = 1, //aka 'Braking' @@ -20,7 +18,8 @@ namespace motor { //-------------------------------------------------- public: 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 constexpr float DEFAULT_ZEROPOINT = 0.0f; // The standard motor deadzone + static constexpr float DEFAULT_DEADZONE = 0.05f; // 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 @@ -42,6 +41,7 @@ namespace motor { // Customisation variables Direction motor_direction; float motor_scale; + float motor_zeropoint; float motor_deadzone; @@ -50,7 +50,7 @@ namespace motor { //-------------------------------------------------- public: MotorState(); - MotorState(Direction direction, float speed_scale, float deadzone); + MotorState(Direction direction, float speed_scale, float zeropoint, float deadzone); //-------------------------------------------------- @@ -84,6 +84,9 @@ namespace motor { float get_speed_scale() const; void set_speed_scale(float speed_scale); + float get_zeropoint() const; + void set_zeropoint(float zeropoint); + float get_deadzone() const; float set_deadzone_with_return(float deadzone); diff --git a/examples/pico_explorer_encoder/demo.cpp b/examples/pico_explorer_encoder/demo.cpp index 4ebfb7ae..0f4e9ce7 100644 --- a/examples/pico_explorer_encoder/demo.cpp +++ b/examples/pico_explorer_encoder/demo.cpp @@ -55,7 +55,7 @@ enum DrawState { uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT]; PicoExplorer pico_explorer(buffer); -Encoder enc(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, NORMAL, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); +Encoder enc(pio0, 0, ENCODER_PINS, ENCODER_PIN_C, NORMAL_DIR, COUNTS_PER_REVOLUTION, COUNT_MICROSTEPS, FREQ_DIVIDER); volatile bool encA_readings[READINGS_SIZE]; volatile bool encB_readings[READINGS_SIZE]; diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/song/demo.cpp index 8f6df835..bd2f2ad5 100644 --- a/examples/pico_motor_shim/song/demo.cpp +++ b/examples/pico_motor_shim/song/demo.cpp @@ -40,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, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); #else - Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); + Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); #endif static bool button_toggle = false; diff --git a/micropython/examples/motor2040/motor_profiler.py b/micropython/examples/motor2040/motor_profiler.py index 0c9532f8..30d2896e 100644 --- a/micropython/examples/motor2040/motor_profiler.py +++ b/micropython/examples/motor2040/motor_profiler.py @@ -25,7 +25,7 @@ CAPTURE_TIME = 0.2 # How long to capture the motor's speed gc.collect() # Create a motor and set its speed scale, and give it a zero deadzone -m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, deadzone=0.0) +m = Motor(MOTOR_PINS, direction=DIRECTION, speed_scale=SPEED_SCALE, zeropoint=0.0, deadzone=0.0) # Create an encoder, using PIO 0 and State Machine 0 enc = Encoder(0, 0, ENCODER_PINS, direction=DIRECTION, counts_per_rev=COUNTS_PER_REV, count_microsteps=True) diff --git a/micropython/modules/encoder/encoder.cpp b/micropython/modules/encoder/encoder.cpp index c080316f..4768f2c3 100644 --- a/micropython/modules/encoder/encoder.cpp +++ b/micropython/modules/encoder/encoder.cpp @@ -40,10 +40,10 @@ void Encoder_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t ki mp_obj_print_helper(print, mp_obj_new_int(self->encoder->common_pin()), PRINT_REPR); mp_print_str(print, ")"); - if(self->encoder->direction() == NORMAL) - mp_print_str(print, ", direction = NORMAL"); + if(self->encoder->direction() == NORMAL_DIR) + mp_print_str(print, ", direction = NORMAL_DIR"); else - mp_print_str(print, ", direction = REVERSED"); + mp_print_str(print, ", direction = REVERSED_DIR"); mp_print_str(print, ", counts_per_rev = "); mp_obj_print_helper(print, mp_obj_new_float(self->encoder->counts_per_rev()), PRINT_REPR); @@ -61,7 +61,7 @@ mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, { MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, { MP_QSTR_common_pin, MP_ARG_INT, {.u_int = PIN_UNUSED} }, - { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL} }, + { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL_DIR} }, { MP_QSTR_counts_per_rev, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_count_microsteps, MP_ARG_BOOL, {.u_bool = false} }, { MP_QSTR_freq_divider, MP_ARG_OBJ, {.u_obj = mp_const_none} }, @@ -120,7 +120,7 @@ mp_obj_t Encoder_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED_DIR (1)"); } float counts_per_rev = Encoder::DEFAULT_COUNTS_PER_REV; @@ -257,7 +257,7 @@ extern mp_obj_t Encoder_direction(size_t n_args, const mp_obj_t *pos_args, mp_ma int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED_DIR (1)"); } self->encoder->direction((Direction)direction); return mp_const_none; diff --git a/micropython/modules/motor/motor.c b/micropython/modules/motor/motor.c index a79ddb11..715984bd 100644 --- a/micropython/modules/motor/motor.c +++ b/micropython/modules/motor/motor.c @@ -17,6 +17,7 @@ MP_DEFINE_CONST_FUN_OBJ_1(Motor_full_positive_obj, Motor_full_positive); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_to_percent_obj, 2, Motor_to_percent); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_direction_obj, 1, Motor_direction); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_speed_scale_obj, 1, Motor_speed_scale); +MP_DEFINE_CONST_FUN_OBJ_KW(Motor_zeropoint_obj, 1, Motor_zeropoint); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_deadzone_obj, 1, Motor_deadzone); MP_DEFINE_CONST_FUN_OBJ_KW(Motor_decay_mode_obj, 1, Motor_decay_mode); diff --git a/micropython/modules/motor/motor.cpp b/micropython/modules/motor/motor.cpp index 1bbc2cf5..8a7f5f6f 100644 --- a/micropython/modules/motor/motor.cpp +++ b/micropython/modules/motor/motor.cpp @@ -42,12 +42,14 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed()), PRINT_REPR); mp_print_str(print, ", freq = "); mp_obj_print_helper(print, mp_obj_new_float(self->motor->frequency()), PRINT_REPR); - if(self->motor->direction() == NORMAL) - mp_print_str(print, ", direction = NORMAL"); + if(self->motor->direction() == NORMAL_DIR) + mp_print_str(print, ", direction = NORMAL_DIR"); else - mp_print_str(print, ", direction = REVERSED"); + mp_print_str(print, ", direction = REVERSED_DIR"); 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, ", zeropoint = "); + mp_obj_print_helper(print, mp_obj_new_float(self->motor->zeropoint()), 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() == SLOW_DECAY) @@ -63,14 +65,16 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _Motor_obj_t *self = nullptr; - enum { ARG_pins, ARG_direction, ARG_speed_scale, ARG_deadzone, ARG_freq, ARG_mode }; + enum { ARG_pins, ARG_direction, ARG_speed_scale, ARG_zeropoint, ARG_deadzone, ARG_freq, ARG_mode, ARG_ph_en_driver }; static const mp_arg_t allowed_args[] = { { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL} }, + { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL_DIR} }, { MP_QSTR_speed_scale, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_zeropoint, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_deadzone, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_mode, MP_ARG_INT, {.u_int = MotorState::DEFAULT_DECAY_MODE} }, + { MP_QSTR_ph_en_driver, MP_ARG_BOOL, {.u_bool = false} } }; // Parse args. @@ -115,7 +119,7 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } float speed_scale = MotorState::DEFAULT_SPEED_SCALE; @@ -126,6 +130,14 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c } } + float zeropoint = MotorState::DEFAULT_ZEROPOINT; + if(args[ARG_zeropoint].u_obj != mp_const_none) { + zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + } + float deadzone = MotorState::DEFAULT_DEADZONE; if(args[ARG_deadzone].u_obj != mp_const_none) { deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); @@ -147,7 +159,7 @@ mp_obj_t Motor_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, c self = m_new_obj_with_finaliser(_Motor_obj_t); self->base.type = &Motor_type; - self->motor = new Motor(pins, (Direction)direction, speed_scale, deadzone, freq, (DecayMode)mode); + self->motor = new Motor(pins, (Direction)direction, speed_scale, zeropoint, deadzone, freq, (DecayMode)mode, args[ARG_ph_en_driver].u_bool); self->motor->init(); return MP_OBJ_FROM_PTR(self); @@ -426,7 +438,7 @@ extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_ int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } self->motor->direction((Direction)direction); return mp_const_none; @@ -470,6 +482,43 @@ extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_ma } } +extern mp_obj_t Motor_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { + if(n_args <= 1) { + enum { ARG_self }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + return mp_obj_new_float(self->motor->zeropoint()); + } + else { + enum { ARG_self, ARG_zeropoint }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_zeropoint, MP_ARG_REQUIRED | MP_ARG_OBJ }, + }; + + // Parse args. + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + + _Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t); + + float zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + self->motor->zeropoint(zeropoint); + return mp_const_none; + } +} + extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { if(n_args <= 1) { enum { ARG_self }; @@ -597,13 +646,14 @@ void MotorCluster_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { _MotorCluster_obj_t *self = nullptr; - enum { ARG_pio, ARG_sm, ARG_pins, ARG_direction, ARG_speed_scale, ARG_deadzone, ARG_freq, ARG_mode, ARG_auto_phase }; + enum { ARG_pio, ARG_sm, ARG_pins, ARG_direction, ARG_speed_scale, ARG_zeropoint, ARG_deadzone, ARG_freq, ARG_mode, ARG_auto_phase }; static const mp_arg_t allowed_args[] = { { MP_QSTR_pio, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_sm, MP_ARG_REQUIRED | MP_ARG_INT }, { MP_QSTR_pins, MP_ARG_REQUIRED | MP_ARG_OBJ }, - { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL} }, + { MP_QSTR_direction, MP_ARG_INT, {.u_int = NORMAL_DIR} }, { MP_QSTR_speed_scale, MP_ARG_OBJ, {.u_obj = mp_const_none} }, + { MP_QSTR_zeropoint, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_deadzone, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_freq, MP_ARG_OBJ, {.u_obj = mp_const_none} }, { MP_QSTR_mode, MP_ARG_INT, {.u_int = MotorState::DEFAULT_DECAY_MODE} }, @@ -703,7 +753,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } float speed_scale = MotorState::DEFAULT_SPEED_SCALE; @@ -714,6 +764,14 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t } } + float zeropoint = MotorState::DEFAULT_ZEROPOINT; + if(args[ARG_zeropoint].u_obj != mp_const_none) { + zeropoint = mp_obj_get_float(args[ARG_zeropoint].u_obj); + if(zeropoint < 0.0f || zeropoint > 1.0f - FLT_EPSILON) { + mp_raise_ValueError("zeropoint out of range. Expected 0.0 to less than 1.0"); + } + } + float deadzone = MotorState::DEFAULT_DEADZONE; if(args[ARG_deadzone].u_obj != mp_const_none) { deadzone = mp_obj_get_float(args[ARG_deadzone].u_obj); @@ -737,7 +795,7 @@ mp_obj_t MotorCluster_make_new(const mp_obj_type_t *type, size_t n_args, size_t MotorCluster *cluster; PWMCluster::Sequence *seq_buffer = m_new(PWMCluster::Sequence, PWMCluster::NUM_BUFFERS * 2); PWMCluster::TransitionData *dat_buffer = m_new(PWMCluster::TransitionData, PWMCluster::BUFFER_SIZE * 2); - cluster = new MotorCluster(pio, sm, pins, pair_count, (Direction)direction, speed_scale, deadzone, + cluster = new MotorCluster(pio, sm, pins, pair_count, (Direction)direction, speed_scale, zeropoint, deadzone, freq, (DecayMode)mode, auto_phase, seq_buffer, dat_buffer); // Cleanup the pins array @@ -2194,7 +2252,7 @@ extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, else { int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } self->cluster->direction((uint)motor, (Direction)direction); } @@ -2233,7 +2291,7 @@ extern mp_obj_t MotorCluster_direction(size_t n_args, const mp_obj_t *pos_args, int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { delete[] motors; - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } self->cluster->direction(motors, length, (Direction)direction); delete[] motors; @@ -2263,7 +2321,7 @@ extern mp_obj_t MotorCluster_all_to_direction(size_t n_args, const mp_obj_t *pos else { int direction = args[ARG_direction].u_int; if(direction < 0 || direction > 1) { - mp_raise_ValueError("direction out of range. Expected NORMAL (0) or REVERSED (1)"); + mp_raise_ValueError("direction out of range. Expected NORMAL_DIR (0) or REVERSED_DIR (1)"); } self->cluster->all_to_direction((Direction)direction); } diff --git a/micropython/modules/motor/motor.h b/micropython/modules/motor/motor.h index 1dff6978..7ce746a4 100644 --- a/micropython/modules/motor/motor.h +++ b/micropython/modules/motor/motor.h @@ -24,6 +24,7 @@ extern mp_obj_t Motor_full_positive(mp_obj_t self_in); extern mp_obj_t Motor_to_percent(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_direction(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_speed_scale(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); +extern mp_obj_t Motor_zeropoint(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_deadzone(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args); extern mp_obj_t Motor_decay_mode(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);