Improvements to MotorState
This commit is contained in:
ZodiusInfuser
parent
7e9860e780
commit
f3c0a305f2
|
|
@ -1,11 +1,11 @@
|
|||
#include "motor2.hpp"
|
||||
#include "hardware/clocks.h"
|
||||
#include "pwm.hpp"
|
||||
#include "math.h"
|
||||
|
||||
namespace motor {
|
||||
Motor2::Motor2(const pin_pair &pins, MotorState::Direction direction, float speed_scale,
|
||||
float deadzone_percent, float freq, MotorState::DecayMode mode)
|
||||
: motor_pins(pins), state(direction, speed_scale, deadzone_percent), pwm_frequency(freq), motor_decay_mode(mode) {
|
||||
Motor2::Motor2(const pin_pair &pins, Direction direction, float speed_scale, float deadzone, float freq, DecayMode mode)
|
||||
: motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_decay_mode(mode) {
|
||||
}
|
||||
|
||||
Motor2::~Motor2() {
|
||||
|
|
@ -47,11 +47,11 @@ namespace motor {
|
|||
}
|
||||
|
||||
void Motor2::enable() {
|
||||
apply_duty(state.enable_with_return());
|
||||
apply_duty(state.enable_with_return(), motor_decay_mode);
|
||||
}
|
||||
|
||||
void Motor2::disable() {
|
||||
apply_duty(state.disable_with_return());
|
||||
apply_duty(state.disable_with_return(), motor_decay_mode);
|
||||
}
|
||||
|
||||
bool Motor2::is_enabled() const {
|
||||
|
|
@ -63,7 +63,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void Motor2::duty(float duty) {
|
||||
apply_duty(state.set_duty_with_return(duty));
|
||||
apply_duty(state.set_duty_with_return(duty), motor_decay_mode);
|
||||
}
|
||||
|
||||
float Motor2::speed() const {
|
||||
|
|
@ -71,7 +71,7 @@ namespace motor {
|
|||
}
|
||||
|
||||
void Motor2::speed(float speed) {
|
||||
apply_duty(state.set_speed_with_return(speed));
|
||||
apply_duty(state.set_speed_with_return(speed), motor_decay_mode);
|
||||
}
|
||||
|
||||
float Motor2::frequency() const {
|
||||
|
|
@ -106,7 +106,7 @@ namespace motor {
|
|||
|
||||
// If the the period is larger, update the pwm before setting the new wraps
|
||||
if(state.is_enabled() && pre_update_pwm) {
|
||||
apply_duty(state.get_duty());
|
||||
apply_duty(state.get_deadzoned_duty(), motor_decay_mode);
|
||||
}
|
||||
|
||||
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
|
||||
|
|
@ -116,7 +116,7 @@ namespace motor {
|
|||
|
||||
// If the the period is smaller, update the pwm after setting the new wraps
|
||||
if(state.is_enabled() && !pre_update_pwm) {
|
||||
apply_duty(state.get_duty());
|
||||
apply_duty(state.get_deadzoned_duty(), motor_decay_mode);
|
||||
}
|
||||
|
||||
success = true;
|
||||
|
|
@ -125,36 +125,50 @@ namespace motor {
|
|||
return success;
|
||||
}
|
||||
|
||||
DecayMode Motor2::decay_mode() {
|
||||
return motor_decay_mode;
|
||||
}
|
||||
|
||||
void Motor2::decay_mode(DecayMode mode) {
|
||||
motor_decay_mode = mode;
|
||||
if(state.is_enabled()) {
|
||||
apply_duty(state.get_deadzoned_duty(), motor_decay_mode);
|
||||
}
|
||||
}
|
||||
|
||||
void Motor2::stop() {
|
||||
apply_duty(state.stop_with_return());
|
||||
apply_duty(state.stop_with_return(), motor_decay_mode);
|
||||
}
|
||||
|
||||
void Motor2::coast() {
|
||||
state.set_duty_with_return(0.0f);
|
||||
disable();
|
||||
apply_duty(state.stop_with_return(), FAST_DECAY);
|
||||
}
|
||||
|
||||
void Motor2::brake() {
|
||||
apply_duty(state.stop_with_return(), SLOW_DECAY);
|
||||
}
|
||||
|
||||
void Motor2::full_negative() {
|
||||
apply_duty(state.full_negative_with_return());
|
||||
apply_duty(state.full_negative_with_return(), motor_decay_mode);
|
||||
}
|
||||
|
||||
void Motor2::full_positive() {
|
||||
apply_duty(state.full_positive_with_return());
|
||||
apply_duty(state.full_positive_with_return(), motor_decay_mode);
|
||||
}
|
||||
|
||||
void Motor2::to_percent(float in, float in_min, float in_max) {
|
||||
apply_duty(state.to_percent_with_return(in, in_min, in_max));
|
||||
apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_decay_mode);
|
||||
}
|
||||
|
||||
void Motor2::to_percent(float in, float in_min, float in_max, float speed_min, float speed_max) {
|
||||
apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max));
|
||||
apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max), motor_decay_mode);
|
||||
}
|
||||
|
||||
MotorState::Direction Motor2::direction() const {
|
||||
Direction Motor2::direction() const {
|
||||
return state.get_direction();
|
||||
}
|
||||
|
||||
void Motor2::direction(MotorState::Direction direction) {
|
||||
void Motor2::direction(Direction direction) {
|
||||
state.set_direction(direction);
|
||||
}
|
||||
|
||||
|
|
@ -166,28 +180,20 @@ namespace motor {
|
|||
state.set_speed_scale(speed_scale);
|
||||
}
|
||||
|
||||
float Motor2::deadzone_percent() const {
|
||||
return state.get_deadzone_percent();
|
||||
float Motor2::deadzone() const {
|
||||
return state.get_deadzone();
|
||||
}
|
||||
|
||||
void Motor2::deadzone_percent(float speed_scale) {
|
||||
apply_duty(state.set_deadzone_percent_with_return(speed_scale));
|
||||
void Motor2::deadzone(float deadzone) {
|
||||
apply_duty(state.set_deadzone_with_return(deadzone), motor_decay_mode);
|
||||
}
|
||||
|
||||
MotorState::DecayMode Motor2::decay_mode() {
|
||||
return motor_decay_mode;
|
||||
}
|
||||
|
||||
void Motor2::decay_mode(MotorState::DecayMode mode) {
|
||||
motor_decay_mode = mode;
|
||||
apply_duty(state.get_duty());
|
||||
}
|
||||
|
||||
void Motor2::apply_duty(float duty) {
|
||||
void Motor2::apply_duty(float duty, DecayMode mode) {
|
||||
if(isfinite(duty)) {
|
||||
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
|
||||
|
||||
switch(motor_decay_mode) {
|
||||
case MotorState::SLOW_DECAY: //aka 'Braking'
|
||||
switch(mode) {
|
||||
case SLOW_DECAY: //aka 'Braking'
|
||||
if(signed_level >= 0) {
|
||||
pwm_set_gpio_level(motor_pins.positive, pwm_period);
|
||||
pwm_set_gpio_level(motor_pins.negative, pwm_period - signed_level);
|
||||
|
|
@ -198,7 +204,7 @@ namespace motor {
|
|||
}
|
||||
break;
|
||||
|
||||
case MotorState::FAST_DECAY: //aka 'Coasting'
|
||||
case FAST_DECAY: //aka 'Coasting'
|
||||
default:
|
||||
if(signed_level >= 0) {
|
||||
pwm_set_gpio_level(motor_pins.positive, signed_level);
|
||||
|
|
@ -211,4 +217,9 @@ namespace motor {
|
|||
break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
pwm_set_gpio_level(motor_pins.positive, 0);
|
||||
pwm_set_gpio_level(motor_pins.negative, 0);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
@ -19,15 +19,14 @@ namespace motor {
|
|||
pwm_config pwm_cfg;
|
||||
uint16_t pwm_period;
|
||||
float pwm_frequency;
|
||||
MotorState::DecayMode motor_decay_mode;
|
||||
|
||||
DecayMode motor_decay_mode;
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
Motor2(const pin_pair &pins, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE);
|
||||
Motor2(const pin_pair &pins, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE);
|
||||
~Motor2();
|
||||
|
||||
|
||||
|
|
@ -53,10 +52,14 @@ namespace motor {
|
|||
float frequency() const;
|
||||
bool frequency(float freq);
|
||||
|
||||
DecayMode decay_mode();
|
||||
void decay_mode(DecayMode mode);
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
void stop();
|
||||
void coast();
|
||||
void brake(); //TODO
|
||||
void full_negative();
|
||||
void full_positive();
|
||||
void to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT);
|
||||
|
|
@ -64,21 +67,18 @@ namespace motor {
|
|||
|
||||
//--------------------------------------------------
|
||||
|
||||
MotorState::Direction direction() const;
|
||||
void direction(MotorState::Direction direction);
|
||||
Direction direction() const;
|
||||
void direction(Direction direction);
|
||||
|
||||
float speed_scale() const;
|
||||
void speed_scale(float speed_scale);
|
||||
|
||||
float deadzone_percent() const;
|
||||
void deadzone_percent(float deadzone_percent);
|
||||
|
||||
MotorState::DecayMode decay_mode();
|
||||
void decay_mode(MotorState::DecayMode mode);
|
||||
float deadzone() const;
|
||||
void deadzone(float deadzone);
|
||||
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
void apply_duty(float duty);
|
||||
void apply_duty(float duty, DecayMode mode);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -1,32 +1,34 @@
|
|||
#include "motor_cluster.hpp"
|
||||
#include "pwm.hpp"
|
||||
#include <cstdio>
|
||||
#include "math.h"
|
||||
|
||||
namespace motor {
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, MotorState::Direction direction,
|
||||
float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode,
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction,
|
||||
float speed_scale, float deadzone, float freq, DecayMode mode,
|
||||
bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_base, (pin_pair_count * 2), seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase);
|
||||
create_motor_states(direction, speed_scale, deadzone, mode, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, MotorState::Direction direction,
|
||||
float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode,
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction,
|
||||
float speed_scale, float deadzone, float freq, DecayMode mode,
|
||||
bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_pairs, length, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase);
|
||||
create_motor_states(direction, speed_scale, deadzone, mode, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, MotorState::Direction direction,
|
||||
float speed_scale, float deadzone_percent, float freq, MotorState::DecayMode mode,
|
||||
MotorCluster::MotorCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, Direction direction,
|
||||
float speed_scale, float deadzone, float freq, DecayMode mode,
|
||||
bool auto_phase, PWMCluster::Sequence *seq_buffer, PWMCluster::TransitionData *dat_buffer)
|
||||
: pwms(pio, sm, pin_pairs, seq_buffer, dat_buffer), pwm_frequency(freq) {
|
||||
create_motor_states(direction, speed_scale, deadzone_percent, mode, auto_phase);
|
||||
create_motor_states(direction, speed_scale, deadzone, mode, auto_phase);
|
||||
}
|
||||
|
||||
MotorCluster::~MotorCluster() {
|
||||
delete[] states;
|
||||
delete[] motor_phases;
|
||||
delete[] motor_modes;
|
||||
}
|
||||
|
||||
bool MotorCluster::init() {
|
||||
|
|
@ -74,7 +76,7 @@ namespace motor {
|
|||
void MotorCluster::enable(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].enable_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) {
|
||||
|
|
@ -106,7 +108,7 @@ namespace motor {
|
|||
void MotorCluster::disable(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].disable_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) {
|
||||
|
|
@ -148,7 +150,7 @@ namespace motor {
|
|||
void MotorCluster::duty(uint8_t motor, float duty, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].set_duty_with_return(duty);
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::duty(const uint8_t *motors, uint8_t length, float duty, bool load) {
|
||||
|
|
@ -185,7 +187,7 @@ namespace motor {
|
|||
void MotorCluster::speed(uint8_t motor, float speed, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].set_speed_with_return(speed);
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) {
|
||||
|
|
@ -270,7 +272,7 @@ namespace motor {
|
|||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint motor = 0; motor < motor_count; motor++) {
|
||||
if(states[motor].is_enabled()) {
|
||||
apply_duty(motor, states[motor].get_duty(), false);
|
||||
apply_duty(motor, states[motor].get_deadzoned_duty(), motor_modes[motor], false);
|
||||
}
|
||||
pwms.set_chan_offset(motor_positive(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
|
||||
pwms.set_chan_offset(motor_negative(motor), (uint32_t)(motor_phases[motor] * (float)pwm_period), false);
|
||||
|
|
@ -293,7 +295,7 @@ namespace motor {
|
|||
void MotorCluster::stop(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].stop_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::stop(const uint8_t *motors, uint8_t length, bool load) {
|
||||
|
|
@ -326,7 +328,7 @@ namespace motor {
|
|||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_duty_with_return(0.0f);
|
||||
float new_duty = states[motor].disable_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::coast(const uint8_t *motors, uint8_t length, bool load) {
|
||||
|
|
@ -358,7 +360,7 @@ namespace motor {
|
|||
void MotorCluster::full_negative(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].full_negative_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::full_negative(const uint8_t *motors, uint8_t length, bool load) {
|
||||
|
|
@ -378,7 +380,7 @@ namespace motor {
|
|||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_full_negative(bool load) {
|
||||
void MotorCluster::all_full_negative(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->full_negative(motor, false);
|
||||
|
|
@ -390,7 +392,7 @@ namespace motor {
|
|||
void MotorCluster::full_positive(uint8_t motor, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].full_positive_with_return();
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::full_positive(const uint8_t *motors, uint8_t length, bool load) {
|
||||
|
|
@ -410,7 +412,7 @@ namespace motor {
|
|||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
void MotorCluster::all_to_full_positive(bool load) {
|
||||
void MotorCluster::all_full_positive(bool load) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->full_positive(motor, false);
|
||||
|
|
@ -422,7 +424,7 @@ namespace motor {
|
|||
void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].to_percent_with_return(in, in_min, in_max);
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) {
|
||||
|
|
@ -454,7 +456,7 @@ namespace motor {
|
|||
void MotorCluster::to_percent(uint8_t motor, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
float new_duty = states[motor].to_percent_with_return(in, in_min, in_max, speed_min, speed_max);
|
||||
apply_duty(motor, new_duty, load);
|
||||
apply_duty(motor, new_duty, motor_modes[motor], load);
|
||||
}
|
||||
|
||||
void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, float speed_min, float speed_max, bool load) {
|
||||
|
|
@ -487,30 +489,30 @@ namespace motor {
|
|||
pwms.load_pwm();
|
||||
}
|
||||
|
||||
MotorState::Direction MotorCluster::direction(uint8_t motor) const {
|
||||
Direction MotorCluster::direction(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_direction();
|
||||
}
|
||||
|
||||
void MotorCluster::direction(uint8_t motor, MotorState::Direction direction) {
|
||||
void MotorCluster::direction(uint8_t motor, Direction direction) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_direction(direction);
|
||||
}
|
||||
|
||||
void MotorCluster::direction(const uint8_t *motors, uint8_t length, MotorState::Direction direction) {
|
||||
void MotorCluster::direction(const uint8_t *motors, uint8_t length, Direction direction) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->direction(motors[i], direction);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::direction(std::initializer_list<uint8_t> motors, MotorState::Direction direction) {
|
||||
void MotorCluster::direction(std::initializer_list<uint8_t> motors, Direction direction) {
|
||||
for(auto motor : motors) {
|
||||
this->direction(motor, direction);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_directions(MotorState::Direction direction) {
|
||||
void MotorCluster::all_directions(Direction direction) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->direction(motor, direction);
|
||||
|
|
@ -547,60 +549,60 @@ namespace motor {
|
|||
}
|
||||
}
|
||||
|
||||
float MotorCluster::deadzone_percent(uint8_t motor) const {
|
||||
float MotorCluster::deadzone(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return states[motor].get_deadzone_percent();
|
||||
return states[motor].get_deadzone();
|
||||
}
|
||||
|
||||
void MotorCluster::deadzone_percent(uint8_t motor, float deadzone_percent) {
|
||||
void MotorCluster::deadzone(uint8_t motor, float deadzone) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
states[motor].set_deadzone_percent_with_return(deadzone_percent); //TODO
|
||||
states[motor].set_deadzone_with_return(deadzone); //TODO
|
||||
}
|
||||
|
||||
void MotorCluster::deadzone_percent(const uint8_t *motors, uint8_t length, float deadzone_percent) {
|
||||
void MotorCluster::deadzone(const uint8_t *motors, uint8_t length, float deadzone) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->deadzone_percent(motors[i], deadzone_percent);
|
||||
this->deadzone(motors[i], deadzone);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::deadzone_percent(std::initializer_list<uint8_t> motors, float deadzone_percent) {
|
||||
void MotorCluster::deadzone(std::initializer_list<uint8_t> motors, float deadzone) {
|
||||
for(auto motor : motors) {
|
||||
this->deadzone_percent(motor, deadzone_percent);
|
||||
this->deadzone(motor, deadzone);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_deadzone_percents(float deadzone_percent) {
|
||||
void MotorCluster::all_deadzones(float deadzone) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->deadzone_percent(motor, deadzone_percent);
|
||||
this->deadzone(motor, deadzone);
|
||||
}
|
||||
}
|
||||
|
||||
MotorState::DecayMode MotorCluster::decay_mode(uint8_t motor) const {
|
||||
DecayMode MotorCluster::decay_mode(uint8_t motor) const {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
return MotorState::SLOW_DECAY;//TODO states[motor].get_decay_mode();
|
||||
return SLOW_DECAY;//TODO states[motor].get_decay_mode();
|
||||
}
|
||||
|
||||
void MotorCluster::decay_mode(uint8_t motor, MotorState::DecayMode mode) {
|
||||
void MotorCluster::decay_mode(uint8_t motor, DecayMode mode) {
|
||||
assert(motor < pwms.get_chan_pair_count());
|
||||
//TODO states[motor].set_decay_mode(mode);
|
||||
}
|
||||
|
||||
void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, MotorState::DecayMode mode) {
|
||||
void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode) {
|
||||
assert(motors != nullptr);
|
||||
for(uint8_t i = 0; i < length; i++) {
|
||||
this->decay_mode(motors[i], mode);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::decay_mode(std::initializer_list<uint8_t> motors, MotorState::DecayMode mode) {
|
||||
void MotorCluster::decay_mode(std::initializer_list<uint8_t> motors, DecayMode mode) {
|
||||
for(auto motor : motors) {
|
||||
this->decay_mode(motor, mode);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::all_decay_modes(MotorState::DecayMode mode) {
|
||||
void MotorCluster::all_to_decay_mode(DecayMode mode) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
for(uint8_t motor = 0; motor < motor_count; motor++) {
|
||||
this->decay_mode(motor, mode);
|
||||
|
|
@ -608,13 +610,12 @@ namespace motor {
|
|||
}
|
||||
|
||||
|
||||
void MotorCluster::apply_duty(uint8_t motor, float duty, bool load) {
|
||||
void MotorCluster::apply_duty(uint8_t motor, float duty, DecayMode mode, bool load) {
|
||||
if(isfinite(duty)) {
|
||||
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
|
||||
|
||||
//TODO move this into motor state
|
||||
MotorState::DecayMode decay_mode = MotorState::SLOW_DECAY;
|
||||
switch(decay_mode) {
|
||||
case MotorState::SLOW_DECAY: //aka 'Braking'
|
||||
switch(mode) {
|
||||
case SLOW_DECAY: //aka 'Braking'
|
||||
if(signed_level >= 0) {
|
||||
pwms.set_chan_level(motor_positive(motor), pwm_period, false);
|
||||
pwms.set_chan_level(motor_negative(motor), pwm_period - signed_level, load);
|
||||
|
|
@ -625,7 +626,7 @@ namespace motor {
|
|||
}
|
||||
break;
|
||||
|
||||
case MotorState::FAST_DECAY: //aka 'Coasting'
|
||||
case FAST_DECAY: //aka 'Coasting'
|
||||
default:
|
||||
if(signed_level >= 0) {
|
||||
pwms.set_chan_level(motor_positive(motor), signed_level, false);
|
||||
|
|
@ -638,17 +639,24 @@ namespace motor {
|
|||
break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
pwms.set_chan_level(motor_positive(motor), 0, false);
|
||||
pwms.set_chan_level(motor_negative(motor), 0, load);
|
||||
}
|
||||
}
|
||||
|
||||
void MotorCluster::create_motor_states(MotorState::Direction direction, float speed_scale,
|
||||
float deadzone_percent, MotorState::DecayMode mode, bool auto_phase) {
|
||||
void MotorCluster::create_motor_states(Direction direction, float speed_scale,
|
||||
float deadzone, DecayMode mode, bool auto_phase) {
|
||||
uint8_t motor_count = pwms.get_chan_pair_count();
|
||||
if(motor_count > 0) {
|
||||
states = new MotorState[motor_count];
|
||||
motor_phases = new float[motor_count];
|
||||
motor_modes = new DecayMode[motor_count];
|
||||
|
||||
for(uint motor = 0; motor < motor_count; motor++) {
|
||||
states[motor] = MotorState(direction, speed_scale, deadzone_percent);
|
||||
states[motor] = MotorState(direction, speed_scale, deadzone);
|
||||
motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
|
||||
motor_modes[motor] = mode;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,20 +18,21 @@ namespace motor {
|
|||
float pwm_frequency;
|
||||
MotorState* states;
|
||||
float* motor_phases;
|
||||
DecayMode* motor_modes;
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
// Constructors/Destructor
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
MotorCluster(PIO pio, uint sm, uint pin_base, uint pin_pair_count, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
MotorCluster(PIO pio, uint sm, const pin_pair *pin_pairs, uint32_t length, Direction direction = NORMAL, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
MotorCluster(PIO pio, uint sm, std::initializer_list<pin_pair> pin_pairs, MotorState::Direction direction = MotorState::DEFAULT_DIRECTION, float speed_scale = MotorState::DEFAULT_SPEED_SCALE,
|
||||
float deadzone_percent = MotorState::DEFAULT_DEADZONE, float freq = MotorState::DEFAULT_FREQUENCY, MotorState::DecayMode mode = MotorState::DEFAULT_DECAY_MODE,
|
||||
MotorCluster(PIO pio, uint sm, std::initializer_list<pin_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,
|
||||
bool auto_phase = true, PWMCluster::Sequence *seq_buffer = nullptr, PWMCluster::TransitionData *dat_buffer = nullptr);
|
||||
~MotorCluster();
|
||||
|
||||
|
|
@ -78,6 +79,12 @@ namespace motor {
|
|||
float frequency() const;
|
||||
bool frequency(float freq);
|
||||
|
||||
DecayMode decay_mode(uint8_t motor) const;
|
||||
void decay_mode(uint8_t motor, DecayMode mode);
|
||||
void decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode);
|
||||
void decay_mode(std::initializer_list<uint8_t> motors, DecayMode mode);
|
||||
void all_to_decay_mode(DecayMode mode);
|
||||
|
||||
//--------------------------------------------------
|
||||
void stop(uint8_t motor, bool load = true);
|
||||
void stop(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
|
|
@ -92,12 +99,12 @@ namespace motor {
|
|||
void full_negative(uint8_t motor, bool load = true);
|
||||
void full_negative(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void full_negative(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_to_full_negative(bool load = true);
|
||||
void all_full_negative(bool load = true);
|
||||
|
||||
void full_positive(uint8_t motor, bool load = true);
|
||||
void full_positive(const uint8_t *motors, uint8_t length, bool load = true);
|
||||
void full_positive(std::initializer_list<uint8_t> motors, bool load = true);
|
||||
void all_to_full_positive(bool load = true);
|
||||
void all_full_positive(bool load = true);
|
||||
|
||||
void to_percent(uint8_t motor, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
void to_percent(const uint8_t *motors, uint8_t length, float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT, bool load = true);
|
||||
|
|
@ -113,11 +120,11 @@ namespace motor {
|
|||
|
||||
//--------------------------------------------------
|
||||
|
||||
MotorState::Direction direction(uint8_t motor) const;
|
||||
void direction(uint8_t motor, MotorState::Direction direction);
|
||||
void direction(const uint8_t *motors, uint8_t length, MotorState::Direction direction);
|
||||
void direction(std::initializer_list<uint8_t> motors, MotorState::Direction direction);
|
||||
void all_directions(MotorState::Direction direction);
|
||||
Direction direction(uint8_t motor) const;
|
||||
void direction(uint8_t motor, Direction direction);
|
||||
void direction(const uint8_t *motors, uint8_t length, Direction direction);
|
||||
void direction(std::initializer_list<uint8_t> motors, Direction direction);
|
||||
void all_directions(Direction direction);
|
||||
|
||||
float speed_scale(uint8_t motor) const;
|
||||
void speed_scale(uint8_t motor, float speed_scale);
|
||||
|
|
@ -125,23 +132,16 @@ namespace motor {
|
|||
void speed_scale(std::initializer_list<uint8_t> motors, float speed_scale);
|
||||
void all_speed_scales(float speed_scale);
|
||||
|
||||
float deadzone_percent(uint8_t motor) const;
|
||||
void deadzone_percent(uint8_t motor, float deadzone_percent);
|
||||
void deadzone_percent(const uint8_t *motors, uint8_t length, float deadzone_percent);
|
||||
void deadzone_percent(std::initializer_list<uint8_t> motors, float deadzone_percent);
|
||||
void all_deadzone_percents(float deadzone_percent);
|
||||
|
||||
MotorState::DecayMode decay_mode(uint8_t motor) const;
|
||||
void decay_mode(uint8_t motor, MotorState::DecayMode mode);
|
||||
void decay_mode(const uint8_t *motors, uint8_t length, MotorState::DecayMode mode);
|
||||
void decay_mode(std::initializer_list<uint8_t> motors, MotorState::DecayMode mode);
|
||||
void all_decay_modes(MotorState::DecayMode mode);
|
||||
float deadzone(uint8_t motor) const;
|
||||
void deadzone(uint8_t motor, float deadzone);
|
||||
void deadzone(const uint8_t *motors, uint8_t length, float deadzone);
|
||||
void deadzone(std::initializer_list<uint8_t> motors, float deadzone);
|
||||
void all_deadzones(float deadzone);
|
||||
|
||||
//--------------------------------------------------
|
||||
private:
|
||||
void apply_duty(uint8_t motor, float duty, bool load);
|
||||
void create_motor_states(MotorState::Direction direction, float speed_scale,
|
||||
float deadzone_percent, MotorState::DecayMode mode, bool auto_phase);
|
||||
void apply_duty(uint8_t motor, float duty, DecayMode mode, bool load);
|
||||
void create_motor_states(Direction direction, float speed_scale, float deadzone, DecayMode mode, bool auto_phase);
|
||||
|
||||
static uint8_t motor_positive(uint8_t motor) {
|
||||
return PWMCluster::channel_from_pair(motor);
|
||||
|
|
|
|||
|
|
@ -1,34 +1,27 @@
|
|||
#include "motor_state.hpp"
|
||||
#include "common/pimoroni_common.hpp"
|
||||
#include "float.h"
|
||||
#include "math.h"
|
||||
|
||||
namespace motor {
|
||||
MotorState::MotorState()
|
||||
: motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false),
|
||||
motor_direction(DEFAULT_DIRECTION), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE) {
|
||||
motor_direction(NORMAL), motor_scale(DEFAULT_SPEED_SCALE), motor_deadzone(DEFAULT_DEADZONE){
|
||||
}
|
||||
|
||||
MotorState::MotorState(Direction direction, float speed_scale, float deadzone_percent)
|
||||
MotorState::MotorState(Direction direction, float speed_scale, float deadzone)
|
||||
: motor_speed(0.0f), last_enabled_duty(0.0f), enabled(false),
|
||||
motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_deadzone(CLAMP(deadzone_percent, 0.0f, 1.0f)) {
|
||||
motor_direction(direction), motor_scale(MAX(speed_scale, FLT_EPSILON)), motor_deadzone(CLAMP(deadzone, 0.0f, 1.0f)) {
|
||||
}
|
||||
|
||||
float MotorState::enable_with_return() {
|
||||
enabled = true;
|
||||
|
||||
float duty = 0.0f;
|
||||
if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) {
|
||||
if(motor_direction == NORMAL)
|
||||
duty = last_enabled_duty;
|
||||
else
|
||||
duty = 0.0f - last_enabled_duty;
|
||||
}
|
||||
return duty;
|
||||
return get_deadzoned_duty();
|
||||
}
|
||||
|
||||
float MotorState::disable_with_return() {
|
||||
enabled = false;
|
||||
return 0.0f; // A zero duty
|
||||
return NAN;
|
||||
}
|
||||
|
||||
bool MotorState::is_enabled() const {
|
||||
|
|
@ -39,6 +32,14 @@ namespace motor {
|
|||
return last_enabled_duty;
|
||||
}
|
||||
|
||||
float MotorState::get_deadzoned_duty() const {
|
||||
float duty = 0.0f;
|
||||
if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) {
|
||||
duty = last_enabled_duty;
|
||||
}
|
||||
return duty;
|
||||
}
|
||||
|
||||
float MotorState::set_duty_with_return(float duty) {
|
||||
// Clamp the duty between the hard limits
|
||||
last_enabled_duty = CLAMP(duty, -1.0f, 1.0f);
|
||||
|
|
@ -48,10 +49,14 @@ namespace motor {
|
|||
}
|
||||
|
||||
float MotorState::get_speed() const {
|
||||
return motor_speed;
|
||||
return (motor_direction == NORMAL) ? motor_speed : 0.0f - motor_speed;
|
||||
}
|
||||
|
||||
float MotorState::set_speed_with_return(float speed) {
|
||||
// Invert provided speed if the motor direction is reversed
|
||||
if(motor_direction == REVERSED)
|
||||
speed = 0.0f - speed;
|
||||
|
||||
// Clamp the speed between the hard limits
|
||||
motor_speed = CLAMP(speed, -motor_scale, motor_scale);
|
||||
last_enabled_duty = motor_speed / motor_scale;
|
||||
|
|
@ -81,16 +86,12 @@ namespace motor {
|
|||
return set_speed_with_return(speed);
|
||||
}
|
||||
|
||||
MotorState::Direction MotorState::get_direction() const {
|
||||
Direction MotorState::get_direction() const {
|
||||
return motor_direction;
|
||||
}
|
||||
|
||||
void MotorState::set_direction(Direction direction) {
|
||||
if(direction != motor_direction) {
|
||||
motor_direction = direction;
|
||||
motor_speed = 0.0f - motor_speed;
|
||||
last_enabled_duty = 0.0f - last_enabled_duty;
|
||||
}
|
||||
}
|
||||
|
||||
float MotorState::get_speed_scale() const {
|
||||
|
|
@ -103,16 +104,16 @@ namespace motor {
|
|||
}
|
||||
|
||||
|
||||
float MotorState::get_deadzone_percent() const {
|
||||
return motor_scale;
|
||||
float MotorState::get_deadzone() const {
|
||||
return motor_deadzone;
|
||||
}
|
||||
|
||||
float MotorState::set_deadzone_percent_with_return(float deadzone_percent) {
|
||||
motor_deadzone = CLAMP(deadzone_percent, 0.0f, 1.0f);
|
||||
float MotorState::set_deadzone_with_return(float deadzone) {
|
||||
motor_deadzone = CLAMP(deadzone, 0.0f, 1.0f);
|
||||
if(enabled)
|
||||
return enable_with_return();
|
||||
return get_deadzoned_duty();
|
||||
else
|
||||
return disable_with_return();
|
||||
return NAN;
|
||||
}
|
||||
|
||||
int32_t MotorState::duty_to_level(float duty, uint32_t resolution) {
|
||||
|
|
|
|||
|
|
@ -4,27 +4,21 @@
|
|||
|
||||
namespace motor {
|
||||
|
||||
class MotorState {
|
||||
//--------------------------------------------------
|
||||
// Enums
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
enum DecayMode {
|
||||
FAST_DECAY = 0, //aka 'Coasting'
|
||||
SLOW_DECAY = 1, //aka 'Braking'
|
||||
};
|
||||
|
||||
enum Direction {
|
||||
NORMAL = 0,
|
||||
REVERSED = 1,
|
||||
};
|
||||
|
||||
enum DecayMode {
|
||||
FAST_DECAY = 0, //aka 'Coasting'
|
||||
SLOW_DECAY = 1, //aka 'Braking'
|
||||
};
|
||||
|
||||
class MotorState {
|
||||
//--------------------------------------------------
|
||||
// Constants
|
||||
//--------------------------------------------------
|
||||
public:
|
||||
static const Direction DEFAULT_DIRECTION = NORMAL; // The standard motor direction
|
||||
static constexpr float DEFAULT_SPEED_SCALE = 1.0f; // The standard motor speed scale
|
||||
static constexpr float DEFAULT_DEADZONE = 0.1f; // The standard motor deadzone
|
||||
|
||||
|
|
@ -56,7 +50,7 @@ namespace motor {
|
|||
//--------------------------------------------------
|
||||
public:
|
||||
MotorState();
|
||||
MotorState(Direction direction, float speed_scale, float deadzone_percent);
|
||||
MotorState(Direction direction, float speed_scale, float deadzone);
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
|
|
@ -68,6 +62,7 @@ namespace motor {
|
|||
bool is_enabled() const;
|
||||
|
||||
float get_duty() const;
|
||||
float get_deadzoned_duty() const;
|
||||
float set_duty_with_return(float duty);
|
||||
|
||||
float get_speed() const;
|
||||
|
|
@ -89,8 +84,8 @@ namespace motor {
|
|||
float get_speed_scale() const;
|
||||
void set_speed_scale(float speed_scale);
|
||||
|
||||
float get_deadzone_percent() const;
|
||||
float set_deadzone_percent_with_return(float deadzone_percent);
|
||||
float get_deadzone() const;
|
||||
float set_deadzone_with_return(float deadzone);
|
||||
|
||||
//--------------------------------------------------
|
||||
static int32_t duty_to_level(float duty, uint32_t resolution);
|
||||
|
|
|
|||
|
|
@ -47,8 +47,8 @@ void Motor_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind
|
|||
mp_print_str(print, ", direction = REVERSED");
|
||||
mp_print_str(print, ", speed_scale = ");
|
||||
mp_obj_print_helper(print, mp_obj_new_float(self->motor->speed_scale()), PRINT_REPR);
|
||||
mp_print_str(print, ", deadzone_percent = ");
|
||||
mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone_percent()), PRINT_REPR);
|
||||
mp_print_str(print, ", deadzone = ");
|
||||
mp_obj_print_helper(print, mp_obj_new_float(self->motor->deadzone()), PRINT_REPR);
|
||||
if(self->motor->decay_mode() == MotorState::SLOW_DECAY)
|
||||
mp_print_str(print, ", decay_mode = SLOW_DECAY");
|
||||
else
|
||||
|
|
@ -465,7 +465,7 @@ extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args,
|
|||
|
||||
_Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t);
|
||||
|
||||
return mp_obj_new_float(self->motor->deadzone_percent());
|
||||
return mp_obj_new_float(self->motor->deadzone());
|
||||
}
|
||||
else {
|
||||
enum { ARG_self, ARG_deadzone_percent };
|
||||
|
|
@ -480,9 +480,9 @@ extern mp_obj_t Motor_deadzone_percent(size_t n_args, const mp_obj_t *pos_args,
|
|||
|
||||
_Motor_obj_t *self = MP_OBJ_TO_PTR2(args[ARG_self].u_obj, _Motor_obj_t);
|
||||
|
||||
float deadzone_percent = mp_obj_get_float(args[ARG_deadzone_percent].u_obj);
|
||||
float deadzone = mp_obj_get_float(args[ARG_deadzone_percent].u_obj);
|
||||
|
||||
self->motor->deadzone_percent(deadzone_percent);
|
||||
self->motor->deadzone(deadzone);
|
||||
return mp_const_none;
|
||||
}
|
||||
}
|
||||
|
|
@ -1605,7 +1605,7 @@ extern mp_obj_t MotorCluster_all_to_full_negative(size_t n_args, const mp_obj_t
|
|||
if(motor_count == 0)
|
||||
mp_raise_ValueError("this cluster does not have any motors");
|
||||
else {
|
||||
self->cluster->all_to_full_negative(args[ARG_load].u_bool);
|
||||
self->cluster->all_full_negative(args[ARG_load].u_bool);
|
||||
}
|
||||
return mp_const_none;
|
||||
}
|
||||
|
|
@ -1693,7 +1693,7 @@ extern mp_obj_t MotorCluster_all_to_full_positive(size_t n_args, const mp_obj_t
|
|||
if(motor_count == 0)
|
||||
mp_raise_ValueError("this cluster does not have any motors");
|
||||
else {
|
||||
self->cluster->all_to_full_positive(args[ARG_load].u_bool);
|
||||
self->cluster->all_full_positive(args[ARG_load].u_bool);
|
||||
}
|
||||
return mp_const_none;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue