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)
|
|
|
|
|
|
|
|
|
|
|
2026-10-31 20:57:16 +00:00
|
|
|
async def settle_gyro(ms=250, max_seconds=2):
|
2026-10-31 13:27:07 -05:00
|
|
|
"""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),
|
|
|
|
|
)
|