110 lines
2.1 KiB
Python
110 lines
2.1 KiB
Python
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()
|