318 lines
12 KiB
Python
318 lines
12 KiB
Python
"""
|
|
movement.py — easy robot moves in centimeters, degrees and % speed.
|
|
|
|
Set it up ONCE (in robot.py, after drive_base is made):
|
|
import movement
|
|
movement.initialize(drive_base, speed=80, turn_speed=72,
|
|
acceleration=500, turn_acceleration=200)
|
|
|
|
Use it in any mission (always with await):
|
|
await robot.move(30) # forward 30 cm, normal speed
|
|
await robot.move(-10, speed=40) # backward 10 cm at 40%
|
|
await robot.move(12, speed=18, acceleration=200) # slow and gentle
|
|
await robot.turn(90) # turn right 90 degrees
|
|
await robot.turn(-45, speed=30) # turn left 45 degrees, slowly
|
|
await robot.arc(20, degrees=90) # quarter circle to the right, 20 cm radius
|
|
await robot.arc(-35, cm=34) # curve left for 34 cm
|
|
await robot.move(2, then=robot.Stop.NONE) # don't stop, flow into the next move
|
|
await robot.move(12, speed=18, max_seconds=2) # push a model, but give up after 2 s
|
|
|
|
SPEED is a PERCENT of the robot's top speed: 1 = crawl, 100 = as fast as it goes.
|
|
move/arc: % of about 544 mm/s. turn: % of about 416 degrees/s.
|
|
ACCELERATION is how quickly it speeds up and slows down:
|
|
move/arc: mm/s per second. turn: degrees/s per second.
|
|
Lower = gentler (less slipping), higher = snappier.
|
|
Leave any of them out to use the normal values from robot.py.
|
|
A value you give is for THAT move only. The next move is back to normal.
|
|
|
|
It's speed, not raw power: the hub keeps the wheels at that speed and
|
|
still stops at exactly the right distance, even when the battery is low.
|
|
|
|
max_seconds= is a safety time limit for move, turn and arc. If a model or wall
|
|
blocks the robot, the move gives up after that many seconds instead of getting
|
|
stuck, and the run carries on. The move returns True if it finished, False if
|
|
it ran out of time. Leave it out for no time limit.
|
|
|
|
then= says what to do at the end of a move (same as Pybricks):
|
|
Stop.HOLD (default) stop and hold the position
|
|
Stop.COAST stop and roll freely
|
|
Stop.NONE don't stop, flow straight into the next move
|
|
"""
|
|
|
|
from pybricks.parameters import Stop
|
|
from pybricks.tools import wait, StopWatch, multitask
|
|
|
|
# The DriveBase. Set by initialize(), not here.
|
|
drive_base = None
|
|
|
|
# Normal values, used when a move doesn't give its own (set in robot.py).
|
|
_default_speed = 80 # % for move/arc
|
|
_default_turn = 72 # % for turn
|
|
_default_acc = 500 # mm/s per second for move/arc
|
|
_default_turn_acc = 200 # deg/s per second for turn
|
|
_max_speed = None # mm/s, the fastest this robot can drive straight (= 100%)
|
|
_max_turn = None # deg/s, the fastest it can turn (= 100%)
|
|
_start = None # the values from robot.py, for reset_settings()
|
|
|
|
|
|
def initialize(drive, speed=80, turn_speed=72, acceleration=500, turn_acceleration=200):
|
|
"""Connect the movement library to the robot's DriveBase and set the normal values."""
|
|
global drive_base, _max_speed, _max_turn, _start
|
|
drive_base = drive
|
|
# Top speeds come from the motors and wheel size, measured by Pybricks itself.
|
|
_max_speed = drive.distance_control.limits()[0]
|
|
_max_turn = drive.heading_control.limits()[0]
|
|
_start = (speed, turn_speed, acceleration, turn_acceleration)
|
|
set_default_speed(*_start)
|
|
|
|
|
|
def set_default_speed(speed=None, turn_speed=None, acceleration=None, turn_acceleration=None):
|
|
"""Change the NORMAL values for the rest of this run. Usually not needed:
|
|
give speed= or acceleration= on the move itself instead.
|
|
"""
|
|
global _default_speed, _default_turn, _default_acc, _default_turn_acc
|
|
if speed is not None:
|
|
_from_percent(speed, 1) # just checks it's 1-100
|
|
_default_speed = speed
|
|
if turn_speed is not None:
|
|
_from_percent(turn_speed, 1)
|
|
_default_turn = turn_speed
|
|
if acceleration is not None:
|
|
_default_acc = acceleration
|
|
if turn_acceleration is not None:
|
|
_default_turn_acc = turn_acceleration
|
|
_apply()
|
|
|
|
|
|
def reset_settings():
|
|
"""Go back to the normal values from robot.py. The menu does this before every run."""
|
|
_check()
|
|
set_default_speed(*_start)
|
|
|
|
|
|
def _apply(speed=None, turn_speed=None, acceleration=None, turn_acceleration=None):
|
|
"""Send the given values (or the normal ones) to the drive base."""
|
|
drive_base.settings(
|
|
straight_speed=_from_percent(_default_speed if speed is None else speed, _max_speed),
|
|
straight_acceleration=_default_acc if acceleration is None else acceleration,
|
|
turn_rate=_from_percent(_default_turn if turn_speed is None else turn_speed, _max_turn),
|
|
turn_acceleration=_default_turn_acc if turn_acceleration is None else turn_acceleration,
|
|
)
|
|
|
|
|
|
def _check():
|
|
if drive_base is None:
|
|
raise RuntimeError(
|
|
"movement isn't set up. Add movement.initialize(drive_base) "
|
|
"in robot.py, after drive_base.settings(...)"
|
|
)
|
|
|
|
|
|
def _mm(cm):
|
|
# Pybricks drops decimals, so round instead: 57.5 cm -> 575 mm
|
|
return int(round(cm * 10))
|
|
|
|
|
|
def _from_percent(percent, top, allow_zero=False):
|
|
"""Turn a percent (like 50) into a real speed (like 272 mm/s)."""
|
|
low = 0 if allow_zero else 1
|
|
if not low <= abs(percent) <= 100:
|
|
raise ValueError(
|
|
"speed is a percent from %d to 100, but got %s" % (low, percent)
|
|
)
|
|
return int(top * percent / 100)
|
|
|
|
|
|
async def _limited(motion, max_seconds):
|
|
"""Wait for a drive base move, or give up after max_seconds. True = finished."""
|
|
if max_seconds is None:
|
|
await motion
|
|
return True
|
|
|
|
async def finish():
|
|
await motion
|
|
return True
|
|
|
|
done, _ = await multitask(finish(), wait(int(max_seconds * 1000)), race=True)
|
|
if done is True:
|
|
return True
|
|
drive_base.stop() # ran out of time: stop pushing and carry on
|
|
return False
|
|
|
|
|
|
async def move(cm, speed=None, acceleration=None, then=Stop.HOLD, max_seconds=None):
|
|
"""Drive straight.
|
|
|
|
cm: distance in centimeters. Positive = forward, negative = backward.
|
|
speed: % of top speed (1-100). Leave out for normal.
|
|
acceleration: mm/s per second. Leave out for normal.
|
|
then: Stop.HOLD (default), Stop.COAST, or Stop.NONE to flow into the next move.
|
|
max_seconds: give up after this long (for pushes that might get blocked).
|
|
"""
|
|
_check()
|
|
_apply(speed=speed, acceleration=acceleration)
|
|
return await _limited(drive_base.straight(_mm(cm), then=then), max_seconds)
|
|
|
|
|
|
async def turn(degrees, speed=None, acceleration=None, then=Stop.HOLD, max_seconds=None):
|
|
"""Turn in place.
|
|
|
|
degrees: positive = right (clockwise), negative = left.
|
|
speed: % of top turning speed (1-100). Leave out for normal.
|
|
acceleration: degrees/s per second. Leave out for normal.
|
|
then: Stop.HOLD (default), Stop.COAST, or Stop.NONE.
|
|
max_seconds: give up after this long.
|
|
"""
|
|
_check()
|
|
_apply(turn_speed=speed, turn_acceleration=acceleration)
|
|
return await _limited(drive_base.turn(degrees, then=then), max_seconds)
|
|
|
|
|
|
async def arc(radius_cm, degrees=None, cm=None, speed=None, turn_speed=None,
|
|
acceleration=None, turn_acceleration=None, then=Stop.HOLD, max_seconds=None):
|
|
"""Drive along part of a circle. Give degrees OR cm, not both.
|
|
|
|
radius_cm: size of the circle in cm.
|
|
Positive = circle to the RIGHT, negative = circle to the LEFT.
|
|
degrees: how far around the circle to go.
|
|
Positive = forward, negative = backward.
|
|
cm: or: how far to drive along the circle, in cm.
|
|
Positive = forward, negative = backward.
|
|
speed, acceleration: limits for driving along the circle.
|
|
turn_speed, turn_acceleration: limits for how fast it turns while curving.
|
|
then: Stop.HOLD (default), Stop.COAST, or Stop.NONE.
|
|
max_seconds: give up after this long.
|
|
|
|
Examples:
|
|
await robot.arc(20, degrees=90) # quarter circle forward, curving right
|
|
await robot.arc(-35, degrees=90) # quarter circle forward, curving left
|
|
await robot.arc(128, cm=34) # 34 cm along a big right-hand curve
|
|
"""
|
|
_check()
|
|
if (degrees is None) == (cm is None):
|
|
raise ValueError("arc needs degrees=... or cm=..., but not both")
|
|
_apply(speed, turn_speed, acceleration, turn_acceleration)
|
|
if cm is None:
|
|
motion = drive_base.arc(_mm(radius_cm), angle=degrees, then=then)
|
|
else:
|
|
motion = drive_base.arc(_mm(radius_cm), distance=_mm(cm), then=then)
|
|
return await _limited(motion, max_seconds)
|
|
|
|
|
|
async def move_for_seconds(seconds, speed=None, backward=False, acceleration=None):
|
|
"""Drive straight for a set time, then stop.
|
|
|
|
seconds: how long to drive, like 1.5
|
|
speed: % of top speed (1-100). Leave out for normal.
|
|
backward: True to drive backward.
|
|
acceleration: mm/s per second. Leave out for normal.
|
|
|
|
Good for pushing a mission model. For exact distances, use move() instead.
|
|
"""
|
|
_check()
|
|
_apply(acceleration=acceleration)
|
|
mm_s = _from_percent(_default_speed if speed is None else speed, _max_speed)
|
|
drive_base.drive(-mm_s if backward else mm_s, 0)
|
|
try:
|
|
await wait(int(seconds * 1000))
|
|
finally:
|
|
drive_base.brake() # also stops if a multitask race cancels us
|
|
|
|
|
|
async def move_until_stalled(speed=None, backward=False, max_seconds=5, acceleration=None):
|
|
"""Drive straight until something blocks the robot, then stop.
|
|
|
|
"Blocked" means EITHER Pybricks says the wheels are stalled, OR the robot
|
|
has almost stopped moving forward (so it also works on light, springy
|
|
models that never make the motors work at full power).
|
|
It can't notice wheels spinning in place, so max_seconds always applies.
|
|
|
|
speed: % of top speed (1-100). Slower is gentler: try 20-40.
|
|
backward: True to drive backward.
|
|
max_seconds: give up after this long.
|
|
acceleration: mm/s per second. Leave out for normal.
|
|
|
|
Returns True if it got blocked, False if it gave up.
|
|
Example:
|
|
hit = await move_until_stalled(speed=30, backward=True)
|
|
if not hit:
|
|
print("Never reached the wall!")
|
|
"""
|
|
_check()
|
|
_apply(acceleration=acceleration)
|
|
mm_s = _from_percent(_default_speed if speed is None else speed, _max_speed)
|
|
drive_base.drive(-mm_s if backward else mm_s, 0)
|
|
|
|
acc = _default_acc if acceleration is None else acceleration
|
|
ramp_ms = int(1000 * mm_s / acc) + 100 # time to reach full speed, plus a moment
|
|
window_ms = 200 # check progress every 0.2 s
|
|
min_progress = max(1, mm_s * window_ms / 1000 / 4) # under 1/4 of normal = stuck
|
|
|
|
timer = StopWatch()
|
|
window = StopWatch()
|
|
last = drive_base.distance()
|
|
try:
|
|
await wait(100) # give the wheels a moment to start rolling
|
|
while timer.time() < max_seconds * 1000:
|
|
if drive_base.stalled():
|
|
return True
|
|
if window.time() >= window_ms:
|
|
now = drive_base.distance()
|
|
if timer.time() > ramp_ms and abs(now - last) < min_progress:
|
|
return True # barely moving: something is in the way
|
|
last = now
|
|
window.reset()
|
|
await wait(10)
|
|
return False
|
|
finally:
|
|
drive_base.stop() # stop pushing, even if a race cancels us
|
|
|
|
|
|
def drive(speed, steer=0):
|
|
"""Start driving and keep going until stop() is called.
|
|
|
|
speed: percent of top speed, -100 to 100. Negative = backward.
|
|
steer: percent of top turning speed, -100 to 100. Positive = steer right.
|
|
Doesn't wait, so don't use await on it. Example:
|
|
drive(40)
|
|
while await lazer_ranger.distance() > 100:
|
|
await wait(10)
|
|
stop()
|
|
"""
|
|
_check()
|
|
_apply()
|
|
drive_base.drive(
|
|
_from_percent(speed, _max_speed, allow_zero=True),
|
|
_from_percent(steer, _max_turn, allow_zero=True),
|
|
)
|
|
|
|
|
|
def stop():
|
|
"""Stop the wheels and let the robot roll to a stop."""
|
|
_check()
|
|
drive_base.stop()
|
|
|
|
|
|
def brake():
|
|
"""Stop the wheels quickly."""
|
|
_check()
|
|
drive_base.brake()
|
|
|
|
|
|
def reset():
|
|
"""Set the distance and angle counters back to 0 (same as drive_base.reset(0, 0))."""
|
|
_check()
|
|
drive_base.reset()
|
|
|
|
|
|
def distance_cm():
|
|
"""How far the robot has driven since the last reset(), in cm."""
|
|
_check()
|
|
return drive_base.distance() / 10
|
|
|
|
|
|
def angle():
|
|
"""How far the robot has turned since the last reset(), in degrees."""
|
|
_check()
|
|
return drive_base.angle()
|