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

Finalised C++ motor implementation

motor-pio
ZodiusInfuser 2022-04-11 13:35:53 +01:00
rodzic f3c0a305f2
commit 5f6e4a3096
5 zmienionych plików z 154 dodań i 115 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

58
drivers/motor/motor2.cpp
Wyświetl plik

@ -5,7 +5,7 @@
namespace motor {
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) {
: motor_pins(pins), state(direction, speed_scale, deadzone), pwm_frequency(freq), motor_mode(mode) {
}
Motor2::~Motor2() {
@ -29,9 +29,9 @@ namespace motor {
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);
gpio_set_function(motor_pins.positive, GPIO_FUNC_PWM);
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);
@ -47,11 +47,11 @@ namespace motor {
}
void Motor2::enable() {
apply_duty(state.enable_with_return(), motor_decay_mode);
apply_duty(state.enable_with_return(), motor_mode);
}
void Motor2::disable() {
apply_duty(state.disable_with_return(), motor_decay_mode);
apply_duty(state.disable_with_return(), motor_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), motor_decay_mode);
apply_duty(state.set_duty_with_return(duty), motor_mode);
}
float Motor2::speed() const {
@ -71,7 +71,7 @@ namespace motor {
}
void Motor2::speed(float speed) {
apply_duty(state.set_speed_with_return(speed), motor_decay_mode);
apply_duty(state.set_speed_with_return(speed), motor_mode);
}
float Motor2::frequency() const {
@ -104,9 +104,9 @@ namespace motor {
if(neg_pin_num != pos_pin_num)
pwm_set_clkdiv_int_frac(neg_pin_num, div, mod);
// If the the period is larger, update the pwm before setting the new wraps
if(state.is_enabled() && pre_update_pwm) {
apply_duty(state.get_deadzoned_duty(), motor_decay_mode);
// 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%)
@ -114,9 +114,9 @@ namespace motor {
if(neg_pin_num != pos_pin_num)
pwm_set_wrap(neg_pin_num, pwm_period - 1);
// If the the period is smaller, update the pwm after setting the new wraps
if(state.is_enabled() && !pre_update_pwm) {
apply_duty(state.get_deadzoned_duty(), motor_decay_mode);
// 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;
@ -125,19 +125,8 @@ 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(), motor_decay_mode);
apply_duty(state.stop_with_return(), motor_mode);
}
void Motor2::coast() {
@ -149,19 +138,19 @@ namespace motor {
}
void Motor2::full_negative() {
apply_duty(state.full_negative_with_return(), motor_decay_mode);
apply_duty(state.full_negative_with_return(), motor_mode);
}
void Motor2::full_positive() {
apply_duty(state.full_positive_with_return(), motor_decay_mode);
apply_duty(state.full_positive_with_return(), motor_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), motor_decay_mode);
apply_duty(state.to_percent_with_return(in, in_min, in_max), motor_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), motor_decay_mode);
apply_duty(state.to_percent_with_return(in, in_min, in_max, speed_min, speed_max), motor_mode);
}
Direction Motor2::direction() const {
@ -185,7 +174,16 @@ namespace motor {
}
void Motor2::deadzone(float deadzone) {
apply_duty(state.set_deadzone_with_return(deadzone), motor_decay_mode);
apply_duty(state.set_deadzone_with_return(deadzone), motor_mode);
}
DecayMode Motor2::decay_mode() {
return motor_mode;
}
void Motor2::decay_mode(DecayMode mode) {
motor_mode = mode;
apply_duty(state.get_deadzoned_duty(), motor_mode);
}
void Motor2::apply_duty(float duty, DecayMode mode) {

10
drivers/motor/motor2.hpp
Wyświetl plik

@ -19,7 +19,7 @@ namespace motor {
pwm_config pwm_cfg;
uint16_t pwm_period;
float pwm_frequency;
DecayMode motor_decay_mode;
DecayMode motor_mode;
//--------------------------------------------------
// Constructors/Destructor
@ -52,14 +52,11 @@ 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 brake();
void full_negative();
void full_positive();
void to_percent(float in, float in_min = MotorState::ZERO_PERCENT, float in_max = MotorState::ONEHUNDRED_PERCENT);
@ -76,6 +73,9 @@ namespace motor {
float deadzone() const;
void deadzone(float deadzone);
DecayMode decay_mode();
void decay_mode(DecayMode mode);
//--------------------------------------------------
private:
void apply_duty(float duty, DecayMode mode);

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

@ -3,6 +3,9 @@
#include <cstdio>
#include "math.h"
#define POS_MOTOR(motor) (PWMCluster::channel_from_pair(motor))
#define NEG_MOTOR(motor) (PWMCluster::channel_from_pair(motor) + 1)
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,
@ -27,8 +30,7 @@ namespace motor {
MotorCluster::~MotorCluster() {
delete[] states;
delete[] motor_phases;
delete[] motor_modes;
delete[] configs;
}
bool MotorCluster::init() {
@ -43,10 +45,10 @@ namespace motor {
// Update the pwm before setting the new wrap
uint8_t motor_count = pwms.get_chan_pair_count();
for(uint8_t motor = 0; motor < motor_count; motor++) {
pwms.set_chan_level(motor_positive(motor), 0, false);
pwms.set_chan_level(motor_negative(motor), 0, 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);
pwms.set_chan_level(POS_MOTOR(motor), 0, false);
pwms.set_chan_level(NEG_MOTOR(motor), 0, false);
pwms.set_chan_offset(POS_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
pwms.set_chan_offset(NEG_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
}
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
@ -70,13 +72,14 @@ namespace motor {
}
pin_pair MotorCluster::pins(uint8_t motor) const {
assert(motor < pwms.get_chan_pair_count());
return pwms.get_chan_pin_pair(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, motor_modes[motor], load);
apply_duty(motor, new_duty, configs[motor].mode, load);
}
void MotorCluster::enable(const uint8_t *motors, uint8_t length, bool load) {
@ -108,7 +111,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, motor_modes[motor], load);
apply_duty(motor, new_duty, configs[motor].mode, load);
}
void MotorCluster::disable(const uint8_t *motors, uint8_t length, bool load) {
@ -150,7 +153,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, motor_modes[motor], load);
apply_duty(motor, new_duty, configs[motor].mode, load);
}
void MotorCluster::duty(const uint8_t *motors, uint8_t length, float duty, bool load) {
@ -187,7 +190,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, motor_modes[motor], load);
apply_duty(motor, new_duty, configs[motor].mode, load);
}
void MotorCluster::speed(const uint8_t *motors, uint8_t length, float speed, bool load) {
@ -218,13 +221,13 @@ namespace motor {
float MotorCluster::phase(uint8_t motor) const {
assert(motor < pwms.get_chan_pair_count());
return motor_phases[motor];
return configs[motor].phase;
}
void MotorCluster::phase(uint8_t motor, float phase, bool load) {
assert(motor < pwms.get_chan_pair_count());
motor_phases[motor] = MIN(MAX(phase, 0.0f), 1.0f);
pwms.set_chan_offset(motor, (uint32_t)(motor_phases[motor] * (float)pwms.get_wrap()), load);
configs[motor].phase = MIN(MAX(phase, 0.0f), 1.0f);
pwms.set_chan_offset(motor, (uint32_t)(configs[motor].phase * (float)pwms.get_wrap()), load);
}
void MotorCluster::phase(const uint8_t *motors, uint8_t length, float phase, bool load) {
@ -271,11 +274,9 @@ namespace motor {
// Update the pwm before setting the new wrap
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_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);
apply_duty(motor, states[motor].get_deadzoned_duty(), configs[motor].mode, false);
pwms.set_chan_offset(POS_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
pwms.set_chan_offset(NEG_MOTOR(motor), (uint32_t)(configs[motor].phase * (float)pwm_period), false);
}
// Set the new wrap (should be 1 less than the period to get full 0 to 100%)
@ -295,7 +296,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, motor_modes[motor], load);
apply_duty(motor, new_duty, configs[motor].mode, load);
}
void MotorCluster::stop(const uint8_t *motors, uint8_t length, bool load) {
@ -326,9 +327,8 @@ namespace motor {
void MotorCluster::coast(uint8_t motor, bool load) {
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, motor_modes[motor], load);
float new_duty = states[motor].stop_with_return();
apply_duty(motor, new_duty, FAST_DECAY, load);
}
void MotorCluster::coast(const uint8_t *motors, uint8_t length, bool load) {
@ -357,10 +357,42 @@ namespace motor {
pwms.load_pwm();
}
void MotorCluster::brake(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, SLOW_DECAY, load);
}
void MotorCluster::brake(const uint8_t *motors, uint8_t length, bool load) {
assert(motors != nullptr);
for(uint8_t i = 0; i < length; i++) {
this->brake(motors[i], false);
}
if(load)
pwms.load_pwm();
}
void MotorCluster::brake(std::initializer_list<uint8_t> motors, bool load) {
for(auto motor : motors) {
this->brake(motor, false);
}
if(load)
pwms.load_pwm();
}
void MotorCluster::brake_all(bool load) {
uint8_t motor_count = pwms.get_chan_pair_count();
for(uint8_t motor = 0; motor < motor_count; motor++) {
this->brake(motor, false);
}
if(load)
pwms.load_pwm();
}
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, motor_modes[motor], load);
apply_duty(motor, new_duty, configs[motor].mode, load);
}
void MotorCluster::full_negative(const uint8_t *motors, uint8_t length, bool load) {
@ -392,7 +424,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, motor_modes[motor], load);
apply_duty(motor, new_duty, configs[motor].mode, load);
}
void MotorCluster::full_positive(const uint8_t *motors, uint8_t length, bool load) {
@ -424,7 +456,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, motor_modes[motor], load);
apply_duty(motor, new_duty, configs[motor].mode, load);
}
void MotorCluster::to_percent(const uint8_t *motors, uint8_t length, float in, float in_min, float in_max, bool load) {
@ -456,7 +488,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, motor_modes[motor], load);
apply_duty(motor, new_duty, configs[motor].mode, 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) {
@ -512,7 +544,7 @@ namespace motor {
}
}
void MotorCluster::all_directions(Direction direction) {
void MotorCluster::all_to_direction(Direction direction) {
uint8_t motor_count = pwms.get_chan_pair_count();
for(uint8_t motor = 0; motor < motor_count; motor++) {
this->direction(motor, direction);
@ -542,7 +574,7 @@ namespace motor {
}
}
void MotorCluster::all_speed_scales(float speed_scale) {
void MotorCluster::all_to_speed_scale(float speed_scale) {
uint8_t motor_count = pwms.get_chan_pair_count();
for(uint8_t motor = 0; motor < motor_count; motor++) {
this->speed_scale(motor, speed_scale);
@ -554,25 +586,26 @@ namespace motor {
return states[motor].get_deadzone();
}
void MotorCluster::deadzone(uint8_t motor, float deadzone) {
void MotorCluster::deadzone(uint8_t motor, float deadzone, bool load) {
assert(motor < pwms.get_chan_pair_count());
states[motor].set_deadzone_with_return(deadzone); //TODO
float new_duty = states[motor].set_deadzone_with_return(deadzone);
apply_duty(motor, new_duty, configs[motor].mode, load);
}
void MotorCluster::deadzone(const uint8_t *motors, uint8_t length, float deadzone) {
void MotorCluster::deadzone(const uint8_t *motors, uint8_t length, float deadzone, bool load) {
assert(motors != nullptr);
for(uint8_t i = 0; i < length; i++) {
this->deadzone(motors[i], deadzone);
}
}
void MotorCluster::deadzone(std::initializer_list<uint8_t> motors, float deadzone) {
void MotorCluster::deadzone(std::initializer_list<uint8_t> motors, float deadzone, bool load) {
for(auto motor : motors) {
this->deadzone(motor, deadzone);
}
}
void MotorCluster::all_deadzones(float deadzone) {
void MotorCluster::all_to_deadzone(float deadzone, bool load) {
uint8_t motor_count = pwms.get_chan_pair_count();
for(uint8_t motor = 0; motor < motor_count; motor++) {
this->deadzone(motor, deadzone);
@ -581,28 +614,29 @@ namespace motor {
DecayMode MotorCluster::decay_mode(uint8_t motor) const {
assert(motor < pwms.get_chan_pair_count());
return SLOW_DECAY;//TODO states[motor].get_decay_mode();
return configs[motor].mode;
}
void MotorCluster::decay_mode(uint8_t motor, DecayMode mode) {
void MotorCluster::decay_mode(uint8_t motor, DecayMode mode, bool load) {
assert(motor < pwms.get_chan_pair_count());
//TODO states[motor].set_decay_mode(mode);
configs[motor].mode = mode;
apply_duty(motor, states[motor].get_deadzoned_duty(), configs[motor].mode, false);
}
void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode) {
void MotorCluster::decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode, bool load) {
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, DecayMode mode) {
void MotorCluster::decay_mode(std::initializer_list<uint8_t> motors, DecayMode mode, bool load) {
for(auto motor : motors) {
this->decay_mode(motor, mode);
}
}
void MotorCluster::all_to_decay_mode(DecayMode mode) {
void MotorCluster::all_to_decay_mode(DecayMode mode, bool load) {
uint8_t motor_count = pwms.get_chan_pair_count();
for(uint8_t motor = 0; motor < motor_count; motor++) {
this->decay_mode(motor, mode);
@ -617,31 +651,31 @@ namespace motor {
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);
pwms.set_chan_level(POS_MOTOR(motor), pwm_period, false);
pwms.set_chan_level(NEG_MOTOR(motor), pwm_period - signed_level, load);
}
else {
pwms.set_chan_level(motor_positive(motor), pwm_period + signed_level, false);
pwms.set_chan_level(motor_negative(motor), pwm_period, load);
pwms.set_chan_level(POS_MOTOR(motor), pwm_period + signed_level, false);
pwms.set_chan_level(NEG_MOTOR(motor), pwm_period, load);
}
break;
case FAST_DECAY: //aka 'Coasting'
default:
if(signed_level >= 0) {
pwms.set_chan_level(motor_positive(motor), signed_level, false);
pwms.set_chan_level(motor_negative(motor), 0, load);
pwms.set_chan_level(POS_MOTOR(motor), signed_level, false);
pwms.set_chan_level(NEG_MOTOR(motor), 0, load);
}
else {
pwms.set_chan_level(motor_positive(motor), 0, false);
pwms.set_chan_level(motor_negative(motor), 0 - signed_level, load);
pwms.set_chan_level(POS_MOTOR(motor), 0, false);
pwms.set_chan_level(NEG_MOTOR(motor), 0 - signed_level, load);
}
break;
}
}
else {
pwms.set_chan_level(motor_positive(motor), 0, false);
pwms.set_chan_level(motor_negative(motor), 0, load);
pwms.set_chan_level(POS_MOTOR(motor), 0, false);
pwms.set_chan_level(NEG_MOTOR(motor), 0, load);
}
}
@ -650,13 +684,12 @@ namespace motor {
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];
configs = new motor_config[motor_count];
for(uint motor = 0; motor < motor_count; motor++) {
states[motor] = MotorState(direction, speed_scale, deadzone);
motor_phases[motor] = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
motor_modes[motor] = mode;
configs[motor].phase = (auto_phase) ? (float)motor / (float)motor_count : 0.0f;
configs[motor].mode = mode;
}
}
}

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

@ -9,6 +9,16 @@ using namespace pimoroni;
namespace motor {
class MotorCluster {
//--------------------------------------------------
// Substructures
//--------------------------------------------------
private:
struct motor_config {
float phase;
DecayMode mode;
};
//--------------------------------------------------
// Variables
//--------------------------------------------------
@ -17,8 +27,7 @@ namespace motor {
uint32_t pwm_period;
float pwm_frequency;
MotorState* states;
float* motor_phases;
DecayMode* motor_modes;
motor_config* configs;
//--------------------------------------------------
@ -79,12 +88,6 @@ 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);
@ -96,6 +99,11 @@ namespace motor {
void coast(std::initializer_list<uint8_t> motors, bool load = true);
void coast_all(bool load = true);
void brake(uint8_t motor, bool load = true);
void brake(const uint8_t *motors, uint8_t length, bool load = true);
void brake(std::initializer_list<uint8_t> motors, bool load = true);
void brake_all(bool load = true);
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);
@ -124,31 +132,30 @@ namespace motor {
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);
void all_to_direction(Direction direction);
float speed_scale(uint8_t motor) const;
void speed_scale(uint8_t motor, float speed_scale);
void speed_scale(const uint8_t *motors, uint8_t length, float speed_scale);
void speed_scale(std::initializer_list<uint8_t> motors, float speed_scale);
void all_speed_scales(float speed_scale);
void all_to_speed_scale(float speed_scale);
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);
void deadzone(uint8_t motor, float deadzone, bool load = true);
void deadzone(const uint8_t *motors, uint8_t length, float deadzone, bool load = true);
void deadzone(std::initializer_list<uint8_t> motors, float deadzone, bool load = true);
void all_to_deadzone(float deadzone, bool load = true);
DecayMode decay_mode(uint8_t motor) const;
void decay_mode(uint8_t motor, DecayMode mode, bool load = true);
void decay_mode(const uint8_t *motors, uint8_t length, DecayMode mode, bool load = true);
void decay_mode(std::initializer_list<uint8_t> motors, DecayMode mode, bool load = true);
void all_to_decay_mode(DecayMode mode, bool load = true);
//--------------------------------------------------
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);
static uint8_t motor_positive(uint8_t motor) {
return PWMCluster::channel_from_pair(motor);
}
static uint8_t motor_negative(uint8_t motor) {
return PWMCluster::channel_from_pair(motor) + 1;
}
};
}

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

@ -37,7 +37,11 @@ namespace motor {
if((last_enabled_duty <= 0.0f - motor_deadzone) || (last_enabled_duty >= motor_deadzone)) {
duty = last_enabled_duty;
}
return duty;
if(enabled)
return duty;
else
return NAN;
}
float MotorState::set_duty_with_return(float duty) {
@ -77,7 +81,7 @@ namespace motor {
}
float MotorState::to_percent_with_return(float in, float in_min, float in_max) {
float speed = MotorState::map_float(in, in_min, in_max, 0.0f - get_speed_scale(), get_speed_scale());
float speed = MotorState::map_float(in, in_min, in_max, 0.0f - motor_speed, motor_speed);
return set_speed_with_return(speed);
}
@ -110,10 +114,7 @@ namespace motor {
float MotorState::set_deadzone_with_return(float deadzone) {
motor_deadzone = CLAMP(deadzone, 0.0f, 1.0f);
if(enabled)
return get_deadzoned_duty();
else
return NAN;
return get_deadzoned_duty();
}
int32_t MotorState::duty_to_level(float duty, uint32_t resolution) {