From 59019ab850731590591f902f8a7cf14c9bcf2f4e Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 10 May 2022 18:36:44 +0100 Subject: [PATCH] Added MP examples for pico motor shim, and tweaked others --- examples/pico_motor_shim/song/demo.cpp | 14 +- .../examples/motor2040/multiple_motors.py | 4 +- .../motor2040/quad_velocity_sequence.py | 2 +- .../examples/pico_motor_shim/dual_motors.py | 71 +++++++ .../examples/pico_motor_shim/motor_song.py | 191 ++++++++++++++++++ .../examples/pico_motor_shim/movements.py | 109 ++++++++++ .../examples/pico_motor_shim/single_motor.py | 54 +++++ .../examples/pico_motor_shim/stop_motors.py | 11 + 8 files changed, 446 insertions(+), 10 deletions(-) create mode 100644 micropython/examples/pico_motor_shim/dual_motors.py create mode 100644 micropython/examples/pico_motor_shim/motor_song.py create mode 100644 micropython/examples/pico_motor_shim/movements.py create mode 100644 micropython/examples/pico_motor_shim/single_motor.py create mode 100644 micropython/examples/pico_motor_shim/stop_motors.py diff --git a/examples/pico_motor_shim/song/demo.cpp b/examples/pico_motor_shim/song/demo.cpp index bd2f2ad5..584dd3a0 100644 --- a/examples/pico_motor_shim/song/demo.cpp +++ b/examples/pico_motor_shim/song/demo.cpp @@ -39,13 +39,8 @@ static const uint STATIONARY_TOGGLE_US = 2000; Button button_a(pico_motor_shim::BUTTON_A, Polarity::ACTIVE_LOW, 0); -#ifdef USE_FAST_DECAY - Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, FAST_DECAY); -#else - Motor motor_1(pico_motor_shim::MOTOR_1, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); - Motor motor_2(pico_motor_shim::MOTOR_2, NORMAL_DIR, MotorState::DEFAULT_SPEED_SCALE, MotorState::DEFAULT_FREQUENCY, SLOW_DECAY); -#endif +Motor motor_1(pico_motor_shim::MOTOR_1); +Motor motor_2(pico_motor_shim::MOTOR_2); static bool button_toggle = false; @@ -70,6 +65,11 @@ int main() { gpio_init(PICO_DEFAULT_LED_PIN); gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); + #ifdef USE_FAST_DECAY + motor_1.decay_mode(FAST_DECAY); + motor_2.decay_mode(FAST_DECAY); + #endif + //Initialise the motor if(!motor_1.init() || !motor_2.init()) { printf("Cannot initialise the motors. Check the provided parameters\n"); diff --git a/micropython/examples/motor2040/multiple_motors.py b/micropython/examples/motor2040/multiple_motors.py index 008e963b..22866c60 100644 --- a/micropython/examples/motor2040/multiple_motors.py +++ b/micropython/examples/motor2040/multiple_motors.py @@ -45,8 +45,8 @@ SPEED_EXTENT = 1.0 # How far from zero to drive the motors when sweeping for j in range(SWEEPS): for i in range(360): speed = math.sin(math.radians(i)) * SPEED_EXTENT - for s in motors: - s.speed(speed) + for m in motors: + m.speed(speed) time.sleep(0.02) # Do a stepped speed sweep diff --git a/micropython/examples/motor2040/quad_velocity_sequence.py b/micropython/examples/motor2040/quad_velocity_sequence.py index 65183356..c153ad2d 100644 --- a/micropython/examples/motor2040/quad_velocity_sequence.py +++ b/micropython/examples/motor2040/quad_velocity_sequence.py @@ -28,7 +28,7 @@ TIME_FOR_EACH_MOVE = 2 # The time to travel between each value UPDATES_PER_MOVE = TIME_FOR_EACH_MOVE * UPDATES PRINT_DIVIDER = 4 # How many of the updates should be printed (i.e. 2 would be every other update) -DRIVING_SPEED = 1.0 # The speed to drive the wheels at +DRIVING_SPEED = 1.0 # The speed to drive the wheels at, from 0.0 to SPEED_SCALE # PID values VEL_KP = 30.0 # Velocity proportional (P) gain diff --git a/micropython/examples/pico_motor_shim/dual_motors.py b/micropython/examples/pico_motor_shim/dual_motors.py new file mode 100644 index 00000000..13ee9b39 --- /dev/null +++ b/micropython/examples/pico_motor_shim/dual_motors.py @@ -0,0 +1,71 @@ +import time +import math +from motor import Motor, pico_motor_shim +# from pimoroni import REVERSED_DIR + +""" +Demonstrates how to create two Motor objects and control them together. +""" + +# Create a list of motors +MOTOR_PINS = [pico_motor_shim.MOTOR_1, pico_motor_shim.MOTOR_2] +motors = [Motor(pins) for pins in MOTOR_PINS] + +# Uncomment the below lines (and the top import) to +# reverse the driving direction of a motor +# motors[0].direction(REVERSED_DIR) +# motors[1].direction(REVERSED_DIR) + +# Enable all motors +for m in motors: + m.enable() +time.sleep(2) + +# Drive at full positive +for m in motors: + m.full_positive() +time.sleep(2) + +# Stop moving +for m in motors: + m.stop() +time.sleep(2) + +# Drive at full negative +for m in motors: + m.full_negative() +time.sleep(2) + +# Coast to a gradual stop +for m in motors: + m.coast() +time.sleep(2) + + +SWEEPS = 2 # How many speed sweeps of the motors to perform +STEPS = 10 # The number of discrete sweep steps +STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence +SPEED_EXTENT = 1.0 # How far from zero to drive the motors when sweeping + +# Do a sine speed sweep +for j in range(SWEEPS): + for i in range(360): + speed = math.sin(math.radians(i)) * SPEED_EXTENT + for m in motors: + m.speed(speed) + time.sleep(0.02) + +# Do a stepped speed sweep +for j in range(SWEEPS): + for i in range(0, STEPS): + for m in motors: + m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + for m in motors: + m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + +# Disable the motors +for m in motors: + m.disable() diff --git a/micropython/examples/pico_motor_shim/motor_song.py b/micropython/examples/pico_motor_shim/motor_song.py new file mode 100644 index 00000000..25285ffb --- /dev/null +++ b/micropython/examples/pico_motor_shim/motor_song.py @@ -0,0 +1,191 @@ +import gc +import time +from machine import Pin +from motor import Motor, pico_motor_shim, SLOW_DECAY # , FAST_DECAY +from pimoroni import Button + +""" +A fun example of how to change a motor's frequency to have it play a song. +""" + +# This handy dictonary converts notes into frequencies +TONES = { + "B0": 31, + "C1": 33, + "CS1": 35, + "D1": 37, + "DS1": 39, + "E1": 41, + "F1": 44, + "FS1": 46, + "G1": 49, + "GS1": 52, + "A1": 55, + "AS1": 58, + "B1": 62, + "C2": 65, + "CS2": 69, + "D2": 73, + "DS2": 78, + "E2": 82, + "F2": 87, + "FS2": 93, + "G2": 98, + "GS2": 104, + "A2": 110, + "AS2": 117, + "B2": 123, + "C3": 131, + "CS3": 139, + "D3": 147, + "DS3": 156, + "E3": 165, + "F3": 175, + "FS3": 185, + "G3": 196, + "GS3": 208, + "A3": 220, + "AS3": 233, + "B3": 247, + "C4": 262, + "CS4": 277, + "D4": 294, + "DS4": 311, + "E4": 330, + "F4": 349, + "FS4": 370, + "G4": 392, + "GS4": 415, + "A4": 440, + "AS4": 466, + "B4": 494, + "C5": 523, + "CS5": 554, + "D5": 587, + "DS5": 622, + "E5": 659, + "F5": 698, + "FS5": 740, + "G5": 784, + "GS5": 831, + "A5": 880, + "AS5": 932, + "B5": 988, + "C6": 1047, + "CS6": 1109, + "D6": 1175, + "DS6": 1245, + "E6": 1319, + "F6": 1397, + "FS6": 1480, + "G6": 1568, + "GS6": 1661, + "A6": 1760, + "AS6": 1865, + "B6": 1976, + "C7": 2093, + "CS7": 2217, + "D7": 2349, + "DS7": 2489, + "E7": 2637, + "F7": 2794, + "FS7": 2960, + "G7": 3136, + "GS7": 3322, + "A7": 3520, + "AS7": 3729, + "B7": 3951, + "C8": 4186, + "CS8": 4435, + "D8": 4699, + "DS8": 4978 +} + +# Put the notes for your song in here! +SONG = ("F6", "F6", "E6", "F6", "F5", "P", "F5", "P", "C6", "AS5", "A5", "C6", "F6", "P", "F6", "P", "G6", "FS6", "G6", "G5", "P", "G5", "P", "G6", "F6", "E6", "D6", "C6", "P", "C6", "P", "D6", "E6", "F6", "E6", "D6", "C6", "D6", "C6", "AS5", "A5", "AS5", "A5", "G5", "F5", "G5", "F5", "E5", "D5", "C5", "D5", "E5", "F5", "G5", "AS5", "A5", "G5", "A5", "F5", "P", "F5") + +NOTE_DURATION = 0.150 # The time (in seconds) to play each note for. Change this to make the song play faster or slower +STATIONARY_TOGGLE_US = 2000 # The time (in microseconds) between each direction switch of the motor when using STATIONARY_PLAYBACK +STATIONARY_PLAYBACK = False # Whether to play the song with or without the motors spinning +DECAY_MODE = SLOW_DECAY # The motor decay mode to use, either FAST_DECAY (0) or SLOW_DECAY (1). Affects the song's quality + + +# Free up hardware resources ahead of creating new Motors +# (avoids occasional issues where the song just stops playing) +gc.collect() + +# Create two motor objects with a given decay mode +motor_1 = Motor(pico_motor_shim.MOTOR_1, mode=DECAY_MODE) +motor_2 = Motor(pico_motor_shim.MOTOR_2, mode=DECAY_MODE) + +# Create a pin for the Pico's onboard LED +led = Pin(25, Pin.OUT) +led.value(False) + +# Create the user button +button_a = Button(pico_motor_shim.BUTTON_A, repeat_time=0) + +# Variable for recording if the button has been toggled +# Starting as True makes the song play automatically +button_toggle = True + + +# Function to check if the button has been toggled +def check_button_toggle(): + global button_toggle + if button_a.read(): + button_toggle = not button_toggle + return button_toggle + + +while True: + # Has the button been toggled? + if check_button_toggle(): + # Turn the Pico's LED on to show that the song has started + led.value(True) + + # Play the song + for i in range(len(SONG)): + if check_button_toggle(): + if SONG[i] == "P": + # This is a "pause" note, so stop the motors + motor_1.stop() + motor_2.stop() + time.sleep(NOTE_DURATION) + else: + # Get the frequency of the note and set the motors to it + freq = TONES[SONG[i]] + motor_1.frequency(freq) + motor_2.frequency(freq) + + if 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 + t = 0 + while t < NOTE_DURATION * 1000000: + motor_1.duty(0.5) + motor_2.duty(0.5) + time.sleep_us(STATIONARY_TOGGLE_US) + t += STATIONARY_TOGGLE_US + + motor_1.duty(-0.5) + motor_2.duty(-0.5) + time.sleep_us(STATIONARY_TOGGLE_US) + t += STATIONARY_TOGGLE_US + else: + # Set the motors to 50% duty cycle to play the note whilst spinning + motor_1.duty(0.5) + motor_2.duty(0.5) + time.sleep(NOTE_DURATION) + else: + # The button was toggled again, so stop playing the song + break + + button_toggle = False + + # The song has finished, so turn off the Pico's LED and disable the motors + led.value(False) + motor_1.disable() + motor_2.disable() + + time.sleep(0.01) diff --git a/micropython/examples/pico_motor_shim/movements.py b/micropython/examples/pico_motor_shim/movements.py new file mode 100644 index 00000000..3b716fbe --- /dev/null +++ b/micropython/examples/pico_motor_shim/movements.py @@ -0,0 +1,109 @@ +import time +from motor import Motor, pico_motor_shim +from pimoroni import NORMAL_DIR, REVERSED_DIR + +""" +An example of how to perform simple movements of a 2-wheeled driving robot. +""" + +SPEED_SCALE = 5.4 # The scaling to apply to each motor's speed to match its real-world speed +DRIVING_SPEED = SPEED_SCALE # The speed to drive the wheels at, from 0.0 to SPEED_SCALE + +# Create the left and right motors with a given speed scale +# Swap the numbers and directions if this is different to your setup +left = Motor(pico_motor_shim.MOTOR_1, direction=NORMAL_DIR, speed_scale=SPEED_SCALE) +right = Motor(pico_motor_shim.MOTOR_2, direction=REVERSED_DIR, speed_scale=SPEED_SCALE) + + +# Helper functions for driving in common directions +def forward(speed=DRIVING_SPEED): + left.speed(speed) + right.speed(speed) + + +def backward(speed=DRIVING_SPEED): + left.speed(-speed) + right.speed(-speed) + + +def turn_left(speed=DRIVING_SPEED): + left.speed(-speed) + right.speed(speed) + + +def turn_right(speed=DRIVING_SPEED): + left.speed(speed) + right.speed(-speed) + + +def curve_forward_left(speed=DRIVING_SPEED): + left.speed(0.0) + right.speed(speed) + + +def curve_forward_right(speed=DRIVING_SPEED): + left.speed(speed) + right.speed(0.0) + + +def curve_backward_left(speed=DRIVING_SPEED): + left.speed(0.0) + right.speed(-speed) + + +def curve_backward_right(speed=DRIVING_SPEED): + left.speed(-speed) + right.speed(0.0) + + +def stop(): + left.stop() + right.stop() + + +def coast(): + left.coast() + right.coast() + + +# Demo each of the move methods +forward() +time.sleep(1) + +backward() +time.sleep(1) + +curve_forward_right() +time.sleep(1) + +curve_forward_left() +time.sleep(1) + +turn_right() +time.sleep(1) + +forward(0.5 * DRIVING_SPEED) # Half speed +time.sleep(1) + +turn_left(0.5 * DRIVING_SPEED) # Half speed +time.sleep(1) + +curve_backward_right(0.75 * DRIVING_SPEED) # Three quarters speed +time.sleep(1) + +forward() # Full speed +time.sleep(0.5) + +coast() # Come to a halt gently +time.sleep(1) + +forward() +time.sleep(0.5) + +stop() # Apply the brakes +time.sleep(1.0) + + +# Disable the motors +left.disable() +right.disable() diff --git a/micropython/examples/pico_motor_shim/single_motor.py b/micropython/examples/pico_motor_shim/single_motor.py new file mode 100644 index 00000000..9fca60e5 --- /dev/null +++ b/micropython/examples/pico_motor_shim/single_motor.py @@ -0,0 +1,54 @@ +import time +import math +from motor import Motor, pico_motor_shim + +""" +Demonstrates how to create a Motor object and control it. +""" + +# Create a motor +m = Motor(pico_motor_shim.MOTOR_1) + +# Enable the motor +m.enable() +time.sleep(2) + +# Drive at full positive +m.full_positive() +time.sleep(2) + +# Stop moving +m.stop() +time.sleep(2) + +# Drive at full negative +m.full_negative() +time.sleep(2) + +# Coast to a gradual stop +m.coast() +time.sleep(2) + + +SWEEPS = 2 # How many speed sweeps of the motor to perform +STEPS = 10 # The number of discrete sweep steps +STEPS_INTERVAL = 0.5 # The time in seconds between each step of the sequence +SPEED_EXTENT = 1.0 # How far from zero to drive the motor when sweeping + +# Do a sine speed sweep +for j in range(SWEEPS): + for i in range(360): + m.speed(math.sin(math.radians(i)) * SPEED_EXTENT) + time.sleep(0.02) + +# Do a stepped speed sweep +for j in range(SWEEPS): + for i in range(0, STEPS): + m.to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + for i in range(0, STEPS): + m.to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT) + time.sleep(STEPS_INTERVAL) + +# Disable the motor +m.disable() diff --git a/micropython/examples/pico_motor_shim/stop_motors.py b/micropython/examples/pico_motor_shim/stop_motors.py new file mode 100644 index 00000000..673c08d8 --- /dev/null +++ b/micropython/examples/pico_motor_shim/stop_motors.py @@ -0,0 +1,11 @@ +from motor import Motor, pico_motor_shim + +""" +A simple program that stops the motors. +""" + +# Create two motor objects. +# This will initialise the pins, stopping any +# previous movement that may be stuck on +m1 = Motor(pico_motor_shim.MOTOR_1) +m2 = Motor(pico_motor_shim.MOTOR_2)