65 lines
1.7 KiB
Python
65 lines
1.7 KiB
Python
import gc
|
|
import time
|
|
import math
|
|
from motor import MotorCluster, motor2040
|
|
|
|
"""
|
|
Demonstrates how to create a MotorCluster object to control multiple motors at once.
|
|
|
|
NOTE: MotorCluster uses the RP2040's PIO system, and as
|
|
such may have problems when running code multiple times.
|
|
If you encounter issues, try resetting your board.
|
|
"""
|
|
|
|
# Free up hardware resources ahead of creating a new MotorCluster
|
|
gc.collect()
|
|
|
|
# Create a motor cluster, using PIO 0 and State Machine 0
|
|
MOTOR_PINS = [motor2040.MOTOR_A, motor2040.MOTOR_B, motor2040.MOTOR_C, motor2040.MOTOR_D]
|
|
motors = MotorCluster(pio=0, sm=0, pins=MOTOR_PINS)
|
|
|
|
# Enable all motors
|
|
motors.enable_all()
|
|
time.sleep(2)
|
|
|
|
# Drive at full positive
|
|
motors.all_full_positive()
|
|
time.sleep(2)
|
|
|
|
# Stop all moving
|
|
motors.stop_all()
|
|
time.sleep(2)
|
|
|
|
# Drive at full negative
|
|
motors.all_full_negative()
|
|
time.sleep(2)
|
|
|
|
# Coast all to a gradual stop
|
|
motors.coast_all()
|
|
time.sleep(2)
|
|
|
|
|
|
SWEEPS = 2 # How many 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
|
|
motors.all_to_speed(speed)
|
|
time.sleep(0.02)
|
|
|
|
# Do a stepped speed sweep
|
|
for j in range(SWEEPS):
|
|
for i in range(0, STEPS):
|
|
motors.all_to_percent(i, 0, STEPS, 0.0 - SPEED_EXTENT, SPEED_EXTENT)
|
|
time.sleep(STEPS_INTERVAL)
|
|
for i in range(0, STEPS):
|
|
motors.all_to_percent(i, STEPS, 0, 0.0 - SPEED_EXTENT, SPEED_EXTENT)
|
|
time.sleep(STEPS_INTERVAL)
|
|
|
|
# Disable the motors
|
|
motors.disable_all()
|