Files
bioglow_solutions/regionals/robot.py

117 lines
4.7 KiB
Python
Raw Normal View History

2026-10-31 13:27:07 -05:00
"""
robot.py — everything about OUR robot, in one place.
(Replaces bioglow_robot_settings.py.)
Every mission starts with:
import robot
Then:
await robot.move(55) # forward 55 cm
await robot.turn(-90) # left 90 degrees
await robot.arc(-35, degrees=90) # curve left
await robot.left_arm.move_by(100, speed=47) # arms (see arm.py)
robot.color_sensor, robot.lazer_ranger, robot.hub
RULE: motors and sensors are created ONLY in this file.
Creating Motor(Port.A) anywhere else gives a "port already in use" error.
Important: test all code with the battery at 100%, and make all updates
with the robot at full charge.
"""
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.tools import run_task, multitask, wait, StopWatch
from pybricks.robotics import DriveBase
from pybricks.hubs import PrimeHub
import umath
# ---------------------------------------------------------------- hub
hub = PrimeHub()
# ---------------------------------------------------------------- wheels
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B, Direction.CLOCKWISE)
WHEEL_DIAMETER = 62.4 # mm (measure your wheels)
AXLE_TRACK = 150 # mm (distance between the two wheels)
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
drive_base.use_gyro(True)
# ---------------------------------------------------------------- arms
# Gear trains: both trains go in ONE list. 2.33:1 reduction, so top speed is ~430 deg/s.
left_arm_motor = Motor(Port.C, Direction.CLOCKWISE, [[20, 28], [12, 20]])
right_arm_motor = Motor(Port.D, Direction.CLOCKWISE, [[20, 28], [12, 20]])
# Each arm gets % speeds, named positions and homing (see arm.py).
# Before using home() or move_to(), on the robot:
# 1. set home_backward: which way each arm turns to reach its hard stop
# 2. add positions, measured from that stop (run arm_calibrate.py),
# for example positions={"down": 0, "up": 120}
from arm import Arm
left_arm = Arm(left_arm_motor, speed=50, positions={}, home_backward=True)
right_arm = Arm(right_arm_motor, speed=50, positions={}, home_backward=False)
# ---------------------------------------------------------------- sensors
lazer_ranger = UltrasonicSensor(Port.E) # mounted at the BACK of the robot
color_sensor = ColorSensor(Port.F)
# ---------------------------------------------------------------- moves
# movement.py does the work, so robot.move(55) means "drive 55 cm".
#
# NORMAL SPEEDS AND ACCELERATIONS. Used whenever a move doesn't give its own:
# await robot.move(30) -> normal values
# await robot.move(30, speed=40, acceleration=200) -> just for this move
# Speeds are % of top speed: 100% straight = about 544 mm/s, 100% turning = about 416 deg/s.
DEFAULT_SPEED = 80 # % for move() and arc() (about 435 mm/s)
DEFAULT_TURN_SPEED = 72 # % for turn() (about 300 deg/s)
DEFAULT_ACCELERATION = 500 # mm/s per second, move() and arc()
DEFAULT_TURN_ACCELERATION = 200 # deg/s per second, turn()
import movement
movement.initialize(drive_base, speed=DEFAULT_SPEED, turn_speed=DEFAULT_TURN_SPEED,
acceleration=DEFAULT_ACCELERATION,
turn_acceleration=DEFAULT_TURN_ACCELERATION)
from movement import (move, turn, arc, move_for_seconds, move_until_stalled,
drive, stop, brake, reset, distance_cm, angle,
set_default_speed, reset_settings)
async def settle_gyro(ms=500, max_seconds=2):
"""Let the gyro settle before a run starts.
Waits ms milliseconds (default half a second), then keeps waiting until the
hub says it's perfectly still, so the first turn starts from a steady heading.
Gives up after max_seconds, so a run never gets stuck here.
"""
await wait(ms)
timer = StopWatch()
while timer.time() < max_seconds * 1000:
still = hub.imu.stationary()
ready = hub.imu.ready() if hasattr(hub.imu, "ready") else True
if still and ready:
return
await wait(10)
async def home_arms():
"""Find both arms' starting positions. Check home_backward first!"""
await multitask(left_arm.home(), right_arm.home())
async def lift_up(degrees, speed=None):
"""Raise both arms together (they face opposite ways). speed is in %."""
await multitask(
right_arm.move_by(degrees, speed),
left_arm.move_by(-degrees, speed),
)
async def lift_down(degrees, speed=None):
"""Lower both arms together. speed is in %."""
await multitask(
right_arm.move_by(-degrees, speed),
left_arm.move_by(degrees, speed),
)