mirror
/
pimoroni-pico
kopia lustrzana https://github.com/pimoroni/pimoroni-pico
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Added in PH_EN motor driver, and zeropoint support

motor-and-encoder
ZodiusInfuser 2022-04-25 16:16:02 +01:00
rodzic 7608e3f293
commit 0efe210c4c
16 zmienionych plików z 528 dodań i 240 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

7
common/pimoroni_common.hpp
Wyświetl plik

@ -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) {}

2
drivers/encoder/encoder.cpp
Wyświetl plik

@ -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)) {

7
drivers/encoder/encoder.hpp
Wyświetl plik

@ -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();

426
drivers/motor/motor.cpp
Wyświetl plik

@ -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);
}
};

84
drivers/motor/motor.hpp
Wyświetl plik

@ -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);
};
}

48
drivers/motor/motor_cluster.cpp
Wyświetl plik

@ -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_pair> 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<uint8_t> 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;
}
}
}
};
}

20
drivers/motor/motor_cluster.hpp
Wyświetl plik

@ -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_pair> 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_pair> 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<uint8_t> 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<uint8_t> 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);
};
}

43
drivers/motor/motor_state.cpp
Wyświetl plik

@ -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;

17
drivers/motor/motor_state.hpp
Wyświetl plik

@ -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);

2
examples/pico_explorer_encoder/demo.cpp
Wyświetl plik

@ -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];

8
examples/pico_motor_shim/song/demo.cpp
Wyświetl plik

@ -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;

2
micropython/examples/motor2040/motor_profiler.py
Wyświetl plik

@ -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)

12
micropython/modules/encoder/encoder.cpp
Wyświetl plik

@ -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;

1
micropython/modules/motor/motor.c
Wyświetl plik

@ -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);

88
micropython/modules/motor/motor.cpp
Wyświetl plik

@ -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);
}

1
micropython/modules/motor/motor.h
Wyświetl plik

@ -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);