🚀 Pybricks Adventure

# This is your first program!
# Lines with # are comments - notes for humans

from pybricks.hubs import PrimeHub
from pybricks.parameters import Color
from pybricks.tools import wait

# Create your robot brain
hub = PrimeHub()

# Turn on the light (like a red stop light!)
hub.light.on(Color.RED)

# Wait 2 seconds (2000 milliseconds)
wait(2000)

# Turn off the light
hub.light.off()

print("Great job! You just programmed a robot!")

# Store numbers
my_age = 12
robot_speed = 500

# Store words (called "strings")
robot_name = "Bolt"
favorite_color = "blue"

# Store yes/no values (called "booleans")
is_moving = True
lights_on = False

# Use your variables!
print("My robot's name is", robot_name)
print("It moves at speed", robot_speed)

# Count from 1 to 5
for number in range(1, 6):
    print("Count:", number)
    hub.display.number(number)
    wait(1000)

print("Counting complete!")

# Blink the light 3 times
for i in range(3):
    hub.light.on(Color.GREEN)
    wait(500)
    hub.light.off()
    wait(500)

# Import Button from parameters first
from pybricks.parameters import Button

# Keep blinking until button is pressed
while Button.CENTER not in hub.buttons.pressed():
    hub.light.on(Color.BLUE)
    wait(200)
    hub.light.off()
    wait(200)

print("Button pressed - stopping!")

# Check the distance
distance = 25  # Pretend we measured this

if distance < 10:
    print("Too close! Stopping!")
    hub.light.on(Color.RED)
elif distance < 30:
    print("Getting close, be careful!")
    hub.light.on(Color.YELLOW)
else:
    print("All clear!")
    hub.light.on(Color.GREEN)

from pybricks.pupdevices import Motor
from pybricks.parameters import Port

# Connect your motor to Port A
my_motor = Motor(Port.A)

# Make it spin forward (positive number)
my_motor.run(500)  # 500 degrees per second
wait(2000)  # Keep spinning for 2 seconds

# Make it spin backward (negative number)
my_motor.run(-500)
wait(2000)

# Stop the motor
my_motor.stop()

# Spin exactly 3 times (3 × 360 degrees)
my_motor.run_angle(500, 1080)  # Speed: 500 deg/s, Angle: 1080 degrees

# Wait for the motor to finish
while not my_motor.done():
    wait(10)

from pybricks.pupdevices import ColorSensor
from pybricks.parameters import Color

# Connect color sensor to Port C
color_sensor = ColorSensor(Port.C)

# Check what color it sees
detected_color = color_sensor.color()

if detected_color == Color.RED:
    print("I see a red apple! 🍎")
    hub.light.on(Color.RED)
elif detected_color == Color.GREEN:
    print("I see green grass! 🌱")
    hub.light.on(Color.GREEN)
elif detected_color == Color.BLUE:
    print("I see the blue sky! ☁️")
    hub.light.on(Color.BLUE)
else:
    print("Hmm, interesting color!")

# Check reflection (0 = dark, 100 = bright)
# This measures reflected light from the sensor's own light
reflection = color_sensor.reflection()
if reflection < 30:
    print("Dark surface detected! 🌙")
else:
    print("Light surface detected! ☀️")

from pybricks.pupdevices import UltrasonicSensor

# Connect distance sensor to Port D
distance_sensor = UltrasonicSensor(Port.D)

# Measure distance in millimeters
distance = distance_sensor.distance()

# Convert to centimeters for easier understanding
if distance is not None:  # Check if reading is valid
    distance_cm = distance / 10
    print(f"Object is {distance_cm} cm away!")
else:
    print("No object detected!")

if distance_cm < 5:
    print("SUPER CLOSE! 🛑")
    hub.speaker.beep(1000, 500)  # High beep
elif distance_cm < 20:
    print("Getting closer! ⚠️")
    hub.speaker.beep(500, 500)  # Medium beep
else:
    print("Lots of room! ✅")
    hub.speaker.beep(200, 500)  # Low beep

from pybricks.robotics import DriveBase
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Direction

# Set up your robot's motors
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)

# Create a DriveBase - this makes driving easy!
# wheel_diameter: How big your wheels are (in mm)
# axle_track: Distance between wheels (in mm)
robot = DriveBase(
    left_motor, 
    right_motor, 
    wheel_diameter=56,  # Standard LEGO wheel
    axle_track=112      # Measure your robot!
)

# Now driving is super easy!
robot.straight(200)    # Drive forward 200mm
robot.turn(90)         # Turn right 90 degrees
robot.straight(200)    # Drive forward again
robot.turn(90)         # Turn right again

print("I just drove in an L shape!")

# Complete obstacle avoiding robot
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, UltrasonicSensor
from pybricks.parameters import Port, Direction, Color, Button
from pybricks.robotics import DriveBase
from pybricks.tools import wait

# Initialize everything
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
eyes = UltrasonicSensor(Port.D)  # The robot's "eyes"

# Create the robot
robot = DriveBase(left_motor, right_motor, 56, 112)

print("Press center button to start!")
print("I'll avoid obstacles automatically!")

# Wait for button press to start
while Button.CENTER not in hub.buttons.pressed():
    wait(10)

# Main robot brain loop
while True:
    # Check if we should stop
    if Button.CENTER in hub.buttons.pressed():
        print("Stopping robot!")
        break
    
    # Look ahead
    distance = eyes.distance()
    
    # Check if reading is valid
    if distance is not None and distance < 200:  # Something is close (200mm = 20cm)
        # Oh no! Obstacle detected!
        hub.light.on(Color.RED)
        robot.stop()
        
        # Back up a bit
        robot.straight(-100)
        
        # Look for a clear path
        robot.turn(45)  # Try turning right
        wait(500)
        
        if eyes.distance() < 200:  # Still blocked?
            robot.turn(-90)  # Try left instead
    else:
        # Path is clear - full speed ahead!
        hub.light.on(Color.GREEN)
        robot.drive(150, 0)  # Drive at 150 mm/s
    
    wait(100)  # Small pause to not overwhelm the robot

robot.stop()
hub.light.off()
print("Adventure complete!")

from pybricks.tools import multitask, run_task

# Define async functions with 'async def'
async def blink_lights():
    """Blink lights continuously"""
    for i in range(10):
        hub.light.on(Color.RED)
        await wait(250)  # 'await' = can do other things
        hub.light.on(Color.GREEN)
        await wait(250)

async def drive_forward():
    """Drive forward for 3 seconds"""
    robot.drive(200, 0)
    await wait(3000)
    robot.stop()

async def make_sounds():
    """Beep every second"""
    for i in range(3):
        hub.speaker.beep()
        await wait(1000)

# Run all three at the same time!
async def main():
    print("Starting multi-tasking robot!")
    await multitask(
        blink_lights(),
        drive_forward(),
        make_sounds()
    )
    print("All tasks complete!")

# Start the async program
run_task(main())

from pybricks.tools import multitask, run_task

# Motors for different parts
drive_base = DriveBase(left_motor, right_motor, 56, 112)
arm_motor = Motor(Port.C)
claw_motor = Motor(Port.D)

async def drive_to_mission():
    """Drive to the mission model"""
    await drive_base.straight(500)  # 500mm forward
    await drive_base.turn(90)       # Turn right
    await drive_base.straight(300)  # 300mm forward

async def prepare_arm():
    """Get arm ready while driving"""
    await arm_motor.run_target(500, 90)  # Lift arm to 90 degrees
    
async def open_claw():
    """Open claw to grab object"""
    await claw_motor.run_angle(200, -45)  # Open claw

# Main mission - do multiple things at once!
async def complete_mission():
    # Drive and prepare arm AT THE SAME TIME
    await multitask(
        drive_to_mission(),
        prepare_arm()
    )
    
    # Now grab the object
    await open_claw()
    await claw_motor.run_angle(200, 45)  # Close claw
    
    # Drive back with object
    await drive_base.straight(-800)
    
    print("Mission complete in record time!")

# Run the mission
run_task(complete_mission())

# Sometimes you want to stop when ANY condition is met
from pybricks.tools import multitask, run_task

async def wait_for_touch():
    """Wait for force sensor to be pressed"""
    force = ForceSensor(Port.E)
    while not force.pressed():
        await wait(50)
    return "Touch detected!"

async def wait_for_color():
    """Wait for red color"""
    color = ColorSensor(Port.F)
    while color.color() != Color.RED:
        await wait(50)
    return "Red detected!"

async def wait_for_timeout():
    """Maximum 10 second timeout"""
    await wait(10000)
    return "Time's up!"

async def main():
    print("Waiting for: touch, red color, or 10 seconds...")
    
    # race=True means stop when FIRST task finishes
    winner = await multitask(
        wait_for_touch(),
        wait_for_color(),
        wait_for_timeout(),
        race=True
    )
    
    # See which one finished first
    print(f"Winner: {winner}")
    hub.speaker.beep()

run_task(main())

from pybricks.robotics import DriveBase

# Create your drive base
robot = DriveBase(left_motor, right_motor, 56, 112)

# Get current settings
print("Default settings:", robot.settings())

# Customize for SPEED!
robot.settings(
    straight_speed=500,     # Max speed for straight (mm/s)
    straight_acceleration=1000,  # How fast to reach max speed
    turn_rate=200,         # Max turn speed (deg/s)
    turn_acceleration=500   # How fast to reach max turn speed
)

# Customize for PRECISION (FLL missions!)
robot.settings(
    straight_speed=200,     # Slower = more accurate
    straight_acceleration=100,   # Gentle acceleration
    turn_rate=90,          # Careful turns
    turn_acceleration=200
)

# Different speeds for different missions
def speed_mode(mode):
    if mode == "FAST":
        robot.settings(500, 1000, 200, 500)
        hub.light.on(Color.RED)
    elif mode == "PRECISE":
        robot.settings(150, 100, 60, 150)
        hub.light.on(Color.GREEN)
    elif mode == "NORMAL":
        robot.settings(300, 500, 120, 300)
        hub.light.on(Color.BLUE)

# The hub has a built-in gyro (IMU)!
from pybricks.robotics import DriveBase

# Create drive base with use_gyro=True for better accuracy
robot = DriveBase(
    left_motor, 
    right_motor, 
    wheel_diameter=56,
    axle_track=112,
    use_gyro=True  # Uses hub's gyro for straight driving!
)

# Reset the gyro angle
robot.reset()

# Super accurate driving - gyro keeps it straight!
robot.straight(1000)  # Will stay perfectly straight

# Check how far you've driven
distance_traveled = robot.distance()
angle_turned = robot.angle()
print(f"Traveled: {distance_traveled}mm")
print(f"Turned: {angle_turned} degrees")

# Drive until you've gone exactly 1 meter
while robot.distance() < 1000:
    robot.drive(200, 0)
robot.stop()

# Drive in curves and arcs!
from pybricks.tools import wait

# Drive in a circle (speed, turn_rate)
robot.drive(200, 50)  # Forward while turning
wait(4000)  # Complete a circle
robot.stop()

# Drive in figure-8 pattern
async def figure_eight():
    # First circle
    robot.drive(200, 45)
    await wait(4000)
    
    # Second circle (opposite direction)
    robot.drive(200, -45)
    await wait(4000)
    
    robot.stop()

# Arc to a specific point
def arc_turn(radius, angle):
    """Drive in an arc with specific radius"""
    # Calculate turn rate based on radius
    speed = 200
    turn_rate = speed / radius
    
    # Calculate time needed
    arc_length = radius * angle * 3.14159 / 180
    time_ms = int(arc_length / speed * 1000)
    
    robot.drive(speed, turn_rate)
    wait(time_ms)
    robot.stop()

# Example: 500mm radius, 90 degree arc
arc_turn(500, 90)

from pybricks.tools import DataLog, StopWatch

# Create a data log with column names
data = DataLog("time", "distance", "reflection", "speed")

# Create a timer
timer = StopWatch()

# Collect data while driving
color_sensor = ColorSensor(Port.C)

for i in range(100):  # Collect 100 samples
    # Log current values
    data.log(
        timer.time(),
        robot.distance(),
        color_sensor.reflection(),
        left_motor.speed()
    )
    
    # Drive forward
    robot.drive(200, 0)
    wait(100)  # Log every 100ms

robot.stop()

# Data is automatically saved!
print("Data logged! Check the Pybricks app to download.")

# Useful debugging helpers
def debug_print(message, value=None):
    """Print with timestamp"""
    time = timer.time() / 1000.0
    if value is not None:
        print(f"[{time:.2f}s] {message}: {value}")
    else:
        print(f"[{time:.2f}s] {message}")

def check_sensors():
    """Print all sensor values"""
    print("=== SENSOR CHECK ===")
    print(f"Color: {color_sensor.color()}")
    print(f"Reflection: {color_sensor.reflection()}")
    print(f"Distance: {distance_sensor.distance()}")
    print(f"Left motor angle: {left_motor.angle()}")
    print(f"Right motor angle: {right_motor.angle()}")
    print("==================")

def beep_error(error_code):
    """Different beep patterns for different errors"""
    if error_code == 1:  # Sensor error
        hub.speaker.beep(1000, 100)
        wait(100)
        hub.speaker.beep(1000, 100)
    elif error_code == 2:  # Motor stall
        hub.speaker.beep(500, 500)
    elif error_code == 3:  # Mission fail
        hub.speaker.beep(200, 1000)

# Use in your code
debug_print("Starting mission")
check_sensors()

if color_sensor.reflection() < 10:
    debug_print("ERROR: Too dark", color_sensor.reflection())
    beep_error(1)