Files
bioglow_solutions/regionals/movement.py

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()