diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index 32c6169c..68549297 100644 --- a/drivers/CMakeLists.txt +++ b/drivers/CMakeLists.txt @@ -29,3 +29,4 @@ add_subdirectory(hub75) add_subdirectory(uc8151) add_subdirectory(pwm) add_subdirectory(servo) +add_subdirectory(motor) diff --git a/drivers/motor/CMakeLists.txt b/drivers/motor/CMakeLists.txt new file mode 100644 index 00000000..086269fe --- /dev/null +++ b/drivers/motor/CMakeLists.txt @@ -0,0 +1 @@ +include(motor.cmake) \ No newline at end of file diff --git a/drivers/motor/motor.cmake b/drivers/motor/motor.cmake new file mode 100644 index 00000000..7f498ab6 --- /dev/null +++ b/drivers/motor/motor.cmake @@ -0,0 +1,14 @@ +set(DRIVER_NAME motor) +add_library(${DRIVER_NAME} INTERFACE) + +target_sources(${DRIVER_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/motor.cpp +) + +target_include_directories(${DRIVER_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +target_link_libraries(${DRIVER_NAME} INTERFACE + pico_stdlib + hardware_pwm + pwm + ) \ No newline at end of file diff --git a/drivers/motor/motor.cpp b/drivers/motor/motor.cpp new file mode 100644 index 00000000..50dbe95b --- /dev/null +++ b/drivers/motor/motor.cpp @@ -0,0 +1,147 @@ +#include "motor.hpp" +#include "pwm.hpp" + +namespace pimoroni { + Motor::Motor(uint pin_pos, uint pin_neg, float freq, DecayMode mode) + : pin_pos(pin_pos), pin_neg(pin_neg), pwm_frequency(freq), motor_decay_mode(mode) { + } + + Motor::~Motor() { + gpio_set_function(pin_pos, GPIO_FUNC_NULL); + gpio_set_function(pin_neg, 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, period - 1); + + //Apply the divider + pwm_config_set_clkdiv(&pwm_cfg, (float)div16 / 16.0f); + + pwm_init(pwm_gpio_to_slice_num(pin_pos), &pwm_cfg, true); + gpio_set_function(pin_pos, GPIO_FUNC_PWM); + + pwm_init(pwm_gpio_to_slice_num(pin_neg), &pwm_cfg, true); + gpio_set_function(pin_neg, GPIO_FUNC_PWM); + update_pwm(); + + success = true; + } + return success; + } + + float Motor::get_speed() { + return motor_speed; + } + + 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(pin_pos); + uint neg_num = pwm_gpio_to_slice_num(pin_neg); + + //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::disable() { + motor_speed = 0.0f; + pwm_set_gpio_level(pin_pos, 0); + pwm_set_gpio_level(pin_neg, 0); + } + + void Motor::update_pwm() { + int32_t signed_duty_cycle = (int32_t)(motor_speed * (float)pwm_period); + + switch(motor_decay_mode) { + case SLOW_DECAY: //aka 'Braking' + if(signed_duty_cycle >= 0) { + pwm_set_gpio_level(pin_pos, pwm_period); + pwm_set_gpio_level(pin_neg, pwm_period - signed_duty_cycle); + } + else { + pwm_set_gpio_level(pin_pos, pwm_period + signed_duty_cycle); + pwm_set_gpio_level(pin_neg, pwm_period); + } + break; + + case FAST_DECAY: //aka 'Coasting' + default: + if(signed_duty_cycle >= 0) { + pwm_set_gpio_level(pin_pos, signed_duty_cycle); + pwm_set_gpio_level(pin_neg, 0); + } + else { + pwm_set_gpio_level(pin_pos, 0); + pwm_set_gpio_level(pin_neg, 0 - signed_duty_cycle); + } + break; + } + } +}; \ No newline at end of file diff --git a/drivers/motor/motor.hpp b/drivers/motor/motor.hpp new file mode 100644 index 00000000..eb3d6922 --- /dev/null +++ b/drivers/motor/motor.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include "pico/stdlib.h" +#include "hardware/pwm.h" + +namespace pimoroni { + + 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: + uint pin_pos; + uint pin_neg; + 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; + + + //-------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------- + public: + Motor(uint pin_pos, uint pin_neg, float freq = DEFAULT_PWM_FREQUENCY, DecayMode mode = DEFAULT_DECAY_MODE); + ~Motor(); + + + //-------------------------------------------------- + // Methods + //-------------------------------------------------- + public: + bool init(); + + float get_speed(); + void set_speed(float speed); + + float get_frequency(); + bool set_frequency(float freq); + + DecayMode get_decay_mode(); + void set_decay_mode(DecayMode mode); + + void stop(); + void disable(); + + //-------------------------------------------------- + private: + void update_pwm(); + }; + +} diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e3293cb9..4cbb1eaf 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -30,6 +30,7 @@ add_subdirectory(pico_scroll) add_subdirectory(pico_enc_explorer) add_subdirectory(pico_explorer) add_subdirectory(pico_pot_explorer) +add_subdirectory(pico_motor_shim) add_subdirectory(pico_rgb_keypad) add_subdirectory(pico_rtc_display) add_subdirectory(pico_tof_display) diff --git a/examples/pico_motor_shim/CMakeLists.txt b/examples/pico_motor_shim/CMakeLists.txt new file mode 100644 index 00000000..91e9e29f --- /dev/null +++ b/examples/pico_motor_shim/CMakeLists.txt @@ -0,0 +1,3 @@ +add_subdirectory(balance) +add_subdirectory(sequence) +add_subdirectory(song) \ No newline at end of file diff --git a/examples/pico_motor_shim/balance/CMakeLists.txt b/examples/pico_motor_shim/balance/CMakeLists.txt new file mode 100644 index 00000000..ee58efe4 --- /dev/null +++ b/examples/pico_motor_shim/balance/CMakeLists.txt @@ -0,0 +1,16 @@ +set(OUTPUT_NAME motor_shim_balance) + +add_executable( + ${OUTPUT_NAME} + demo.cpp +) + +# enable usb output, disable uart output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 1) + +# Pull in pico libraries that we need +target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button breakout_msa301) + +# create map/bin/hex file etc. +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/pico_motor_shim/balance/demo.cpp b/examples/pico_motor_shim/balance/demo.cpp new file mode 100644 index 00000000..8c964d72 --- /dev/null +++ b/examples/pico_motor_shim/balance/demo.cpp @@ -0,0 +1,118 @@ +#include +#include "pico_motor_shim.hpp" + +#include "common/pimoroni_common.hpp" +#include "motor.hpp" +#include "button.hpp" +#include "breakout_msa301.hpp" +#include + +/* +A very basic balancing robot implementation, using an MSA301 to give accelerating values that are passed to the motors using proportional control. +Press "A" to start and stop the balancer +*/ + +using namespace pimoroni; + +static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 +static constexpr float Z_BIAS_CORRECTION = 0.5f; //A magic number that seems to correct the MSA301's Z bias +static constexpr float PROPORTIONAL = 0.03f; + + + +Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); + +Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); + +I2C i2c(BOARD::BREAKOUT_GARDEN); +BreakoutMSA301 msa301(&i2c); + +static bool button_toggle = false; +static float target_angle = 0.0f; + +/** + * Checks if the button has been pressed, toggling a value that is also returned. + */ +bool check_button_toggle() { + bool button_pressed = button_a.read(); + if(button_pressed) { + button_toggle = !button_toggle; + } + return button_toggle; +} + +/** + * Takes and angle and wraps it around so that it stays within a -180 to +180 degree range. + * + * Note, it will only work for values between -540 and +540 degrees. + * This can be resolved by changing the 'if's into 'while's, but for most uses it is unnecessary + */ +float wrap_angle(float angle) { + if(angle <= -180.0f) + angle += 360.0f; + + if(angle > 180.0f) + angle -= 360.0f; + + return angle; +} + +/** + * The entry point of the program. + */ +int main() { + stdio_init_all(); + + //Initialise the LED. We use this to indicate that the sequence is running. + gpio_init(PICO_DEFAULT_LED_PIN); + gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); + for(int i = 0; i < 20; i++) { + gpio_put(PICO_DEFAULT_LED_PIN, true); + sleep_ms(250); + gpio_put(PICO_DEFAULT_LED_PIN, false); + sleep_ms(250); + } + + //Initialise the two motors + if(!motor_1.init() || !motor_2.init()) { + printf("Cannot initialise motors. Check the provided parameters\n"); + return 0; + } + + if(!msa301.init()) { + printf("Cannot initialise msa301. Check that it is connected\n"); + return 0; + } + + printf("Ready\n"); + + while(true) { + //Turn the Pico's LED on to show that the sequence has started + gpio_put(PICO_DEFAULT_LED_PIN, true); + sleep_ms(50); + + //Has the user has pressed the button to start the sequence + while(check_button_toggle()) { + float y = msa301.get_y_axis(); + float z = msa301.get_z_axis() + Z_BIAS_CORRECTION; + + float current_angle = (atan2(z, -y) * 180.0f) / M_PI; + float angle_error = wrap_angle(target_angle - current_angle); + 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); + + sleep_ms(1); + } + + //The sequence loop has ended, so turn off the Pico's LED and disable the motors + gpio_put(PICO_DEFAULT_LED_PIN, false); + motor_1.disable(); + motor_2.disable(); + sleep_ms(50); + } + return 0; +} diff --git a/examples/pico_motor_shim/sequence/CMakeLists.txt b/examples/pico_motor_shim/sequence/CMakeLists.txt new file mode 100644 index 00000000..eafeebe8 --- /dev/null +++ b/examples/pico_motor_shim/sequence/CMakeLists.txt @@ -0,0 +1,16 @@ +set(OUTPUT_NAME motor_shim_sequence) + +add_executable( + ${OUTPUT_NAME} + demo.cpp +) + +# enable usb output, disable uart output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 1) + +# Pull in pico libraries that we need +target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button) + +# create map/bin/hex file etc. +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/pico_motor_shim/sequence/demo.cpp b/examples/pico_motor_shim/sequence/demo.cpp new file mode 100644 index 00000000..0b9767a7 --- /dev/null +++ b/examples/pico_motor_shim/sequence/demo.cpp @@ -0,0 +1,169 @@ +#include +#include "pico_motor_shim.hpp" + +#include "common/pimoroni_common.hpp" +#include "motor.hpp" +#include "button.hpp" + +/* +Program showing how the two motors of the Pico Motor Shim can be perform a sequence of movements. +Press "A" to start and stop the movement sequence +*/ + +using namespace pimoroni; + +static constexpr float TOP_SPEED = 1.0f; //A value between 0 and 1 +static const uint32_t ACCELERATE_TIME_MS = 2000; +static const uint32_t WAIT_TIME_MS = 1000; +static const uint32_t STOP_TIME_MS = 1000; + + +Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); + +Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); +Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG);//, Motor::DEFAULT_PWM_FREQUENCY, Motor::DEFAULT_DECAY_MODE); + +static bool button_toggle = false; + +/** + * Checks if the button has been pressed, toggling a value that is also returned. + */ +bool check_button_toggle() { + bool button_pressed = button_a.read(); + if(button_pressed) { + button_toggle = !button_toggle; + } + return button_toggle; +} + +/** + * Waits for a given amount of time (in milliseconds). + * Exits early if the user presses the button to stop the sequence, returning false. + */ +bool wait_for(uint32_t duration_ms) { + uint32_t start_time = millis(); + uint32_t ellapsed = 0; + + //Loops until the duration has elapsed, checking the button state every millisecond + while(ellapsed < duration_ms) { + if(!check_button_toggle()) + return false; + + sleep_ms(1); + ellapsed = millis() - start_time; + } + return true; +} + +/** + * Accelerate/Decelerate the motors from their current speed to the target speed over the given amount of time (in milliseconds). + * Exits early if the user presses the button to stop the sequence, returning false. + */ +bool accelerate_over(float left_speed, float right_speed, uint32_t duration_ms) { + uint32_t start_time = millis(); + uint32_t ellapsed = 0; + + //Get the current motor speeds + float last_left = motor_1.get_speed(); + float last_right = motor_2.get_speed(); + + //Loops until the duration has elapsed, checking the button state every millisecond, and updating motor speeds + while(ellapsed <= duration_ms) { + if(!check_button_toggle()) + return false; + + //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); + + 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); + + return true; +} + +/** + * The function that performs the driving sequence. + * Exits early if the user presses the button to stop the sequence, returning false. + */ +bool sequence() { + printf("accelerate forward\n"); + if(!accelerate_over(-TOP_SPEED, TOP_SPEED, ACCELERATE_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("driving forward\n"); + if(!wait_for(WAIT_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("deccelerate forward\n"); + if(!accelerate_over(0.0f, 0.0f, ACCELERATE_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("stop\n"); + motor_1.stop(); + motor_2.stop(); + if(!wait_for(STOP_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("accelerate turn left\n"); + if(!accelerate_over(TOP_SPEED * 0.5f, TOP_SPEED * 0.5f, ACCELERATE_TIME_MS * 0.5f)) + return false; //Early exit if the button was toggled + + printf("turning left\n"); + if(!wait_for(WAIT_TIME_MS)) + return false; //Early exit if the button was toggled + + printf("deccelerate turn left\n"); + if(!accelerate_over(0.0f, 0.0f, ACCELERATE_TIME_MS * 0.5f)) + return false; //Early exit if the button was toggled + + printf("stop\n"); + motor_1.stop(); + motor_2.stop(); + if(!wait_for(STOP_TIME_MS)) + return false; //Early exit if the button was toggled + + //Signal that the sequence completed successfully + return true; +} + +/** + * The entry point of the program. + */ +int main() { + stdio_init_all(); + + //Initialise the LED. We use this to indicate that the sequence is running. + gpio_init(PICO_DEFAULT_LED_PIN); + gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); + + //Initialise the two motors + if(!motor_1.init() || !motor_2.init()) { + printf("Cannot initialise motors. Check the provided parameters\n"); + return 0; + } + + while(true) { + //Has the user has pressed the button to start the sequence + if(check_button_toggle()) { + + //Turn the Pico's LED on to show that the sequence has started + gpio_put(PICO_DEFAULT_LED_PIN, true); + + //Run the sequence in a perpetual loop, exiting early if the button is pressed again + while(sequence()); + } + + //The sequence loop has ended, so turn off the Pico's LED and disable the motors + gpio_put(PICO_DEFAULT_LED_PIN, false); + motor_1.disable(); + motor_2.disable(); + } + return 0; +} diff --git a/examples/pico_motor_shim/song/CMakeLists.txt b/examples/pico_motor_shim/song/CMakeLists.txt new file mode 100644 index 00000000..b5adb316 --- /dev/null +++ b/examples/pico_motor_shim/song/CMakeLists.txt @@ -0,0 +1,16 @@ +set(OUTPUT_NAME motor_shim_song) + +add_executable( + ${OUTPUT_NAME} + demo.cpp +) + +# enable usb output, disable uart output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 1) + +# Pull in pico libraries that we need +target_link_libraries(${OUTPUT_NAME} pico_stdlib pico_motor_shim motor button breakout_msa301) + +# create map/bin/hex file etc. +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/song/demo.cpp new file mode 100644 index 00000000..75d920db --- /dev/null +++ b/examples/pico_motor_shim/song/demo.cpp @@ -0,0 +1,126 @@ +#include +#include "pico_motor_shim.hpp" + +#include "common/pimoroni_common.hpp" +#include "motor.hpp" +#include "button.hpp" + +/* +Play a song using a motor! Works by setting the PWM duty cycle to 50% and changing the frequency on the fly. +Plug a motor into connector 1, and press "A" to start the song playing (does not loop). Press the button again will stop the song early. +*/ + +using namespace pimoroni; + +// List frequencies (in hz) to play in sequence here. Use zero for when silence or a pause is wanted +// Song from PicoExplorer noise.py +constexpr float SONG[] = {1397, 1397, 1319, 1397, 698, 0, 698, 0, 1047, 932, + 880, 1047, 1397, 0, 1397, 0, 1568, 1480, 1568, 784, + 0, 784, 0, 1568, 1397, 1319, 1175, 1047, 0, 1047, + 0, 1175, 1319, 1397, 1319, 1175, 1047, 1175, 1047, 932, + 880, 932, 880, 784, 698, 784, 698, 659, 587, 523, + 587, 659, 698, 784, 932, 880, 784, 880, 698, 0, 698}; +constexpr uint SONG_LENGTH = sizeof(SONG) / sizeof(float); + +//The time (in milliseconds) to play each note for. Change this to make the song play faster or slower +static const uint NOTE_DURATION_MS = 150; + +//Uncomment this lineto have the song be played without the motor turning +//Note, this will affect the audio quality of the sound produced +//#define STATIONARY_PLAYBACK + +//The time (in microseconds) between each direction switch of the motor when using STATIONARY_PLAYBACK +static const uint STATIONARY_TOGGLE_US = 2000; + +//Uncomment this line to use the fast decay (coasting) motor mode. +//This seems to produce a louder sound with STATIONARY_PLAYBACK enabled, but will make movement poorer when STATIONARY_PLAYBACK is disabled +//#define USE_FAST_DECAY + + +Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); +#ifdef USE_FAST_DECAY + Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::FAST_DECAY); +#else + Motor motor_1(pico_motor_shim::MOTOR_1_POS, pico_motor_shim::MOTOR_1_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); + Motor motor_2(pico_motor_shim::MOTOR_2_POS, pico_motor_shim::MOTOR_2_NEG, Motor::DEFAULT_PWM_FREQUENCY, Motor::SLOW_DECAY); +#endif + +static bool button_toggle = false; + +/** + * Checks if the button has been pressed, toggling a value that is also returned. + */ +bool check_button_toggle() { + bool button_pressed = button_a.read(); + if(button_pressed) { + button_toggle = !button_toggle; + } + return button_toggle; +} + +/** + * The entry point of the program. + */ +int main() { + stdio_init_all(); + + //Initialise the LED. We use this to indicate that the sequence is running. + gpio_init(PICO_DEFAULT_LED_PIN); + gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); + + //Initialise the motor + if(!motor_1.init() || !motor_2.init()) { + printf("Cannot initialise the motors. Check the provided parameters\n"); + return 0; + } + + while(true) { + if(check_button_toggle()) { + //Turn the Pico's LED on to show that the song has started + gpio_put(PICO_DEFAULT_LED_PIN, true); + + //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])) { + #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); + sleep_us(STATIONARY_TOGGLE_US); + t += STATIONARY_TOGGLE_US; + + motor_1.set_speed(-0.5f); + motor_2.set_speed(-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); + sleep_ms(NOTE_DURATION_MS); + #endif + } + else { + //The frequency was invalid, so we are treating that to mean this is a pause note + motor_1.stop(); + motor_2.stop(); + sleep_ms(NOTE_DURATION_MS); + } + } + button_toggle = false; + + //The song has finished, so turn off the Pico's LED and disable the motors + gpio_put(PICO_DEFAULT_LED_PIN, false); + motor_1.disable(); + motor_2.disable(); + } + + sleep_ms(10); + } + return 0; +} diff --git a/libraries/CMakeLists.txt b/libraries/CMakeLists.txt index ca810b56..e606e4fd 100644 --- a/libraries/CMakeLists.txt +++ b/libraries/CMakeLists.txt @@ -23,6 +23,7 @@ add_subdirectory(pico_display_2) add_subdirectory(pico_unicorn) add_subdirectory(pico_scroll) add_subdirectory(pico_explorer) +add_subdirectory(pico_motor_shim) add_subdirectory(pico_rgb_keypad) add_subdirectory(pico_wireless) add_subdirectory(plasma2040) diff --git a/libraries/pico_motor_shim/CMakeLists.txt b/libraries/pico_motor_shim/CMakeLists.txt new file mode 100644 index 00000000..f1101b9a --- /dev/null +++ b/libraries/pico_motor_shim/CMakeLists.txt @@ -0,0 +1 @@ +include(pico_motor_shim.cmake) \ No newline at end of file diff --git a/libraries/pico_motor_shim/pico_motor_shim.cmake b/libraries/pico_motor_shim/pico_motor_shim.cmake new file mode 100644 index 00000000..d28b6c2f --- /dev/null +++ b/libraries/pico_motor_shim/pico_motor_shim.cmake @@ -0,0 +1,6 @@ +add_library(pico_motor_shim INTERFACE) + +target_include_directories(pico_motor_shim INTERFACE ${CMAKE_CURRENT_LIST_DIR}) + +# Pull in pico libraries that we need +target_link_libraries(pico_motor_shim INTERFACE pico_stdlib) \ No newline at end of file diff --git a/libraries/pico_motor_shim/pico_motor_shim.hpp b/libraries/pico_motor_shim/pico_motor_shim.hpp new file mode 100644 index 00000000..e93d6bf5 --- /dev/null +++ b/libraries/pico_motor_shim/pico_motor_shim.hpp @@ -0,0 +1,12 @@ +#pragma once +#include "pico/stdlib.h" + +namespace pico_motor_shim { + const uint8_t BUTTON_A = 2; + + const uint MOTOR_1_POS = 6; + const uint MOTOR_1_NEG = 7; + + const uint MOTOR_2_POS = 27; + const uint MOTOR_2_NEG = 26; +} \ No newline at end of file diff --git a/micropython/modules/micropython.cmake b/micropython/modules/micropython.cmake index 930d15fe..f9e6314b 100644 --- a/micropython/modules/micropython.cmake +++ b/micropython/modules/micropython.cmake @@ -36,6 +36,7 @@ include(pico_unicorn/micropython) include(pico_display/micropython) include(pico_display_2/micropython) include(pico_explorer/micropython) +include(pico_motor_shim/micropython) include(pico_wireless/micropython) include(plasma/micropython) diff --git a/micropython/modules/pico_motor_shim/micropython.cmake b/micropython/modules/pico_motor_shim/micropython.cmake new file mode 100644 index 00000000..f444ad49 --- /dev/null +++ b/micropython/modules/pico_motor_shim/micropython.cmake @@ -0,0 +1,18 @@ +set(MOD_NAME picomotorshim) +string(TOUPPER ${MOD_NAME} MOD_NAME_UPPER) +add_library(usermod_${MOD_NAME} INTERFACE) + +target_sources(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR}/pico_motor_shim.c +) + +target_include_directories(usermod_${MOD_NAME} INTERFACE + ${CMAKE_CURRENT_LIST_DIR} + ${CMAKE_CURRENT_LIST_DIR}/../../../libraries/pico_motor_shim/ +) + +target_compile_definitions(usermod_${MOD_NAME} INTERFACE + -DMODULE_${MOD_NAME_UPPER}_ENABLED=1 +) + +target_link_libraries(usermod INTERFACE usermod_${MOD_NAME}) \ No newline at end of file diff --git a/micropython/modules/pico_motor_shim/pico_motor_shim.c b/micropython/modules/pico_motor_shim/pico_motor_shim.c new file mode 100644 index 00000000..d860bbdd --- /dev/null +++ b/micropython/modules/pico_motor_shim/pico_motor_shim.c @@ -0,0 +1,40 @@ +#include "pico_motor_shim.h" + +/***** Constants *****/ +enum pins +{ + BUTTON_A = 2, + + MOTOR_1_POS = 6, + MOTOR_1_NEG = 7, + + MOTOR_2_POS = 27, + MOTOR_2_NEG = 26, +}; + + +//////////////////////////////////////////////////////////////////////////////////////////////////// +// picomotorshim Module +//////////////////////////////////////////////////////////////////////////////////////////////////// + +/***** Globals Table *****/ +STATIC const mp_map_elem_t picomotorshim_globals_table[] = { + { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_picomotorshim) }, + { MP_ROM_QSTR(MP_QSTR_PIN_BUTTON_A), MP_ROM_INT(BUTTON_A) }, + { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_1_POS), MP_ROM_INT(MOTOR_1_POS) }, + { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_1_NEG), MP_ROM_INT(MOTOR_1_NEG) }, + { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_2_POS), MP_ROM_INT(MOTOR_2_POS) }, + { MP_ROM_QSTR(MP_QSTR_PIN_MOTOR_2_NEG), MP_ROM_INT(MOTOR_2_NEG) }, +}; +STATIC MP_DEFINE_CONST_DICT(mp_module_picomotorshim_globals, picomotorshim_globals_table); + +/***** Module Definition *****/ +const mp_obj_module_t picomotorshim_user_cmodule = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t*)&mp_module_picomotorshim_globals, +}; + +//////////////////////////////////////////////////////////////////////////////////////////////////// +MP_REGISTER_MODULE(MP_QSTR_picomotorshim, picomotorshim_user_cmodule, MODULE_PICOMOTORSHIM_ENABLED); +//////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////// \ No newline at end of file diff --git a/micropython/modules/pico_motor_shim/pico_motor_shim.h b/micropython/modules/pico_motor_shim/pico_motor_shim.h new file mode 100644 index 00000000..707d6769 --- /dev/null +++ b/micropython/modules/pico_motor_shim/pico_motor_shim.h @@ -0,0 +1,3 @@ +// Include MicroPython API. +#include "py/runtime.h" +#include "py/objstr.h" \ No newline at end of file diff --git a/micropython/modules_py/pimoroni.py b/micropython/modules_py/pimoroni.py index bc67d0f7..d051833f 100644 --- a/micropython/modules_py/pimoroni.py +++ b/micropython/modules_py/pimoroni.py @@ -141,3 +141,76 @@ class RGBLED: self.led_r.duty_u16(int((r * 65535) / 255)) self.led_g.duty_u16(int((g * 65535) / 255)) self.led_b.duty_u16(int((b * 65535) / 255)) + + +class Motor: + FAST_DECAY = 0 # Recirculation current fast decay mode (coasting) + SLOW_DECAY = 1 # Recirculation current slow decay mode (braking) + + def __init__(self, pos, neg, freq=25000, decay_mode=SLOW_DECAY): + self.speed = 0.0 + self.freq = freq + if decay_mode in (self.FAST_DECAY, self.SLOW_DECAY): + self.decay_mode = decay_mode + else: + raise ValueError("Decay mode value must be either Motor.FAST_DECAY or Motor.SLOW_DECAY") + + self.pos_pwm = PWM(Pin(pos)) + self.pos_pwm.freq(freq) + self.neg_pwm = PWM(Pin(neg)) + self.neg_pwm.freq(freq) + + def get_speed(self): + return self.speed + + def set_speed(self, speed): + if speed > 1.0 or speed < -1.0: + raise ValueError("Speed must be between -1.0 and +1.0") + self.speed = speed + self._update_pwm() + + def get_frequency(self): + return self.freq + + def set_frequency(self, freq): + self.pos_pwm.freq(freq) + self.neg_pwm.freq(freq) + self._update_pwm() + + def get_decay_mode(self): + return self.decay_mode + + def set_decay_mode(self, mode): + if mode in (self.FAST_DECAY, self.SLOW_DECAY): + self.decay_mode = mode + self._update_pwm() + else: + raise ValueError("Decay mode value must be either Motor.FAST_DECAY or Motor.SLOW_DECAY") + + def stop(self): + self.speed = 0.0 + self._update_pwm() + + def disable(self): + self.speed = 0.0 + self.pos_pwm.duty_u16(0) + self.neg_pwm.duty_u16(0) + + def _update_pwm(self): + signed_duty_cycle = int(self.speed * 0xFFFF) + + if self.decay_mode is self.SLOW_DECAY: # aka 'Braking' + if signed_duty_cycle >= 0: + self.pos_pwm.duty_u16(0xFFFF) + self.neg_pwm.duty_u16(0xFFFF - signed_duty_cycle) + else: + self.pos_pwm.duty_u16(0xFFFF + signed_duty_cycle) + self.neg_pwm.duty_u16(0xFFFF) + + else: # aka 'Coasting' + if signed_duty_cycle >= 0: + self.pos_pwm.duty_u16(signed_duty_cycle) + self.neg_pwm.duty_u16(0) + else: + self.pos_pwm.duty_u16(0) + self.neg_pwm.duty_u16(0 - signed_duty_cycle)