14 Commits

Author SHA1 Message Date
63cbf571e6 Upload files to "/" 2025-11-09 01:16:52 +00:00
5f33f5c859 Upload files to "/" 2025-11-09 01:13:50 +00:00
05aaf697ce Upload files to "/" 2025-11-09 01:10:38 +00:00
d192664084 Upload files to "/" 2025-11-09 01:06:01 +00:00
alkadienePhoton
1d7b3be640 Made it a true do-not-merge 2025-11-08 18:42:54 -06:00
fa11d612f7 Upload files to "/" 2025-11-09 00:39:30 +00:00
e00fbebcc4 Upload files to "/" 2025-10-28 18:07:56 +00:00
3e2a37c035 Upload files to "/" 2025-10-28 18:05:04 +00:00
53d13476a9 Add missions/Heavy lifting.py 2025-10-19 03:02:40 +00:00
08e0b9a521 Delete missions/Lift2.py 2025-10-08 23:21:43 +00:00
4533f952ed Add stuff from Johannes accidental fork 2025-10-07 12:32:40 +00:00
7eeead7d99 Sand Mission 2025-09-12 21:55:38 +00:00
e8176500f9 Send Over 2025-09-12 21:54:15 +00:00
c4aa9548ed Boat mission 2025-09-12 21:52:55 +00:00
33 changed files with 2 additions and 2092 deletions

View File

@@ -1,25 +1,3 @@
# 65266 Lego Dynamics - UNEARTHED Season Repository
# DO NOT MERGE THIS BRANCH!!!!!
## Project Overview
This repository contains the Pybricks code for Team 65266 Lego Dynamics' robot for the UNEARTHED season.
## Robot Hardware
* **Robot Name:** Optimus Prime III
* **Robot Firmware:** PyBricks firmware
* **Motors:** Two large motors for attachments, C left, D right, Two small motors for drive, A left, B right
* **Sensors:** visible up-facing color sensor for quick starts
* **Attachments:** Lots of 'em
## Code Structure
Files are the different runs we do, with each run consisting of one or multiple mission completions. Another script combines these files into a "master" file, which then adds the color-sensor-to-start logic.
## How to Use
Load the master file into PyBricks, then send it over to the robot. Then you hold a color LEGO up to the sensor to start it.
## License
GNU General Public License
You can take inspiration from our code, but you can't take our exact code.
# MERGING THIS BRANCH WILL CAUSE THE CODE TO BE REMOVED!!! THIS IS ONLY FOR A FILE SYSTEM! DO NOT, UNDER ANY CIRCUMSTANCES, MERGE ANYTHING INTO THIS BRANCH OR MERGE THIS BRANCH INTO ANY OTHER!!!!

View File

@@ -1,42 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, run_task, multitask
hub = PrimeHub()
# Initialize both motors. In this example, the motor on the
# left must turn counterclockwise to make the robot go forward.
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
arm_motor = Motor(Port.D, Direction.CLOCKWISE)
arm_motor_left= Motor(Port.C, Direction.CLOCKWISE)
# Initialize the drive base. In this example, the wheel diameter is 56mm.
# The distance between the two wheel-ground contact points is 112mm.
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
print('The default settings are: ' + str(drive_base.settings()))
drive_base.settings(300,1000,300,750)
# Optionally, uncomment the line below to use the gyro for improved accuracy.
drive_base.use_gyro(True)
async def main():
await drive_base.straight(519)
await arm_motor_left.run_angle(-10000, 300)
await arm_motor_left.run_angle(10000, 600)
await drive_base.straight(160)
await drive_base.turn(-30)
await drive_base.straight(50)
await arm_motor.run_angle(3000, 3000)
await drive_base.straight(-150)
await drive_base.turn(135)
await drive_base.straight(50)
await arm_motor.run_angle(10000, -3000)
await drive_base.straight(-100)
await drive_base.turn(-54)
await arm_motor.run_angle(10000, -3000)
await drive_base.straight(200)
await arm_motor.run_angle(10000, 10000)
run_task(main())

BIN
bluetooth.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 2.3 KiB

View File

@@ -1,40 +0,0 @@
# config.py - Robot configuration shared across all modules
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor
from pybricks.parameters import Port
# Initialize hub
hub = PrimeHub()
# Robot hardware configuration
ROBOT_CONFIG = {
# Drive motors
'left_motor': Motor(Port.A),
'right_motor': Motor(Port.B),
# Attachment motors
'attachment_motor': Motor(Port.C),
'lift_motor': Motor(Port.D),
# Sensors
#'color_left': ColorSensor(Port.E),
'color_back': ColorSensor(Port.F),
'ultrasonic': UltrasonicSensor(Port.E),
# Hub reference
'hub': hub
}
# Speed and distance constants
SPEEDS = {
'FAST': 400,
'NORMAL': 250,
'SLOW': 100,
'PRECISE': 50
}
DISTANCES = {
'TILE_SIZE': 300, # mm per field tile
'ROBOT_LENGTH': 180, # mm
'ROBOT_WIDTH': 140 # mm
}

View File

@@ -1,116 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port, Stop, Color, Direction
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, multitask, run_task
import asyncio
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
atarm1 = Motor(Port.E, Direction.COUNTERCLOCKWISE)
atarm2 = Motor(Port.F)
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=56, axle_track=112)
color_sensor = ColorSensor(Port.C)
drive_base.settings(300, 500, 300, 200)
Color.ORANGE = Color(10, 100, 100)
Color.MAGENTA = Color(321, 100, 86)
async def Run1():
left_arm.run_angle(1000, 300)
right_arm.run_angle(1000,500)
await drive_base.straight(320)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await drive_base.turn(-20)
await drive_base.straight(275)
await drive_base.turn(20)
await drive_base.straight(63)
await drive_base.turn(-30)
right_arm.run_angle(50,500)
await drive_base.turn(45)
await drive_base.straight(-135)
await drive_base.turn(-60)
await drive_base.straight(90)
await left_arm.run_angle(1000,-450)
await drive_base.straight(-145)
await left_arm.run_angle(1000,450)
await drive_base.straight(10)
await drive_base.turn(35)
await drive_base.straight(-500)
async def Run2():
await drive_base.straight(200)
await drive_base.turn(-20)
await drive_base.straight(525)
await drive_base.turn(60)
await drive_base.straight(50)
await right_arm.run_angle(2000,1000)
await drive_base.straight(-50)
await drive_base.turn(45)
await drive_base.straight(50)
await right_arm.run_angle(350,-1000)
await drive_base.turn(-100)
await drive_base.straight(-600)
async def Run3():
await drive_base.straight(915)
await drive_base.turn(-90)
await drive_base.straight(60)
await left_arm.run_angle(10000,-15000)
await drive_base.straight(-60)
await drive_base.turn(85)
await drive_base.straight(2000)
async def Run5():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
async def Run6():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
async def main():
if color_sensor.color() == Color.ORANGE:
await Run1()
if color_sensor.color() == Color.GREEN:
await Run2()
if color_sensor.color() == Color.WHITE:
await Run3()
if color_sensor.color() == Color.YELLOW:
await Run5()
if color_sensor.color() == Color.BLUE:
await Run6()
print(f'Detected color: {color_sensor.color()}')
# Main execution loop
while True:
run_task(main())
wait(100)

View File

@@ -1,113 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port, Stop, Color, Direction
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, multitask, run_task
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_arm = Motor(Port.D)
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
drive_base.settings(550,700,100,100)
drive_base.use_gyro(True)
async def Run1():
left_arm.run_angle(1000, 300)
right_arm.run_angle(1000,500)
await drive_base.straight(320)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await drive_base.turn(-20)
await drive_base.straight(275)
await drive_base.turn(20)
await drive_base.straight(63)
await drive_base.turn(-30)
right_arm.run_angle(50,500)
await drive_base.turn(45)
await drive_base.straight(-135)
await drive_base.turn(-60)
await drive_base.straight(90)
await left_arm.run_angle(1000,-450)
await drive_base.straight(-145)
await left_arm.run_angle(1000,450)
await drive_base.straight(10)
await drive_base.turn(35)
await drive_base.straight(-500)
async def Run2():
await drive_base.straight(200)
await drive_base.turn(-20)
await drive_base.straight(525)
await drive_base.turn(60)
await drive_base.straight(50)
await right_arm.run_angle(2000,1000)
await drive_base.straight(-50)
await drive_base.turn(45)
await drive_base.straight(50)
await right_arm.run_angle(350,-1000)
await drive_base.turn(-100)
await drive_base.straight(-600)
async def Run3():
await drive_base.straight(915)
await drive_base.turn(-90)
await drive_base.straight(60)
await left_arm.run_angle(10000,-15000)
await drive_base.straight(-60)
await drive_base.turn(85)
await drive_base.straight(2000)
async def Run5():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
async def Run6():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
def main():
if color_sensor.color() == Color.GREEN:
Run1()
if color_sensor.color() == Color.WHITE:
Run2()
if color_sensor.color() == Color.YELLOW:
Run3()
if color_sensor.color() == Color.BLUE:
Run5()
if color_sensor.color() == Color.RED:
Run6()
print(f'Detected color: {color_sensor.color()}')
# Main execution loop
while True:
main()
wait(100)

View File

@@ -1,102 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port, Stop, Color, Direction
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, multitask, run_task
async def Run1():
await multitask(left_arm.run_angle(1000, 300), right_arm.run_angle(1000,500))
await drive_base.straight(320)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await drive_base.turn(-20)
await drive_base.straight(275)
await drive_base.turn(20)
await drive_base.straight(63)
await multitask(drive_base.turn(-30), right_arm.run_angle(50,500))
await drive_base.turn(45)
await drive_base.straight(-135)
await drive_base.turn(-60)
await drive_base.straight(90)
await left_arm.run_angle(1000,-450)
await drive_base.straight(-145)
await left_arm.run_angle(1000,450)
await drive_base.straight(10)
await drive_base.turn(35)
await drive_base.straight(-500)
async def Run2():
await drive_base.straight(200)
await drive_base.turn(-20)
await drive_base.straight(525)
await drive_base.turn(60)
await drive_base.straight(50)
await right_arm.run_angle(2000,1000)
await drive_base.straight(-50)
await drive_base.turn(45)
await drive_base.straight(50)
await right_arm.run_angle(350,-1000)
await drive_base.turn(-100)
await drive_base.straight(-600)
async def Run3():
await drive_base.straight(915)
await drive_base.turn(-90)
await drive_base.straight(60)
await left_arm.run_angle(10000,-15000)
await drive_base.straight(-60)
await drive_base.turn(85)
await drive_base.straight(2000)
async def Run5():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
async def Run6():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
async def main():
while True:
detected_color = color_sensor.color(True)
if detected_color == Color.GREEN:
print('Detected color: Green')
await Run1()
elif detected_color == Color.WHITE:
print('Detected color: White')
await Run2()
elif detected_color == Color.YELLOW:
print('Detected color: Yellow')
await Run3()
elif detected_color == Color.BLUE:
print('Detected color: Blue')
await Run5()
elif detected_color == Color.RED:
print('Detected color: Red')
await Run6()
else:
print("None detected")
# Main execution
run_task(main())

View File

@@ -1,274 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port, Stop, Color, Direction
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, multitask, run_task
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_arm = Motor(Port.D)
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
drive_base.settings(550,700,100,100)
drive_base.use_gyro(True)
color_sensor = ColorSensor(Port.F)
hub.speaker.volume(50) # Set the volume of the speaker
cn = {
"Cs3": 138.59,
"D3": 146.83,
"Ds3": 155.56,
"E3": 164.81,
"F3": 174.61,
"Fs3": 185.00,
"G3": 196.00,
"Gs3": 207.65,
"A3": 220.00,
"As3": 233.08,
"B3": 246.94,
"C4": 261.63,
"Cs4": 277.18,
"D4": 293.66,
"Ds4": 311.13,
"E4": 329.63,
"F4": 349.23,
"Fs4": 369.99,
"G4": 392.00,
"Gs4": 415.30,
"A4": 440.00,
"As4": 466.16,
"B4": 493.88,
"C5": 523.25,
"Cs5": 554.37
}
async def Run1():
left_arm.run_angle(1000, 300)
right_arm.run_angle(1000,500)
await drive_base.straight(320)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await drive_base.turn(-20)
await drive_base.straight(275)
await drive_base.turn(20)
await drive_base.straight(63)
await drive_base.turn(-30)
right_arm.run_angle(50,500)
await drive_base.turn(45)
await drive_base.straight(-135)
await drive_base.turn(-60)
await drive_base.straight(90)
await left_arm.run_angle(1000,-450)
await drive_base.straight(-145)
await left_arm.run_angle(1000,450)
await drive_base.straight(10)
await drive_base.turn(35)
await drive_base.straight(-500)
async def Run2():
await drive_base.straight(200)
await drive_base.turn(-20)
await drive_base.straight(525)
await drive_base.turn(60)
await drive_base.straight(50)
await right_arm.run_angle(2000,1000)
await drive_base.straight(-50)
await drive_base.turn(45)
await drive_base.straight(50)
await right_arm.run_angle(350,-1000)
await drive_base.straight(-100)
await drive_base.turn(-100)
await drive_base.straight(-600)
async def Run3():
await drive_base.straight(915)
await drive_base.turn(-90)
await drive_base.straight(60)
await left_arm.run_angle(10000,-15000)
await drive_base.straight(-60)
await drive_base.turn(85)
await drive_base.straight(2000)
async def Run5():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
async def Run6():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
# Celebration sound types
class CelebrationSound:
VICTORY_FANFARE = 0
LEVEL_UP = 1
SUCCESS_CHIME = 2
TA_DA = 3
POWER_UP = 4
RICKROLL_INSPIRED = 5
async def play_victory_fanfare():
"""Classic victory fanfare"""
notes = [
(330, 200), # E4
(330, 100), # E4
(330, 100), # E4
(440, 600), # A4
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
await wait(50)
async def play_level_up():
"""Upward scale for level completion"""
notes = [
(cn["Cs4"], 100),
(cn["E4"], 100),
(cn["Cs4"], 100),
(cn["Cs5"], 100),
(cn["Cs4"], 100),
(cn["Gs4"], 100),
(cn["E4"], 100),
(cn["Cs4"], 100),
(cn["Gs4"], 100),
(cn["Cs4"], 100),
(cn["Cs5"], 100),
(cn["Gs4"], 100),
(cn["Cs4"], 100),
(cn["Gs4"], 100),
(cn["Cs5"], 100),
(cn["Gs4"], 100)
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
await wait(20)
async def play_success_chime():
"""Simple success notification"""
notes = [
(cn["E4"], 150),
(cn["Gs4"], 150),
(cn["B4"], 300),
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
await wait(50)
async def play_ta_da():
"""Classic "ta-da!" sound"""
notes = [
(cn["B3"], 200), # B4
(cn["E4"], 400), # E5
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
await wait(100)
async def play_power_up():
"""Rising power-up sound"""
frequencies = [
164.81, 207.65, 246.94, 329.63, 415.30, 493.88
]
for freq in frequencies:
await hub.speaker.beep(freq, 50)
await wait(10)
await hub.speaker.beep(659.24, 200)
async def play_rickroll_inspired():
"""Fun 80s-style dance beat inspired sound"""
# Upbeat bouncy rhythm
pattern = [
(cn["B3"], 200), (cn["Cs4"], 200), (cn["Ds4"], 200), (cn["E4"], 200),
(cn["Cs4"], 200), (cn["B3"], 200), (cn["A3"], 200), (cn["B3"], 300),
(cn["Cs4"], 200), (cn["B3"], 200), (cn["A3"], 200), (cn["Gs3"], 400),
]
for freq, duration in pattern:
await hub.speaker.beep(freq, duration)
await wait(50)
async def celebrate_mission_complete(sound_type=CelebrationSound.RICKROLL_INSPIRED):
"""
Main celebration function to call after completing a mission.
Plays a sound and shows light animation.
Args:
sound_type: CelebrationSound enum value (default: SUCCESS_CHIME)
"""
# Light show
hub.light.on(Color.GREEN)
# Play the selected celebration sound
if sound_type == CelebrationSound.VICTORY_FANFARE:
await play_victory_fanfare()
elif sound_type == CelebrationSound.LEVEL_UP:
await play_level_up()
elif sound_type == CelebrationSound.SUCCESS_CHIME:
await play_success_chime()
elif sound_type == CelebrationSound.TA_DA:
await play_ta_da()
elif sound_type == CelebrationSound.POWER_UP:
await play_power_up()
elif sound_type == CelebrationSound.RICKROLL_INSPIRED:
await play_rickroll_inspired()
else:
await play_success_chime() # Default fallback
# Blink the light
for _ in range(3):
hub.light.off()
await wait(100)
hub.light.on(Color.GREEN)
await wait(100)
hub.light.off()
async def main():
while True:
await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
color_reflected_percent = await color_sensor.reflection()
print(color_reflected_percent)
color_detected = await color_sensor.color()
print(f'Detected color: {color_detected}')
if color_detected == Color.GREEN:
print('Running Mission 1')
await Run1()
await celebrate_mission_complete(CelebrationSound.VICTORY_FANFARE)
elif color_detected == Color.WHITE:
print('Running Mission 2')
await Run2()
await celebrate_mission_complete(CelebrationSound.RICKROLL_INSPIRED)
elif color_detected == Color.YELLOW:
print('Running Mission 3')
await Run3()
await celebrate_mission_complete(CelebrationSound.SUCCESS_CHIME)
elif color_detected == Color.BLUE:
print('Running Mission 5')
await Run5()
await celebrate_mission_complete(CelebrationSound.POWER_UP)
elif color_detected == Color.RED:
print('Running Mission 6')
await Run6()
await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
else:
hub.light.off()
left_motor.stop()
right_motor.stop()
await wait(100) #prevent loop from iterating fast
# Main execution loop
run_task(main())

BIN
import.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 836 B

View File

View File

View File

@@ -1,7 +0,0 @@
I am Johannes
I am the building manager for the team. I like making art.
Parthiv, Diddy will touch you tonight.

View File

@@ -1 +0,0 @@
Hello my name is Vickram and I like coding and video games.

View File

@@ -1,3 +0,0 @@
About Me
I am Atharv, the Git manager for the team. I like to produce music.

View File

@@ -1,49 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
from pybricks.tools import run_task, multitask
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_arm = Motor(Port.D)
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
drive_base.settings(600,500,300,200)
drive_base.use_gyro(True)
async def main():
left_arm.run_angle(1000, 300)
right_arm.run_angle(1000,500)
await drive_base.straight(320)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await drive_base.turn(-20)
await drive_base.straight(275)
await drive_base.turn(20)
await drive_base.straight(63)
await drive_base.turn(-30)
right_arm.run_angle(50,500)
await drive_base.turn(45)
await drive_base.straight(-135)
await drive_base.turn(-60)
await drive_base.straight(90)
await left_arm.run_angle(1000,-450)
await drive_base.straight(-145)
await left_arm.run_angle(1000,450)
await drive_base.straight(10)
await drive_base.turn(35)
await drive_base.straight(-500)
run_task(main())

View File

@@ -1,34 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
from pybricks.tools import run_task, multitask
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_arm = Motor(Port.D)
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=64.8, axle_track=180)
drive_base.settings(600,500,300,200)
drive_base.use_gyro(True)
async def main():
await drive_base.straight(855)
await drive_base.turn(-90)
await drive_base.straight(50)
await left_arm.run_angle(10000,-15000)
await drive_base.straight(-50)
await drive_base.turn(90)
await drive_base.straight(2000)
run_task(main())

View File

@@ -1,43 +0,0 @@
# ---JOHANNES---
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
hub = PrimeHub()
# Initialize both motors. In this example, the motor on the
# left must turn counterclockwise to make the robot go forward.
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
arm_motor = Motor(Port.E, Direction.CLOCKWISE)
arm_motor.run_angle(299,90, Stop.HOLD)
# Initialize the drive base. In this example, the wheel diameter is 56mm.
# The distance between the two wheel-ground contact points is 112mm.
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=140)
print('The default settings are: ' + str(drive_base.settings()))
drive_base.settings(100,1000,166,750)
# Optionally, uncomment the line below to use the gyro for improved accuracy.
drive_base.use_gyro(True)
async def solveM9():
print("Solving Mission 9")
await drive_base.turn(45)
await drive_base.straight(260)
await arm_motor.run_angle(500,-500, Stop.HOLD)
await drive_base.straight(-40)
await drive_base.turn(92)
await drive_base.straight(-120)
await drive_base.straight(220)
await arm_motor.run_angle(500,100, Stop.HOLD)
await drive_base.turn(-50)
await drive_base.straight(-600)
async def main():
await drive_base.straight(50)
print("Hello, Robot is starting to run.")
await solveM9()
run_task(main())

BIN
r1l.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 154 KiB

BIN
r2l.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 149 KiB

BIN
r3l.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 139 KiB

BIN
r4l.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 152 KiB

BIN
r5.1l.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 141 KiB

BIN
r5.2l.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 140 KiB

BIN
run.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 878 B

View File

@@ -1,300 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port, Stop, Color, Direction
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, multitask, run_task
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
left_arm = Motor(Port.C)#, Direction.COUNTERCLOCKWISE)
right_arm = Motor(Port.D)
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
drive_base.settings(550,700,100,100)
drive_base.use_gyro(True)
color_sensor = ColorSensor(Port.F)
Color.ORANGE = Color(28, 61, 61)
Color.BLUE = Color(230,100,100)
Color.YELLOW = Color(37, 85, 95)
Color.PURPLE = Color(326, 60, 87)
color_sensor.detectable_colors([Color.ORANGE, Color.BLUE, Color.GREEN, Color.WHITE, Color.RED, Color.YELLOW, Color.NONE, Color.PURPLE])
hub.speaker.volume(50) # Set the volume of the speaker
color_sensor.detectable_colors()
# Celebration sound types
class CelebrationSound:
VICTORY_FANFARE = 0
LEVEL_UP = 1
SUCCESS_CHIME = 2
TA_DA = 3
POWER_UP = 4
RICKROLL_INSPIRED = 5
async def play_victory_fanfare():
"""Classic victory fanfare"""
notes = [
(262, 200), # C4
(262, 200), # C4
(262, 200), # C4
(349, 600), # F4
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
await wait(50)
async def play_level_up():
"""Upward scale for level completion"""
notesold = [
(262, 100), # C4
(294, 100), # D4
(330, 100), # E4
(349, 100), # F4
(392, 100), # G4
(440, 100), # A4
(494, 100), # B4
(523, 300), # C5
]
notes = [
(277, 100),
(330, 100),
(277, 100),
(554, 100),
(277, 100),
(413, 100),
(330, 100),
(277, 100),
(413, 100),
(277, 100),
(554, 100),
(413, 100),
(277, 100),
(413, 100),
(554, 100),
(413, 100)
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
#await wait(20)
async def play_success_chime():
"""Simple success notification"""
notes = [
(523, 150), # C5
(659, 150), # E5
(784, 300), # G5
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
await wait(50)
async def play_ta_da():
"""Classic "ta-da!" sound"""
notes = [
(392, 200), # G4
(523, 400), # C5
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
await wait(100)
async def play_power_up():
"""Rising power-up sound"""
for freq in range(200, 800, 50):
await hub.speaker.beep(freq, 50)
await wait(10)
await hub.speaker.beep(1000, 200)
async def play_rickroll_inspired():
"""Fun 80s-style dance beat inspired sound"""
# Upbeat bouncy rhythm
pattern = [
(392, 200), (440, 200), (494, 200), (523, 200),
(440, 200), (392, 200), (349, 200), (392, 300),
(440, 200), (392, 200), (349, 200), (330, 400),
]
for freq, duration in pattern:
await hub.speaker.beep(freq, duration)
await wait(50)
async def celebrate_mission_complete(sound_type=CelebrationSound.SUCCESS_CHIME):
"""
Main celebration function to call after completing a mission.
Plays a sound and shows light animation.
Args:
sound_type: CelebrationSound enum value (default: SUCCESS_CHIME)
"""
# Light show
hub.light.on(Color.GREEN)
# Play the selected celebration sound
if sound_type == CelebrationSound.VICTORY_FANFARE:
await play_victory_fanfare()
elif sound_type == CelebrationSound.LEVEL_UP:
await play_level_up()
elif sound_type == CelebrationSound.SUCCESS_CHIME:
await play_success_chime()
elif sound_type == CelebrationSound.TA_DA:
await play_ta_da()
elif sound_type == CelebrationSound.POWER_UP:
await play_power_up()
elif sound_type == CelebrationSound.RICKROLL_INSPIRED:
await play_rickroll_inspired()
else:
await play_success_chime() # Default fallback
# Blink the light
for _ in range(3):
hub.light.off()
await wait(100)
hub.light.on(Color.GREEN)
await wait(100)
hub.light.off()
async def Run1():
left_arm.run_angle(1000, -300)
right_arm.run_angle(1000,500)
await drive_base.straight(320)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await drive_base.turn(-20)
await drive_base.straight(277)
await drive_base.turn(20)
await drive_base.straight(65)
await drive_base.turn(-30)
right_arm.run_angle(50,500)
await drive_base.turn(45)
await drive_base.straight(-145)
await drive_base.turn(-60)
await drive_base.straight(90)
await left_arm.run_angle(1000, 450)
await drive_base.straight(-145)
await left_arm.run_angle(1000,-450)
await drive_base.straight(10)
await drive_base.turn(35)
await drive_base.straight(-700)
async def Run2():
await drive_base.straight(200)
await drive_base.turn(-20)
await drive_base.straight(525)
await drive_base.turn(60)
await drive_base.straight(50)
await right_arm.run_angle(2000,1000)
await drive_base.straight(-50)
await drive_base.turn(45)
await drive_base.straight(50)
await right_arm.run_angle(350,-1000)
await drive_base.straight(-100)
await drive_base.turn(-100)
await drive_base.straight(-750)
async def Run3():
await drive_base.straight(920)
await drive_base.turn(-90)
await drive_base.straight(60)
drive_base.turn(-10)
await left_arm.run_angle(10000,-4000)
await drive_base.straight(-110)
await drive_base.turn(90)
await drive_base.straight(2000)
async def Run4():
await drive_base.straight(519)
await left_arm.run_angle(300, -200)
await left_arm.run_angle(800, 1000)
await drive_base.straight(180)
await drive_base.turn(-37)
await right_arm.run_angle(300, -50)
await drive_base.straight(50)
await right_arm.run_angle(300, -350)
await drive_base.straight(-150)
await drive_base.turn(125)
await drive_base.straight(50)
await right_arm.run_angle(300, 400)
await drive_base.straight(-75)
await right_arm.run_angle(300, -200)
await drive_base.turn(-40)
await right_arm.run_angle(300, 250)
await drive_base.straight(260)
await right_arm.run_angle(100, -300)
await drive_base.straight(30)
await right_arm.run_angle(50,-250)
await drive_base.turn(30)
await drive_base.straight(-200)
await drive_base.turn(-67)
await drive_base.straight(-800)
async def Run5():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
async def Run6():
left_arm.run_angle(500,200)
right_arm.run_angle(500,200)
await drive_base.straight(70)
await drive_base.turn(-55)
await drive_base.straight(900)
await drive_base.turn(92.5)
await drive_base.straight(75)
await drive_base.straight(21)
await right_arm.run_angle(500,-250)
await right_arm.run_angle(500,250)
await drive_base.turn(55)
await left_arm.run_angle(300,-400)
await drive_base.turn(46.5)
await drive_base.turn(-40)
await drive_base.straight(900)
async def main():
while True:
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
color_reflected_percent = await color_sensor.reflection()
print(color_reflected_percent)
color_detected = await color_sensor.color()
print(f'Detected color: {color_detected.h}, {color_detected.s}, {color_detected.v}')
if color_reflected_percent > 1:
if color_detected == Color.GREEN:
print('Running Mission 1')
await Run1()
#await celebrate_mission_complete(CelebrationSound.VICTORY_FANFARE)
elif color_detected == Color.WHITE:
print('Running Mission 2')
await Run2()
#await celebrate_mission_complete(CelebrationSound.RICKROLL_INSPIRED)
elif color_detected == Color.YELLOW:
print('Running Mission 3')
await Run3()
#await celebrate_mission_complete(CelebrationSound.SUCCESS_CHIME )
elif color_detected == Color.BLUE:
print('Running Mission 4')
await Run4()
#await celebrate_mission_complete(CelebrationSound.POWER_UP)
elif color_detected == Color.ORANGE:
print('Running Mission 5')
await Run5()
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
elif color_detected == Color.PURPLE:
print('Running Mission 6 (this is ayaan\'s code)')
await Run6()
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
else:
hub.light.off()
left_motor.stop()
right_motor.stop()
await wait(1000) #prevent loop from iterating fast
# Main execution loop
run_task(main())

View File

@@ -1,345 +0,0 @@
# Imports... because we kinda need them...
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port, Stop, Color, Direction
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, multitask, run_task
# Sets all the default values for the runs
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
left_arm = Motor(Port.C)#, Direction.COUNTERCLOCKWISE)
right_arm = Motor(Port.D)
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
drive_base.settings(550,700,100,100)
drive_base.use_gyro(True)
color_sensor = ColorSensor(Port.F)
Color.ORANGE = Color(37, 85, 95)
Color.BLUE = Color(230,100,100)
Color.YELLOW = Color(53, 75, 84)
color_sensor.detectable_colors([Color.YELLOW, Color.BLUE, Color.GREEN, Color.WHITE, Color.RED, Color.ORANGE, Color.NONE])
hub.speaker.volume(50) # Set the volume of the speaker
color_sensor.detectable_colors()
# Notes to hertz for nice music
cn = {
"Cs3": 138.59,
"D3": 146.83,
"Ds3": 155.56,
"E3": 164.81,
"F3": 174.61,
"Fs3": 185.00,
"G3": 196.00,
"Gs3": 207.65,
"A3": 220.00,
"As3": 233.08,
"B3": 246.94,
"C4": 261.63,
"Cs4": 277.18,
"D4": 293.66,
"Ds4": 311.13,
"E4": 329.63,
"F4": 349.23,
"Fs4": 369.99,
"G4": 392.00,
"Gs4": 415.30,
"A4": 440.00,
"As4": 466.16,
"B4": 493.88,
"C5": 523.25,
"Cs5": 554.37
}
# RUNS - contains the mission code that will be executed on color sensor detection
async def Run1():
left_arm.run_angle(1000, 300)
right_arm.run_angle(1000,500)
await drive_base.straight(320)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await right_arm.run_angle(5000,500, Stop.HOLD)
await right_arm.run_angle(5000,-500, Stop.HOLD)
await drive_base.turn(-20)
await drive_base.straight(277)
await drive_base.turn(20)
await drive_base.straight(65)
await drive_base.turn(-30)
right_arm.run_angle(50,500)
await drive_base.turn(45)
await drive_base.straight(-145)
await drive_base.turn(-60)
await drive_base.straight(90)
await left_arm.run_angle(1000,-450)
await drive_base.straight(-145)
await left_arm.run_angle(1000,450)
await drive_base.straight(10)
await drive_base.turn(35)
await drive_base.straight(-600)
async def Run2():
await drive_base.straight(200)
await drive_base.turn(-20)
await drive_base.straight(525)
await drive_base.turn(60)
await drive_base.straight(50)
await right_arm.run_angle(2000,1000)
await drive_base.straight(-50)
await drive_base.turn(45)
await drive_base.straight(50)
await right_arm.run_angle(350,-1000)
await drive_base.straight(-100)
await drive_base.turn(-100)
await drive_base.straight(-600)
async def Run3():
await drive_base.straight(920)
await drive_base.turn(-90)
await drive_base.straight(60)
drive_base.turn(-10)
await left_arm.run_angle(10000,-4000)
await drive_base.straight(-110)
await drive_base.turn(90)
await drive_base.straight(2000)
async def Run4():
await drive_base.straight(519)
await left_arm.run_angle(300, -100)
await left_arm.run_angle(300, 500)
await drive_base.straight(180)
await drive_base.turn(-37)
await drive_base.straight(50)
await right_arm.run_angle(300, -400)
await drive_base.straight(-150)
await drive_base.turn(125)
await drive_base.straight(50)
await right_arm.run_angle(300, 400)
await drive_base.straight(-75)
await right_arm.run_angle(300, 300)
await drive_base.turn(-40)
await drive_base.straight(250)
await right_arm.run_angle(100, -300)
await drive_base.straight(30)
await right_arm.run_angle(50,-250)
await drive_base.turn(30)
await drive_base.straight(-200)
await drive_base.turn(-50)
await drive_base.straight(-800)
async def Run5():
await drive_base.straight(420)
await right_arm.run_angle(300,-100)
await drive_base.straight(-100)
await right_arm.run_angle(300, 100)
await drive_base.straight(-350)
async def Run6():
left_arm.run_angle(500,200)
right_arm.run_angle(500,200)
await drive_base.straight(70)
await drive_base.turn(-55)
await drive_base.straight(900)
await drive_base.turn(92.5)
await drive_base.straight(75)
await drive_base.straight(21)
await right_arm.run_angle(500,-250)
await right_arm.run_angle(500,250)
await drive_base.straight(-20)
await drive_base.turn(55)
await left_arm.run_angle(300,-400)
await drive_base.turn(70)
await left_arm.run_angle(300,400)
await drive_base.turn(-60)
await drive_base.straight(900)
# Celebration sound types
class CelebrationSound:
VICTORY_FANFARE = 0
WAITING_SOUND = 1
SUCCESS_CHIME = 2
TA_DA = 3
POWER_UP = 4
RICKROLL_INSPIRED = 5
# Sounds functions
async def play_victory_fanfare():
"""Classic victory fanfare"""
notes = [
(cn["B3"], 200),
(cn["B3"], 100),
(cn["B3"], 100),
(cn["E4"], 600)
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
await wait(50)
async def play_waiting_sound():
# T3rm1na1 V3l0c1ty arpeggio recreated in pybricks
notes = [
(cn["Cs4"], 100),
(cn["E4"], 100),
(cn["Cs4"], 100),
(cn["Cs5"], 100),
(cn["Cs4"], 100),
(cn["Gs4"], 100),
(cn["E4"], 100),
(cn["Cs4"], 100),
(cn["Gs4"], 100),
(cn["Cs4"], 100),
(cn["Cs5"], 100),
(cn["Gs4"], 100),
(cn["Cs4"], 100),
(cn["Gs4"], 100),
(cn["Cs5"], 100),
(cn["Gs4"], 100)
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
async def play_success_chime():
"""Simple success notification"""
notes = [
(cn["E4"], 150),
(cn["Gs4"], 150),
(cn["B4"], 300),
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
await wait(50)
async def play_ta_da():
"""Classic "ta-da!" sound"""
notes = [
(cn["Cs4"], 200),
(cn["Ds4"], 200),
(cn["E4"], 200),
(cn["Ds4"], 200),
(cn["E4"], 200),
(cn["Fs4"], 400),
(cn["Gs4"], 600),
(cn["Fs4"], 400),
(cn["Gs4"], 200),
(cn["A4"], 200),
(cn["B4"], 200),
(cn["C5"], 200),
(cn["Cs5"], 200)
]
for freq, duration in notes:
await hub.speaker.beep(freq, duration)
#await wait(100)
async def play_power_up():
"""Rising power-up sound"""
frequencies = [
164.81, 207.65, 246.94, 329.63, 415.30, 493.88
]
for freq in frequencies:
await hub.speaker.beep(freq, 50)
await wait(10)
await hub.speaker.beep(659.24, 200)
async def play_rickroll_inspired():
"""Fun 80s-style dance beat inspired sound"""
# Upbeat bouncy rhythm
pattern = [
(cn["Gs3"], 200), (cn["A3"], 200), (cn["B3"], 200), (cn["Cs4"], 200),
(cn["A3"], 200), (cn["Gs3"], 200), (cn["Fs3"], 200), (cn["Gs3"], 400),
(cn["A3"], 200), (cn["Gs3"], 200), (cn["Fs3"], 200), (cn["E3"], 400),
]
for freq, duration in pattern:
await hub.speaker.beep(freq, duration)
#await wait(50)
# Basically a big if else statement that calls the functions above
async def celebrate_mission_complete(sound_type=CelebrationSound.WAITING_SOUND):
"""
Main celebration function to call after completing a mission.
Plays a sound and shows light animation.
Args:
sound_type: CelebrationSound enum value (default: SUCCESS_CHIME)
"""
# Light show
hub.light.on(Color.GREEN)
# Play the selected celebration sound
if sound_type == CelebrationSound.VICTORY_FANFARE:
await play_victory_fanfare()
elif sound_type == CelebrationSound.WAITING_SOUND:
await play_waiting_sound()
elif sound_type == CelebrationSound.SUCCESS_CHIME:
await play_success_chime()
elif sound_type == CelebrationSound.TA_DA:
await play_ta_da()
elif sound_type == CelebrationSound.POWER_UP:
await play_power_up()
elif sound_type == CelebrationSound.RICKROLL_INSPIRED:
await play_rickroll_inspired()
else:
await play_success_chime() # Default fallback
# Blink the light
for _ in range(3):
hub.light.off()
await wait(100)
hub.light.on(Color.GREEN)
await wait(100)
hub.light.off()
# This where everything happens
async def main():
# MAIN LOOP
while True:
await celebrate_mission_complete(CelebrationSound.WAITING_SOUND)
color_reflected_percent = await color_sensor.reflection()
print(f"Color reflection percentage: {color_reflected_percent}")
color_detected = await color_sensor.color()
print(f'Detected color: {color_detected}')
hsv = await color_sensor.hsv()
print(f"Measured HSV: {hsv}")
if color_reflected_percent > 0:
if color_detected == Color.GREEN:
print('Running Mission 1')
await Run1()
await celebrate_mission_complete(CelebrationSound.VICTORY_FANFARE)
elif color_detected == Color.WHITE:
print('Running Mission 2')
await Run2()
await celebrate_mission_complete(CelebrationSound.RICKROLL_INSPIRED)
elif color_detected == Color.YELLOW:
print('Running Mission 3')
await Run3()
await celebrate_mission_complete(CelebrationSound.SUCCESS_CHIME)
elif color_detected == Color.ORANGE:
print('Running Mission 4')
await Run4()
await celebrate_mission_complete(CelebrationSound.POWER_UP)
elif color_detected == Color.BLUE:
print('Running Mission 5')
await Run5()
await celebrate_mission_complete(CelebrationSound.POWER_UP)
elif color_detected == Color.RED:
print('Running Mission 6')
await Run6()
await celebrate_mission_complete(CelebrationSound.TA_DA)
else:
hub.light.off()
left_motor.stop()
right_motor.stop()
else:
print("No color was detected.")
await wait(100) # prevent loop from iterating fast
run_task(main())

View File

@@ -1,145 +0,0 @@
# --- VICKRAM's CODE ---
# To do - make another version of this using DriveBase
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Stop
from pybricks.tools import wait, StopWatch, multitask
from umath import pi, sin, cos, radians
import asyncio
# Keep these---------------------------------------------------
class Tracker:
def __init__(self, starting_pos_x, starting_pos_y, starting_angle):
self.position = [starting_pos_x, starting_pos_y]
self.angle = starting_angle
def update(self, straight_distance, delta_angle):
delta_angle = radians(delta_angle) # Convert to radians
self.position[0] += straight_distance * cos(delta_angle) # Calculate x coordinate
self.position[1] += straight_distance * sin(delta_angle) # Calculate y coordinate
self.angle += delta_angle
def get_state():
return self.position, self. angle
class Attachment:
def __init__(self, port, start_angle=0):
self.motor = Motor(port)
self.start_angle = start_angle
def move(self, degrees, speed):
self.motor.reset_angle(0)
target_angle = degrees
tolerance = 2
# Movement with timeout
movement_timer = StopWatch()
movement_timeout = 3000 # 3 seconds timeout
while movement_timer.time() < movement_timeout:
current_angle = self.motor.angle()
error = target_angle - current_angle
if abs(error) <= tolerance:
self.motor.stop()
break
if error > 0:
self.motor.run(speed)
else:
self.motor.run(-speed)
wait(5)
self.motor.stop()
self.motor.reset_angle(0)
def reset(self):
self.motor.reset_angle(0)
self.move(self.start_angle)
# Initialize hub and motors
hub = PrimeHub()
left_motor = Motor(Port.C)
right_motor = Motor(Port.A)
hub.imu.reset_heading(0)
tracker = Tracker(0, 0, 0)
# Make sure to measure robot
def straight(distance, speed): # Distance in millimeters
target_heading = hub.imu.heading()
# Reset distance tracking
left_motor.reset_angle(0)
right_motor.reset_angle(0)
# Calculate target distance in motor degrees
wheel_circumference = pi * 62.4
target_degrees = abs(distance) / wheel_circumference * 360
while True:
# Check current distance traveled - Fixed: Make left_motor abs negative
left_angle = -abs(left_motor.angle())
right_angle = abs(right_motor.angle())
average_angle = (left_angle + right_angle) / 2
# Stop if it reached the target
if abs(average_angle) >= target_degrees:
break
# Get heading error for correction
current_heading = hub.imu.heading()
heading_error = target_heading - current_heading
# Handle wraparound at 0°/360°
if heading_error > 180:
heading_error -= 360
if heading_error < -180:
heading_error += 360
# Calculate motor speeds with correction
direction = 1 if distance > 0 else -1
correction = heading_error * 2.0
left_speed = -direction * (speed + correction)
right_speed = direction * (speed - correction)
# Limit speeds to prevent excessive values
left_speed = max(-200, min(200, left_speed))
right_speed = max(-200, min(200, right_speed))
# Apply speeds to motors
left_motor.run(left_speed)
right_motor.run(right_speed)
wait(10)
# Stop motors
left_motor.stop()
right_motor.stop()
wait(20)
tracker.update(distance, 0)
print(tracker.get_position())
def turn(theta, speed): # Negative value is left and positive is right
start_angle = hub.imu.heading()
target_angle = theta + start_angle
# Normalize target angle to -180 to 180 range
if target_angle > 180:
target_angle -= 360
if target_angle < -180:
target_angle += 360
while True:
current_angle = hub.imu.heading()
# Calculate angle error (how much we still need to turn)
angle_error = target_angle - current_angle
# Handle wraparound for shortest path
if angle_error > 180:
angle_error -= 360
if angle_error < -180:
angle_error += 360
# Stop if we're close enough (within 2 degrees)
if abs(angle_error) < 2:
break
# Determine turn direction and speed based on error
if angle_error > 0:
# Turn right
left_motor.run(-speed)
right_motor.run(-speed)
else:
# Turn left
left_motor.run(speed)
right_motor.run(speed)
wait(20)
# Stop both motors
left_motor.stop()
right_motor.stop()
trcker.update(0, theta)
print(tracker.get_position())
# Setup attachments here-------------------------------
attachment1 = Attachment(Port.D)
attachment2 = Attachment(Port.B)
# Run code goes here-----------------------------------
async def main():
#------------------------------------------------------
run_task(main())

View File

@@ -1,41 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
hub = PrimeHub()
# Robot hardware configurationROBOT_CONFIG = { # Drive motors 'left_motor': Motor(Port.A), 'right_motor': Motor(Port.B), # Attachment motors 'attachment_motor': Motor(Port.C), 'lift_motor': Motor(Port.D), # Sensors 'color_left': ColorSensor(Port.E), 'color_right': ColorSensor(Port.F), 'ultrasonic': UltrasonicSensor(Port.S1), # Hub reference 'hub': hub}# Speed and distance constantsSPEEDS = { 'FAST': 400, 'NORMAL': 250, 'SLOW': 100, 'PRECISE': 50}DISTANCES = { 'TILE_SIZE': 300, # mm per field tile 'ROBOT_LENGTH': 180, # mm 'ROBOT_WIDTH': 140 # mm}
def mission_run_1():
print('Running missions in Run 1')
def mission_run_2():
print('Running missions in Run 2')
def mission_run_3():
print('Running missions in Run 3')
# In main.py - sensor-based navigation
def main(self):
"""Use color sensor to select runs"""
print("Place colored object in front of sensor:")
print("RED=Run1, GREEN=Run2, BLUE=Run3, YELLOW=Test")
while True:
color = ROBOT_CONFIG['color_left'].color()
if color == Color.RED:
mission_run_1()
break
elif color == Color.GREEN:
mission_run_2()
break
elif color == Color.BLUE:
mission_run_3()
break
elif color == Color.YELLOW:
self.test_mode()
break
wait(1000)
main()

View File

@@ -1,134 +0,0 @@
# Guys please use the same setup code and put into the imports for consistency
script_names = ["Run1.py", "Run2.py", "Run3.py", "Run5.py", "Run6.py"] # This is a list of the files of the mission runs
content = ""
imports = """
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port, Stop, Color, Direction
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, multitask, run_task
import asyncio
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
atarm1 = Motor(Port.E, Direction.COUNTERCLOCKWISE)
atarm2 = Motor(Port.F)
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=56, axle_track=112)
color_sensor = ColorSensor(Port.C)
drive_base.settings(300, 500, 300, 200)
Color.ORANGE = Color(10, 100, 100)
Color.MAGENTA = Color(321, 100, 86)
"""
def extract_main_function(content):
lines = content.split('\n')
main_content = []
in_main_function = False
main_indent = 0
is_async = False
for line in lines:
stripped_line = line.strip()
# Find the start of main function
if stripped_line.startswith('def main') or stripped_line.startswith('async def main'):
in_main_function = True
is_async = stripped_line.startswith('async def main')
continue
if in_main_function:
# If we hit another function or class definition at the same level, we're done
if stripped_line and not line.startswith(' ') and not line.startswith('\t'):
if stripped_line.startswith('def ') or stripped_line.startswith('class '):
break
# Skip the first line after def main() if it's empty
if not stripped_line and not main_content:
continue
# If this is the first content line, determine the indent level
if main_content == [] and stripped_line:
main_indent = len(line) - len(line.lstrip())
# Remove the main function's indentation
if line.strip(): # Don't process empty lines
if len(line) - len(line.lstrip()) >= main_indent:
main_content.append(line[main_indent:])
else:
main_content.append(line)
else:
main_content.append('') # Keep empty lines
return '\n'.join(main_content), is_async
# Clear the main.py file and write the required imports
with open("main.py", 'w') as required_imports:
required_imports.write(imports)
function_calls = []
# Define colors properly - one per script
colors = [
'Color.ORANGE', 'Color.GREEN', 'Color.WHITE',
'Color.YELLOW', 'Color.BLUE', 'Color.MAGENTA', 'Color.RED', 'Color.BROWN'
]
# Process each script file and create individual functions
for i, f_name in enumerate(script_names):
try:
with open(f_name, 'r') as f:
content = f.read()
# Extract only the main function content
main_function_content, is_async = extract_main_function(content)
if main_function_content.strip(): # Only proceed if it found main function content
func_name = f_name.replace('.py', '').replace('-', '_')
func_def = f"\n{'async ' if is_async else ''}def {func_name}():\n"
indented_content = '\n'.join([' ' + line if line.strip() else line for line in main_function_content.split('\n')])
func_def += indented_content + "\n"
with open("main.py", 'a') as m:
m.write(func_def)
# Assign one color per script
color_condition = colors[i]
function_calls.append({
'name': func_name,
'is_async': is_async,
'color': color_condition,
'filename': f_name
})
else:
print(f"Warning: No main() function found in {f_name}")
except FileNotFoundError:
print(f"Warning: File {f_name} not found")
# Write the main function that checks colors and calls appropriate functions
with open("main.py", 'a') as m:
m.write("\nasync def main():\n")
for func_info in function_calls:
m.write(f" if color_sensor.color() == {func_info['color']}:\n")
if func_info['is_async']:
m.write(f" await {func_info['name']}()\n")
else:
m.write(f" {func_info['name']}()\n")
m.write(" \n")
# Add a default case
m.write(" \n")
m.write(" print(f'Detected color: {color_sensor.color()}')\n")
# Write the main loop
with open("main.py", 'a') as m:
m.write("\n# Main execution loop\n")
m.write("while True:\n")
m.write(" run_task(main())\n")
m.write(" wait(100)\n")

View File

@@ -1,13 +0,0 @@
#Speed and distance constants
SPEEDS = {
'FAST': 400,
'NORMAL': 250,
'SLOW': 100,
'PRECISE': 50
}
DISTANCES = {
'TILE_SIZE': 300, # mm per field tile
'ROBOT_LENGTH': 180, # mm
'ROBOT_WIDTH': 140 # mm
}

View File

@@ -1,187 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Stop
from pybricks.tools import wait, StopWatch, multitask
from umath import pi
class Attachment:
def __init__(self, port, start_angle=0):
self.motor = Motor(port)
self.start_angle = start_angle
def move_attachment(self, degrees, speed):
self.motor.reset_angle(0)
target_angle = degrees
tolerance = 2
movement_timer = StopWatch()
movement_timeout = 3000 # 3 seconds timeout
stuck_threshold = 50 # ms without significant movement
last_angle = 0
stuck_timer = StopWatch()
while movement_timer.time() < movement_timeout:
current_angle = self.motor.angle()
error = target_angle - current_angle
if abs(error) <= tolerance:
self.motor.stop()
# Check if motor is stuck
if abs(current_angle - last_angle) < 1: # Less than 1 degree movement
if stuck_timer.time() > stuck_threshold:
print("Motor appears stuck")
self.motor.stop()
else:
stuck_timer.reset()
last_angle = current_angle
if error > 0:
self.motor.run(speed)
else:
self.motor.run(-speed)
wait(10) # Consistent timing
self.motor.stop()
print("Movement timeout reached")
def reset_attachment(self):
self.motor.reset_angle(0)
return self.move_attachment(self.start_angle, 100)
class Robot:
def __init__(self, left_port, right_port, wheel_diameter):
# Initialize hub and motors
self.hub = PrimeHub()
try:
self.left_motor = Motor(left_port)
self.right_motor = Motor(right_port)
except Exception as e:
print(f"Failed to initialize motors: {e}")
# Reset and calibrate IMU
try:
self.hub.imu.reset_heading(0)
wait(1000) # Give IMU time to calibrate
except Exception as e:
print(f"Failed to initialize IMU: {e}")
# Robot specs
self.wheel_diameter = wheel_diameter
self.wheel_circumference = pi * self.wheel_diameter
# Attachments list
self.attachments = {}
def add_attachment(self, name, port, start_angle=0):
try:
self.attachments[name] = Attachment(port, start_angle)
except Exception as e:
print(f"Failed to add attachment '{name}': {e}")
def move_attachment(self, attachment_name, degrees, speed):
if attachment_name in self.attachments:
return self.attachments[attachment_name].move_attachment(degrees, speed)
else:
print(f"Attachment '{attachment_name}' not found")
def reset_attachment(self, attachment_name):
if attachment_name in self.attachments:
return self.attachments[attachment_name].reset_attachment()
else:
print(f"Attachment '{attachment_name}' not found")
def straight(self, distance, speed):
if not (-200 <= speed <= 200):
print(f"Invalid speed: {speed}. Must be between -200 and 200.")
target_heading = self.hub.imu.heading()
# Reset distance tracking
self.left_motor.reset_angle(0)
self.right_motor.reset_angle(0)
# Calculate target distance in motor degrees
target_degrees = abs(distance) / self.wheel_circumference * 360
while True:
# Check current distance traveled (assuming both motors should be positive for forward)
left_angle = abs(self.left_motor.angle())
right_angle = abs(self.right_motor.angle())
average_angle = (left_angle + right_angle) / 2
# Stop if it reached the target
if average_angle >= target_degrees:
break
# Get heading error for correction
current_heading = self.hub.imu.heading()
heading_error = target_heading - current_heading
# Handle wraparound at 0°/360°
if heading_error > 180:
heading_error -= 360
elif heading_error < -180:
heading_error += 360
# Calculate motor speeds with correction
direction = 1 if distance > 0 else -1
correction = heading_error * 1.5 # Reduced gain for stability
left_speed = direction * (speed + correction)
right_speed = direction * (speed - correction)
# Limit speeds to prevent excessive values
left_speed = max(-200, min(200, left_speed))
right_speed = max(-200, min(200, right_speed))
# Apply speeds to motors
self.left_motor.run(left_speed)
self.right_motor.run(right_speed)
wait(10) # Consistent timing
# Stop motors
self.left_motor.stop()
self.right_motor.stop()
wait(50) # Allow motors to fully stop
def turn(self, theta, speed):
if not (-200 <= speed <= 200):
print(f"Invalid speed: {speed}. Must be between -200 and 200.")
start_angle = self.hub.imu.heading()
target_angle = start_angle + theta
# Normalize target angle to -180 to 180 range
while target_angle > 180:
target_angle -= 360
while target_angle < -180:
target_angle += 360
while True:
current_angle = self.hub.imu.heading()
# Calculate angle error
angle_error = target_angle - current_angle
# Handle wraparound for shortest path
if angle_error > 180:
angle_error -= 360
elif angle_error < -180:
angle_error += 360
# Stop if we're close enough
if abs(angle_error) < 2:
break
# Positive angle_error means we need to turn clockwise (right)
if angle_error > 0:
# Turn right: left motor forward, right motor backward
self.left_motor.run(speed)
self.right_motor.run(-speed)
else:
# Turn left: left motor backward, right motor forward
self.left_motor.run(-speed)
self.right_motor.run(speed)
wait(10) # Consistent timing
# Stop both motors
self.left_motor.stop()
self.right_motor.stop()

View File

@@ -1,43 +0,0 @@
# utils/robot_control.py - Shared driving and control functions
from pybricks.robotics import DriveBase
from config import ROBOT_CONFIG
import time
# Initialize drive base (done once)
drive_base = DriveBase(
ROBOT_CONFIG['left_motor'],
ROBOT_CONFIG['right_motor'],
wheel_diameter=56,
axle_track=114
)
def drive_straight(distance, speed=200):
"""Drive straight for a given distance in mm"""
drive_base.straight(distance, speed=speed)
def turn_angle(angle, speed=100):
"""Turn by a given angle in degrees"""
drive_base.turn(angle, speed=speed)
def drive_until_line(speed=100, sensor='color_left'):
"""Drive until line is detected"""
sensor_obj = ROBOT_CONFIG[sensor]
drive_base.drive(speed, 0)
while sensor_obj.color() != Color.BLACK:
time.sleep(0.01)
drive_base.stop()
def return_to_base():
"""Return robot to launch area - implement based on your strategy"""
print("Returning to base...")
# Add your return-to-base logic here
pass
def reset_robot():
"""Reset all motors and sensors to default state"""
drive_base.stop()
for motor in ROBOT_CONFIG.get('attachment_motors', []):
motor.stop()
print("Robot reset completed")

View File

@@ -1,36 +0,0 @@
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
hub = PrimeHub()
# Initialize both motors. In this example, the motor on the
# left must turn counterclockwise to make the robot go forward.
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
arm_motor = Motor(Port.E, Direction.CLOCKWISE)
arm_motor.run_angle(299,90, Stop.HOLD)
# Initialize the drive base. In this example, the wheel diameter is 56mm.
# The distance between the two wheel-ground contact points is 112mm.
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=112)
print('The default settings are: ' + str(drive_base.settings()))
drive_base.settings(100,1000,166,750)
# Optionally, uncomment the line below to use the gyro for improved accuracy.
drive_base.use_gyro(True)
# Drive forward by 500mm (half a meter).
drive_base.straight(500)
# Turn around clockwise by 180 degrees.
drive_base.turn(180)
# Drive forward again to get back to the start.
drive_base.straight(500)
# Turn around counterclockwise.
drive_base.turn(-180)
arm_motor.run_angle(299,-90, Stop.HOLD)