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

Removed old C++ motor implementation

motor-and-encoder
ZodiusInfuser 2022-04-22 13:59:40 +01:00
rodzic 98c57741b6
commit 652612fc31
15 zmienionych plików z 252 dodań i 490 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

1
drivers/motor/CMakeLists.txt
Wyświetl plik

@ -1,3 +1,2 @@
include(motor.cmake)
include(motor2.cmake)
include(motor_cluster.cmake)

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

@ -1,14 +1,16 @@
#include "motor.hpp"
#include "hardware/clocks.h"
#include "pwm.hpp"
#include "math.h"
namespace motor {
Motor::Motor(const pin_pair &pins, float freq, DecayMode mode)
: pins(pins), pwm_frequency(freq), motor_decay_mode(mode) {
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::~Motor() {
gpio_set_function(pins.positive, GPIO_FUNC_NULL);
gpio_set_function(pins.negative, GPIO_FUNC_NULL);
gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL);
gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL);
}
bool Motor::init() {
@ -20,128 +22,202 @@ namespace motor {
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);
// 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);
// 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(pins.positive), &pwm_cfg, true);
gpio_set_function(pins.positive, GPIO_FUNC_PWM);
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);
pwm_init(pwm_gpio_to_slice_num(pins.negative), &pwm_cfg, true);
gpio_set_function(pins.negative, GPIO_FUNC_PWM);
update_pwm();
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;
}
float Motor::get_speed() {
return motor_speed;
pin_pair Motor::pins() const {
return motor_pins;
}
void Motor::set_speed(float speed) {
motor_speed = MIN(MAX(speed, -1.0f), 1.0f);
update_pwm();
}
float Motor::get_frequency() {
return pwm_frequency;
}
bool Motor::set_frequency(float freq) {
bool success = false;
//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 values 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_num = pwm_gpio_to_slice_num(pins.positive);
uint neg_num = pwm_gpio_to_slice_num(pins.negative);
//Apply the new divider
uint8_t div = div16 >> 4;
uint8_t mod = div16 % 16;
pwm_set_clkdiv_int_frac(pos_num, div, mod);
if(neg_num != pos_num) {
pwm_set_clkdiv_int_frac(neg_num, div, mod);
}
//If the the period is larger, update the pwm before setting the new wraps
if(pre_update_pwm)
update_pwm();
//Set the new wrap (should be 1 less than the period to get full 0 to 100%)
pwm_set_wrap(pos_num, pwm_period - 1);
if(neg_num != pos_num) {
pwm_set_wrap(neg_num, pwm_period - 1);
}
//If the the period is smaller, update the pwm after setting the new wraps
if(!pre_update_pwm)
update_pwm();
success = true;
}
return success;
}
Motor::DecayMode Motor::get_decay_mode() {
return motor_decay_mode;
}
void Motor::set_decay_mode(Motor::DecayMode mode) {
motor_decay_mode = mode;
update_pwm();
}
void Motor::stop() {
motor_speed = 0.0f;
update_pwm();
void Motor::enable() {
apply_duty(state.enable_with_return(), motor_mode);
}
void Motor::disable() {
motor_speed = 0.0f;
pwm_set_gpio_level(pins.positive, 0);
pwm_set_gpio_level(pins.negative, 0);
apply_duty(state.disable_with_return(), motor_mode);
}
void Motor::update_pwm() {
int32_t signed_duty_cycle = (int32_t)(motor_speed * (float)pwm_period);
bool Motor::is_enabled() const {
return state.is_enabled();
}
switch(motor_decay_mode) {
case SLOW_DECAY: //aka 'Braking'
if(signed_duty_cycle >= 0) {
pwm_set_gpio_level(pins.positive, pwm_period);
pwm_set_gpio_level(pins.negative, pwm_period - signed_duty_cycle);
}
else {
pwm_set_gpio_level(pins.positive, pwm_period + signed_duty_cycle);
pwm_set_gpio_level(pins.negative, pwm_period);
}
break;
float Motor::duty() const {
return state.get_duty();
}
case FAST_DECAY: //aka 'Coasting'
default:
if(signed_duty_cycle >= 0) {
pwm_set_gpio_level(pins.positive, signed_duty_cycle);
pwm_set_gpio_level(pins.negative, 0);
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;
}
else {
pwm_set_gpio_level(pins.positive, 0);
pwm_set_gpio_level(pins.negative, 0 - signed_duty_cycle);
}
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) {
if(isfinite(duty)) {
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
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);
}
else {
pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level);
pwm_set_gpio_level(motor_pins.negative, pwm_period);
}
break;
case FAST_DECAY: //aka 'Coasting'
default:
if(signed_level >= 0) {
pwm_set_gpio_level(motor_pins.positive, signed_level);
pwm_set_gpio_level(motor_pins.negative, 0);
}
else {
pwm_set_gpio_level(motor_pins.positive, 0);
pwm_set_gpio_level(motor_pins.negative, 0 - signed_level);
}
break;
}
break;
}
else {
pwm_set_gpio_level(motor_pins.positive, 0);
pwm_set_gpio_level(motor_pins.negative, 0);
}
}
};

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

@ -10,42 +10,23 @@ using namespace pimoroni;
namespace motor {
class Motor {
//--------------------------------------------------
// Enums
//--------------------------------------------------
public:
enum DecayMode {
FAST_DECAY = 0, //aka 'Coasting'
SLOW_DECAY = 1, //aka 'Braking'
};
//--------------------------------------------------
// Constants
//--------------------------------------------------
public:
static const uint16_t DEFAULT_PWM_FREQUENCY = 25000; // Chose 25KHz because it is outside of hearing
// and divides nicely into the RP2040's 125MHz PWM frequency
static const DecayMode DEFAULT_DECAY_MODE = SLOW_DECAY;
//--------------------------------------------------
// Variables
//--------------------------------------------------
private:
pin_pair pins;
pin_pair motor_pins;
MotorState state;
pwm_config pwm_cfg;
uint16_t pwm_period;
float pwm_frequency = DEFAULT_PWM_FREQUENCY;
DecayMode motor_decay_mode = DEFAULT_DECAY_MODE;
float motor_speed = 0.0f;
float pwm_frequency;
DecayMode motor_mode;
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
public:
Motor(const pin_pair &pins, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE);
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();
@ -55,21 +36,49 @@ namespace motor {
public:
bool init();
float get_speed();
void set_speed(float speed);
// For print access in micropython
pin_pair pins() const;
float get_frequency();
bool set_frequency(float freq);
void enable();
void disable();
bool is_enabled() const;
DecayMode get_decay_mode();
void set_decay_mode(DecayMode mode);
float duty() const;
void duty(float duty);
float speed() const;
void speed(float speed);
float frequency() const;
bool frequency(float freq);
//--------------------------------------------------
void stop();
void disable();
void coast();
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);
void to_percent(float in, float in_min, float in_max, float speed_min, float speed_max);
//--------------------------------------------------
Direction direction() const;
void direction(Direction direction);
float speed_scale() const;
void speed_scale(float speed_scale);
float deadzone() const;
void deadzone(float deadzone);
DecayMode decay_mode();
void decay_mode(DecayMode mode);
//--------------------------------------------------
private:
void update_pwm();
void apply_duty(float duty, DecayMode mode);
};
}
}

15
drivers/motor/motor2.cmake
Wyświetl plik

@ -1,15 +0,0 @@
set(DRIVER_NAME motor2)
add_library(${DRIVER_NAME} INTERFACE)
target_sources(${DRIVER_NAME} INTERFACE
${CMAKE_CURRENT_LIST_DIR}/motor2.cpp
${CMAKE_CURRENT_LIST_DIR}/motor_state.cpp
)
target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})
target_link_libraries(${DRIVER_NAME} INTERFACE
pico_stdlib
hardware_pwm
pwm
)

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

@ -1,223 +0,0 @@
#include "motor2.hpp"
#include "hardware/clocks.h"
#include "pwm.hpp"
#include "math.h"
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_mode(mode) {
}
Motor2::~Motor2() {
gpio_set_function(motor_pins.positive, GPIO_FUNC_NULL);
gpio_set_function(motor_pins.negative, GPIO_FUNC_NULL);
}
bool Motor2::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 Motor2::pins() const {
return motor_pins;
}
void Motor2::enable() {
apply_duty(state.enable_with_return(), motor_mode);
}
void Motor2::disable() {
apply_duty(state.disable_with_return(), motor_mode);
}
bool Motor2::is_enabled() const {
return state.is_enabled();
}
float Motor2::duty() const {
return state.get_duty();
}
void Motor2::duty(float duty) {
apply_duty(state.set_duty_with_return(duty), motor_mode);
}
float Motor2::speed() const {
return state.get_speed();
}
void Motor2::speed(float speed) {
apply_duty(state.set_speed_with_return(speed), motor_mode);
}
float Motor2::frequency() const {
return pwm_frequency;
}
bool Motor2::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;
}
}
return success;
}
void Motor2::stop() {
apply_duty(state.stop_with_return(), motor_mode);
}
void Motor2::coast() {
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(), motor_mode);
}
void Motor2::full_positive() {
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_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_mode);
}
Direction Motor2::direction() const {
return state.get_direction();
}
void Motor2::direction(Direction direction) {
state.set_direction(direction);
}
float Motor2::speed_scale() const {
return state.get_speed_scale();
}
void Motor2::speed_scale(float speed_scale) {
state.set_speed_scale(speed_scale);
}
float Motor2::deadzone() const {
return state.get_deadzone();
}
void Motor2::deadzone(float deadzone) {
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) {
if(isfinite(duty)) {
int32_t signed_level = MotorState::duty_to_level(duty, pwm_period);
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);
}
else {
pwm_set_gpio_level(motor_pins.positive, pwm_period + signed_level);
pwm_set_gpio_level(motor_pins.negative, pwm_period);
}
break;
case FAST_DECAY: //aka 'Coasting'
default:
if(signed_level >= 0) {
pwm_set_gpio_level(motor_pins.positive, signed_level);
pwm_set_gpio_level(motor_pins.negative, 0);
}
else {
pwm_set_gpio_level(motor_pins.positive, 0);
pwm_set_gpio_level(motor_pins.negative, 0 - signed_level);
}
break;
}
}
else {
pwm_set_gpio_level(motor_pins.positive, 0);
pwm_set_gpio_level(motor_pins.negative, 0);
}
}
};

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

@ -1,84 +0,0 @@
#pragma once
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "common/pimoroni_common.hpp"
#include "motor_state.hpp"
using namespace pimoroni;
namespace motor {
class Motor2 {
//--------------------------------------------------
// Variables
//--------------------------------------------------
private:
pin_pair motor_pins;
MotorState state;
pwm_config pwm_cfg;
uint16_t pwm_period;
float pwm_frequency;
DecayMode motor_mode;
//--------------------------------------------------
// Constructors/Destructor
//--------------------------------------------------
public:
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();
//--------------------------------------------------
// Methods
//--------------------------------------------------
public:
bool init();
// For print access in micropython
pin_pair pins() const;
void enable();
void disable();
bool is_enabled() const;
float duty() const;
void duty(float duty);
float speed() const;
void speed(float speed);
float frequency() const;
bool frequency(float freq);
//--------------------------------------------------
void stop();
void coast();
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);
void to_percent(float in, float in_min, float in_max, float speed_min, float speed_max);
//--------------------------------------------------
Direction direction() const;
void direction(Direction direction);
float speed_scale() const;
void speed_scale(float speed_scale);
float deadzone() const;
void deadzone(float deadzone);
DecayMode decay_mode();
void decay_mode(DecayMode mode);
//--------------------------------------------------
private:
void apply_duty(float duty, DecayMode mode);
};
}

2
examples/motor2040/motor2040_single_motor.cpp
Wyświetl plik

@ -22,7 +22,7 @@ constexpr float SWEEP_EXTENT = 90.0f;
// Create a motor on pin 0 and 1
Motor2 m = Motor2(motor2040::MOTOR_A);
Motor m = Motor(motor2040::MOTOR_A);
int main() {

4
examples/pico_motor_shim/balance/demo.cpp
Wyświetl plik

@ -103,8 +103,8 @@ int main() {
printf("Y: %f, Z: %f, AngErr: %f\n", y, z, angle_error);
float output = angle_error * PROPORTIONAL; //No need to clamp this value as set_speed does this internally
motor_1.set_speed(output);
motor_2.set_speed(-output);
motor_1.speed(output);
motor_2.speed(-output);
sleep_ms(1);
}

12
examples/pico_motor_shim/sequence/demo.cpp
Wyświetl plik

@ -65,8 +65,8 @@ bool accelerate_over(float left_speed, float right_speed, uint32_t duration_ms)
uint32_t ellapsed = 0;
//Get the current motor speeds
float last_left = motor_1.get_speed();
float last_right = motor_2.get_speed();
float last_left = motor_1.speed();
float last_right = motor_2.speed();
//Loops until the duration has elapsed, checking the button state every millisecond, and updating motor speeds
while(ellapsed <= duration_ms) {
@ -75,16 +75,16 @@ bool accelerate_over(float left_speed, float right_speed, uint32_t duration_ms)
//Calculate and set the new motor speeds
float percentage = (float)ellapsed / (float)duration_ms;
motor_1.set_speed(((left_speed - last_left) * percentage) + last_left);
motor_2.set_speed(((right_speed - last_right) * percentage) + last_right);
motor_1.speed(((left_speed - last_left) * percentage) + last_left);
motor_2.speed(((right_speed - last_right) * percentage) + last_right);
sleep_ms(1);
ellapsed = millis() - start_time;
}
//Set the final motor speeds as loop may not reach 100%
motor_1.set_speed(left_speed);
motor_2.set_speed(right_speed);
motor_1.speed(left_speed);
motor_2.speed(right_speed);
return true;
}

22
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, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY);
Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::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);
#else
Motor motor_1(pico_motor_shim::MOTOR_1, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY);
Motor motor_2(pico_motor_shim::MOTOR_2, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY);
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);
#endif
static bool button_toggle = false;
@ -83,26 +83,26 @@ int main() {
//Play the song
for(uint i = 0; i < SONG_LENGTH && check_button_toggle(); i++) {
if(motor_1.set_frequency(SONG[i]) && motor_2.set_frequency(SONG[i])) {
if(motor_1.frequency(SONG[i]) && motor_2.frequency(SONG[i])) {
#ifdef STATIONARY_PLAYBACK
//Set the motors to 50% duty cycle to play the note, but alternate
//the direction so that the motor does not actually spin
uint t = 0;
while(t < NOTE_DURATION_MS * 1000) {
motor_1.set_speed(0.5f);
motor_2.set_speed(0.5f);
motor_1.duty(0.5f);
motor_2.duty(0.5f);
sleep_us(STATIONARY_TOGGLE_US);
t += STATIONARY_TOGGLE_US;
motor_1.set_speed(-0.5f);
motor_2.set_speed(-0.5f);
motor_1.duty(-0.5f);
motor_2.duty(-0.5f);
sleep_us(STATIONARY_TOGGLE_US);
t += STATIONARY_TOGGLE_US;
}
#else
//Set the motors to 50% duty cycle to play the note
motor_1.set_speed(0.5f);
motor_2.set_speed(0.5f);
motor_1.duty(0.5f);
motor_2.duty(0.5f);
sleep_ms(NOTE_DURATION_MS);
#endif
}

2
libraries/motor2040/motor2040.cmake
Wyświetl plik

@ -3,4 +3,4 @@ add_library(motor2040 INTERFACE)
target_include_directories(motor2040 INTERFACE ${CMAKE_CURRENT_LIST_DIR})
# Pull in pico libraries that we need
target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor2 motor_cluster)
target_link_libraries(motor2040 INTERFACE pico_stdlib plasma motor motor_cluster)

2
libraries/motor2040/motor2040.hpp
Wyświetl plik

@ -2,7 +2,7 @@
#include "pico/stdlib.h"
#include "ws2812.hpp"
#include "motor2.hpp"
#include "motor.hpp"
#include "motor_cluster.hpp"
namespace motor {

2
libraries/pico_motor_shim/pico_motor_shim.hpp
Wyświetl plik

@ -1,7 +1,7 @@
#pragma once
#include "pico/stdlib.h"
#include "motor2.hpp"
#include "motor.hpp"
namespace motor {
namespace pico_motor_shim {

2
micropython/modules/motor/micropython.cmake
Wyświetl plik

@ -7,7 +7,7 @@ target_sources(usermod_${MOD_NAME} INTERFACE
${CMAKE_CURRENT_LIST_DIR}/${MOD_NAME}.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/pwm/pwm_cluster.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor2.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor_cluster.cpp
${CMAKE_CURRENT_LIST_DIR}/../../../drivers/motor/motor_state.cpp
)

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

@ -1,4 +1,4 @@
#include "drivers/motor/motor2.hpp"
#include "drivers/motor/motor.hpp"
#include "drivers/motor/motor_cluster.hpp"
#include "common/pimoroni_common.hpp"
#include <cstdio>
@ -19,7 +19,7 @@ extern "C" {
/***** Variables Struct *****/
typedef struct _Motor_obj_t {
mp_obj_base_t base;
Motor2* motor;
Motor* motor;
} _Motor_obj_t;
@ -147,7 +147,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 Motor2(pins, (Direction)direction, speed_scale, deadzone, freq, (DecayMode)mode);
self->motor = new Motor(pins, (Direction)direction, speed_scale, deadzone, freq, (DecayMode)mode);
self->motor->init();
return MP_OBJ_FROM_PTR(self);