Added object oriented version of codes
This commit is contained in:
317
regionals/movement.py
Normal file
317
regionals/movement.py
Normal file
@@ -0,0 +1,317 @@
|
||||
"""
|
||||
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()
|
||||
Reference in New Issue
Block a user