diff --git a/README.md b/README.md index 49a400b..5187815 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ ## Project Overview -Welcome to the official code repository for **Team 65266 Lego Dynamics**! This repository contains all the Pybricks code powering our robot through the UNEARTHED season missions. Our modular approach allows for flexible mission execution and quick color-sensor-based run selection. +Welcome to the official code repository for **Team 65266 - Lego Dynamics**! This repository contains all the Pybricks code powering our robot through the UNEARTHED season missions. Our modular approach allows for flexible mission execution and quick color-sensor-based run selection. --- @@ -26,8 +26,8 @@ Welcome to the official code repository for **Team 65266 Lego Dynamics**! This r |-----------|--------------| | **Robot Name** | Optimus Prime III | | **Firmware** | Pybricks | -| **Attachment Motors** | 2× Large Motors (Ports C & D) | -| **Drive Motors** | 2× Small Motors (Ports A & B) | +| **Attachment Motors** | 2x Large Motors (Ports C & D) | +| **Drive Motors** | 2x Small Motors (Ports A & B) | | **Sensors** | Up-facing Color Sensor (Quick Start) | | **Attachments** | Multiple mission-specific tools | @@ -46,10 +46,10 @@ Our codebase is organized for maximum efficiency and modularity: ``` Repository ┣ run_1.py # Individual mission runs - ┣ run_2.py # Each file = 1+ mission completions + ┣ run_2.py ┣ run_3.py ┣ ... - ┗ master.py # 🎯 Combined master file with color-start logic + ┗ masterFile.py # 🎯 Combined master file with color-start logic - this changes periodically as we release new versions. Check this README if you are unsure what code should be loaded on the robot. ``` ### Workflow @@ -61,34 +61,45 @@ Repository ## How to Use -### Installation & Deployment +### Installation & Deployment - from the server - everyday -1. **Load the Code** - ```bash - # Open the master.py file in Pybricks IDE - ``` +1. Download the file codes_for_scrimmage/hazmat/mainhazmatUPD.py + - You can do this through the repo, by using cURL, or by using git. + - Repo - Go to [codes_for_scrimmage/hazmat/mainhazmatUPD.py](codes_for_scrimmage/hazmat/mainhazmatUPD.py) and click the "Download" button. + - cURL or another HTTP data transferrer - -2. **Connect to Robot** - - Pair your robot via Bluetooth in Pybricks + ```curl -o mainhazmatUPD.py https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/main/codes_for_scrimmage/hazmat/mainhazmatUPD.py``` + - git CLI - -3. **Upload to Robot** - - Click "Download and Run" or send the program to the robot + ```git clone -b dev https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git && cd solutions_season_unearthed/codes_for_scrimmage/hazmat``` + + Then use mainhazmatUPD.py. + +2. Open https://code.pybricks.com/ and select the "Import a file" button on the top of the left bar. Import the .py file. Pair your robot via Bluetooth in Pybricks by selecting the Bluetooth button. + + - Import button looks like this: + ![alt text](https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/do-not-merge/import.png) + - Bluetooth button looks like this: + ![alt text](https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/do-not-merge/bluetooth.png) + +3. **Upload to Robot** - Click "Download and Run" or send the program to the robot + - Run button looks like this: ![alt text](https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/do-not-merge/run.png) + 4. **Start Your Run** - - Hold a colored LEGO brick up to the color sensor - - Different colors trigger different mission runs! + - If starting without Pybricks, press the center circular button on the SPIKE Prime Hub to start the program. + - Hold a colored LEGO brick up to the color sensor. + - Different colors trigger different mission runs - color mapping is below. ### Color Start System -| Color | Mission | Celebration Sound | -|-------|-----------|------------------| -| Green 🟩 | Run 1 | Victory Fanfare | -| White ⚪ | Run 2 | Rickroll Inspired | -| Yellow 🟨 | Run 3 | Success Chime | -| Orange 🟧 | Run 4 | Power Up | -| Blue 🟦 | Run 5 | Power Up | -| Red 🟥 | Run 6 | Ta-Da! | - -> **Tip** Organize your colored bricks before the match for quick run selection! +| Color | Mission | +|-------|-----------| +| Green 🟩 | Run 1 | +| Purple 🟪 | Run 2 | +| Red 🟥| Run 3 | +| Yellow 🟨| Run 4 | +| Blue 🟦| Run 5 | +| Orange 🟧| Run 6 | --- @@ -114,9 +125,7 @@ Team members can contribute by: **GNU General Public License v3.0** -``` You can take inspiration from our code, but you can't take our exact code. -``` This project is licensed under GPL-3.0 - see the [LICENSE](LICENSE) file for complete details. @@ -130,14 +139,8 @@ This project is licensed under GPL-3.0 - see the [LICENSE](LICENSE) file for com ## Contact & Support -**Team 65266 Lego Dynamics** +**Team 65266 - Lego Dynamics** Questions about our approach? Interested in collaboration? Reach out! ---- - -
- -Star this repo if you found it helpful! - -
\ No newline at end of file +--- \ No newline at end of file diff --git a/RoshoDaGoat.py b/RoshoDaGoat.py new file mode 100644 index 0000000..7d84ee2 --- /dev/null +++ b/RoshoDaGoat.py @@ -0,0 +1,42 @@ +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()) \ No newline at end of file diff --git a/codes_for_scrimmage/hazmat/HazmatCodeChanges.py b/codes_for_scrimmage/hazmat/HazmatCodeChanges.py new file mode 100644 index 0000000..c48de0b --- /dev/null +++ b/codes_for_scrimmage/hazmat/HazmatCodeChanges.py @@ -0,0 +1,227 @@ +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 +from pybricks.tools import wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub + +# Initialize hub and devices +hub = PrimeHub() +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B) +left_arm = Motor(Port.C) +right_arm = Motor(Port.D) +lazer_ranger = UltrasonicSensor(Port.E) +color_sensor = ColorSensor(Port.F) + +# DriveBase configuration +WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) +AXLE_TRACK = 180 # mm (distance between wheels) +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(600, 500, 300, 200) +drive_base.use_gyro(True) + +WALL_DISTANCE = 300 # mm + +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(400, 0) + + +async def monitor_distance(): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...',distance) + + if distance < WALL_DISTANCE: + # Stop the drivebase + await drive_base.stop + print(f"Wall detected at {distance}mm!") + break + + # Small delay to prevent overwhelming the sensor + await wait(50) + +# New Section +async def Run1(): # From M8_5.py + 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(): # From Heavy_lifting_final.py + await drive_base.straight(200) + await drive_base.turn(-20) + await drive_base.straight(536) + await drive_base.turn(60, Stop.HOLD) + await drive_base.straight(30) + await right_arm.run_angle(5000, 2900) + await drive_base.straight(40) + await right_arm.run_angle(5000, -4000) + await drive_base.straight(-60) + await drive_base.turn(-60) + await drive_base.straight(-670) + + +async def Run3(): # tip the scale.py + left_arm.run_angle(600,-200) + right_arm.run_angle(500,200) + await drive_base.straight(70) + + await drive_base.turn(-70) + await drive_base.straight(900) + await drive_base.turn(115) + + await drive_base.straight(75) + await drive_base.straight(33) + await right_arm.run_angle(500,-250) + await right_arm.run_angle(500,250) + await drive_base.turn(66) + await drive_base.straight(7) + + await left_arm.run_angle(560,390) #going down + print('turning now...') + await drive_base.turn(40) # turning right + await left_arm.run_angle(410,-400) #lift a little bit + await drive_base.straight(80) + await drive_base.turn(-41) #ma din din din dun 67 41 21 69 + await drive_base.straight(900) + + +async def Run4(): # From Send_Over_Final.py + #Get to mission + await drive_base.straight(920) + await drive_base.turn(-90,Stop.HOLD) + await drive_base.straight(65) + #Solve mission + drive_base.turn(-10) + await left_arm.run_angle(10000, 4000) + #Get to Red Start + await drive_base.straight(-110) + await drive_base.turn(90) + # while True: + # distance_mm = await lazer_ranger.distance() + # print('distancing...',distance_mm) + + # if distance_mm < 300: + # drive_base.stop + # break + # else: + # drive_base.straight(300) + # print('running...') + # await wait(10) + await multitask( + drive_forward(), + monitor_distance() + ) + + + +# Add Rishi's code here +async def Run5(): + await drive_base.straight(519) + await left_arm.run_angle(-10000, 300) + await left_arm.run_angle(10000, 600) + await drive_base.straight(160) + await drive_base.turn(-30) + await drive_base.straight(50) + await right_arm.run_angle(3000, 3000) + await drive_base.straight(-150) + await drive_base.turn(120) + await drive_base.straight(25) + await right_arm.run_angle(10000, -3000) + await drive_base.straight(-110) + await drive_base.turn(-43) + await right_arm.run_angle(10000, -3000) + await drive_base.straight(295) + await right_arm.run_angle(10000, 9000) + await drive_base.straight(-65) + await drive_base.turn(45) + await drive_base.straight(175) + await drive_base.turn(24.5) + await drive_base.straight(-100) + await right_arm.run_angle(10000, -8500) + await drive_base.straight(100) + await right_arm.run_angle(10000, 3500) + await drive_base.turn(-30) + await drive_base.straight(-300) + await drive_base.turn(-80) + await drive_base.straight(-700) + + +# Add - Adi's code here +async def Run6(): + await drive_base.straight(750) + await drive_base.straight(-650) + + +# Function to classify color based on HSV +def detect_color(h, s, v, reflected): + if reflected > 4: + if h < 4 or h > 350: # red + return "Red" + elif 3 < h < 40 and s > 70: # orange + return "Orange" + elif 47 < h < 56: # yellow + return "Yellow" + elif 70 < h < 160: # green - do it vertically not horizontally for accuracy + return "Green" + elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + return "Blue" + elif 260 < h < 350: # purple + return "Purple" + else: + return "Unknown" + return "Unknown" + + +async def main(): + while True: + h, s, v = await color_sensor.hsv() + reflected = await color_sensor.reflection() + color = detect_color(h, s, v, reflected) + + if color == "Red": + print('Running Mission 3') + await Run3() #red + elif color == "Orange": + print('Running Mission 6') + await Run6() #orange + elif color == "Yellow": + print('Running Mission 4') + await Run4() #yellow + elif color == "Green": + print('Running Mission 1') + await Run1() #green - vertically + elif color == "Blue": + print('Running Mission 5') + await Run5() #blue - vertically + elif color == "Purple": + print('Running Mission 2') + await Run2() #purple - vertically + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file diff --git a/codes_for_scrimmage/hazmat/Heavy_lifting_final.py b/codes_for_scrimmage/hazmat/Heavy_lifting_final.py new file mode 100644 index 0000000..86a67e9 --- /dev/null +++ b/codes_for_scrimmage/hazmat/Heavy_lifting_final.py @@ -0,0 +1,33 @@ +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(): + #await drive_base.straight(200) + await drive_base.turn(-20) + await drive_base.straight(536) + await drive_base.turn(60, Stop.HOLD) + await drive_base.straight(30) + + await right_arm.run_angle(5000,2900) + await drive_base.straight(40) + await right_arm.run_angle(5000,-4000) + await drive_base.straight(-60) + + await drive_base.turn(-60) + await drive_base.straight(-670) +run_task(main()) \ No newline at end of file diff --git a/codes_for_scrimmage/hazmat/M8_5.py b/codes_for_scrimmage/hazmat/M8_5.py new file mode 100644 index 0000000..95ecc9e --- /dev/null +++ b/codes_for_scrimmage/hazmat/M8_5.py @@ -0,0 +1,49 @@ +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(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) +run_task(main()) \ No newline at end of file diff --git a/codes_for_scrimmage/hazmat/Send_Over_Final.py b/codes_for_scrimmage/hazmat/Send_Over_Final.py new file mode 100644 index 0000000..db9a179 --- /dev/null +++ b/codes_for_scrimmage/hazmat/Send_Over_Final.py @@ -0,0 +1,46 @@ +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) +lazer_ranger = UltrasonicSensor(Port.E) + +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(): + #Get to mission + await drive_base.straight(920) + await drive_base.turn(-90,Stop.HOLD) + await drive_base.straight(65) + #Solve mission + drive_base.turn(-10) + await left_arm.run_angle(10000,-4000) + #Get to Red Start + await drive_base.straight(-110) + await drive_base.turn(90) + await drive_base.straight(500) + while True: + distance_mm = await lazer_ranger.distance() + print('distancing...',distance_mm) + + if distance_mm < 300: + drive_base.stop + break + else: + drive_base.straight(300) + print('running...') + await wait(10) + +run_task(main()) \ No newline at end of file diff --git a/codes_for_scrimmage/hazmat/hazmat_main.py b/codes_for_scrimmage/hazmat/hazmat_main.py new file mode 100644 index 0000000..ea87f5e --- /dev/null +++ b/codes_for_scrimmage/hazmat/hazmat_main.py @@ -0,0 +1,384 @@ +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 +from pybricks.tools import wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub + +# Initialize hub and devices +hub = PrimeHub() + +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B) + +left_arm = Motor(Port.C) +right_arm = Motor(Port.D) +lazer_ranger = UltrasonicSensor(Port.E) +color_sensor = ColorSensor(Port.F) + +# DriveBase configuration +WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) +AXLE_TRACK = 180 # mm (distance between wheels) +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(600,500,300,200) +drive_base.use_gyro(True) + +# Color Settings +# https://docs.pybricks.com/en/latest/parameters/color.html +print("Default Detected Colors:", color_sensor.detectable_colors()) + +# Custom color Hue, Saturation, Brightness value for Lego bricks +Color.MAGENTA = Color(300,100,100) +Color.VIOLET = Color(277,68,32) +Color.BLUE = Color(240,100,100) +Color.CYAN = Color(180,100,100) + +LEGO_BRICKS_COLOR = [ + Color.BLUE, + Color.GREEN, + Color.WHITE, + Color.RED, + Color.YELLOW, + Color.MAGENTA, + Color.VIOLET, + Color.NONE +] +#Update Detectable colors +color_sensor.detectable_colors(LEGO_BRICKS_COLOR) + +print("Updated Detected Colors:", color_sensor.detectable_colors()) + +hub.speaker.volume(50) # Set the volume of the speaker + +# 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() + +WALL_DISTANCE = 200 # mm +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + #await drive_base.straight(5000) + drive_base.drive(400,0) +async def monitor_distance(): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('distancing...',distance) + + if distance < WALL_DISTANCE: + # Stop the drivebase + await drive_base.turn(-180) + drive_base.brake + print(f"Wall detected at {distance}mm!") + break + + # Small delay to prevent overwhelming the sensor + await wait(50) + +async def Run1(): #From M8_5.py + 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(): #From Heavy_lifting_final.py + await drive_base.straight(200) + await drive_base.turn(-20) + await drive_base.straight(536) + await drive_base.turn(60, Stop.HOLD) + await drive_base.straight(30) + + await right_arm.run_angle(5000,2900) + await drive_base.straight(40) + await right_arm.run_angle(5000,-4000) + await drive_base.straight(-60) + + await drive_base.turn(-60) + await drive_base.straight(-670) + + +async def Run3(): #tip the scale.py + left_arm.run_angle(600,200) + right_arm.run_angle(500,200) + await drive_base.straight(70) + + await drive_base.turn(-70) + await drive_base.straight(900) + await drive_base.turn(115) + + await drive_base.straight(75) + await drive_base.straight(33) + await right_arm.run_angle(500,-250) + await right_arm.run_angle(500,250) + await drive_base.turn(66) + await drive_base.straight(7) + + await left_arm.run_angle(560,-390) #going down + await drive_base.turn(40) # turning right + await left_arm.run_angle(-410,-400) #lift a little bit + + await drive_base.turn(-46.5) #ma din din din dun + await drive_base.turn(-40) + await drive_base.straight(900) + +async def Run4(): #From Send_Over_Final.py + + #Get to mission + await drive_base.straight(920) + await drive_base.turn(-90,Stop.HOLD) + await drive_base.straight(65) + #Solve mission + drive_base.turn(-10) + await left_arm.run_angle(10000,-4000) + #Get to Red Start + await drive_base.straight(-110) + await drive_base.turn(90) + await drive_base.straight(500) + + # while True: + # distance_mm = await lazer_ranger.distance() + # print('distancing...',distance_mm) + + # if distance_mm < 300: + # drive_base.stop + # break + # else: + # drive_base.straight(300) + # print('running...') + # await wait(10) + await multitask( + drive_forward(), + monitor_distance() + ) + +#Add Rishi's code here +async def Run5(): + await drive_base.straight(519) + await left_arm.run_angle(-10000, 300) + await left_arm.run_angle(10000, 600) + await drive_base.straight(160) + await drive_base.turn(-30) + await drive_base.straight(50) + await right_arm.run_angle(3000, 3000) + await drive_base.straight(-150) + await drive_base.turn(135) + await drive_base.straight(25) + await right_arm.run_angle(10000, -3000) + await drive_base.straight(-100) + await drive_base.turn(-55) + await right_arm.run_angle(10000, -3000) + await drive_base.straight(250) + await drive_base.turn(-5) + await right_arm.run_angle(10000, 7000) + await drive_base.straight(-50) + await drive_base.turn(45) + await drive_base.straight(100) + await drive_base.turn(37) + await right_arm.run_angle(10000, -6000) + await drive_base.straight(90) + await right_arm.run_angle(3000, 3000) + await drive_base.turn(-40) + +#Add - Adi's code here +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: + + #await celebrate_mission_complete(CelebrationSound.LEVEL_UP) + color_reflected_percent = await color_sensor.reflection() + print(color_reflected_percent) + + + if color_reflected_percent > 0: # Make sure we actually have color reflections before checking for color + color_detected = await color_sensor.color() # Moved this inside the if clause + print(f'Detected color:{color_detected} : {color_detected.h}, {color_detected.s}, {color_detected.v}') + + 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.RED: + print('Running Mission 3') + await Run3() + #await celebrate_mission_complete(CelebrationSound.SUCCESS_CHIME ) + elif color_detected == Color.YELLOW: + 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.LEVEL_UP) + elif color_detected == Color.VIOLET: + print('Running Mission 6') + await Run6() + else: + hub.light.off() + left_motor.stop() + right_motor.stop() + color_detected = Color.NONE + else: + color_detected = Color.NONE + + await wait(1000) #prevent loop from iterating fast +# Main execution loop +run_task(main()) diff --git a/codes_for_scrimmage/hazmat/mainhazmatUPD.py b/codes_for_scrimmage/hazmat/mainhazmatUPD.py new file mode 100644 index 0000000..cebadd0 --- /dev/null +++ b/codes_for_scrimmage/hazmat/mainhazmatUPD.py @@ -0,0 +1,208 @@ +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 +from pybricks.tools import wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub + +# Initialize hub and devices +hub = PrimeHub() +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B) +left_arm = Motor(Port.C) +right_arm = Motor(Port.D) +lazer_ranger = UltrasonicSensor(Port.E) +color_sensor = ColorSensor(Port.F) + +# DriveBase configuration +WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) +AXLE_TRACK = 180 # mm (distance between wheels) +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(600, 500, 300, 200) +drive_base.use_gyro(True) + +WALL_DISTANCE = 300 # mm + +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(400, 0) + + +async def monitor_distance(): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...',distance) + + if distance < WALL_DISTANCE: + # Stop the drivebase + await drive_base.stop + print(f"Wall detected at {distance}mm!") + break + + # Small delay to prevent overwhelming the sensor + await wait(50) + +# New Section +async def Run1(): # From M8_5.py + 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(): # From Heavy_lifting_final.py + await drive_base.straight(200) + await drive_base.turn(-20) + await drive_base.straight(536) + await drive_base.turn(60, Stop.HOLD) + await drive_base.straight(30) + await right_arm.run_angle(5000, 2900) + await drive_base.straight(40) + await right_arm.run_angle(5000, -4000) + await drive_base.straight(-60) + await drive_base.turn(-60) + await drive_base.straight(-670) + + +async def Run3(): # tip the scale.py + left_arm.run_angle(600,-200) + right_arm.run_angle(500,200) + await drive_base.straight(70) + + await drive_base.turn(-70) + await drive_base.straight(900) + await drive_base.turn(115) + + await drive_base.straight(75) + await drive_base.straight(33) + await right_arm.run_angle(500,-250) + await right_arm.run_angle(500,250) + await drive_base.turn(66) + await drive_base.straight(7) + + await left_arm.run_angle(560,390) #going down + print('turning now...') + await drive_base.turn(40) # turning right + await left_arm.run_angle(410,-400) #lift a little bit + await drive_base.straight(80) + await drive_base.turn(-41) #ma din din din dun 67 41 21 69 + await drive_base.straight(900) + + +async def Run4(): # From Send_Over_Final.py + #Get to mission + await drive_base.straight(920) + await drive_base.turn(-90,Stop.HOLD) + await drive_base.straight(65) + #Solve mission + drive_base.turn(-10) + await left_arm.run_angle(10000, 4000) + #Get to Red Start + await drive_base.straight(-110) + await drive_base.turn(90) + # while True: + # distance_mm = await lazer_ranger.distance() + # print('distancing...',distance_mm) + + # if distance_mm < 300: + # drive_base.stop + # break + # else: + # drive_base.straight(300) + # print('running...') + # await wait(10) + await multitask( + drive_forward(), + monitor_distance() + ) + + + +# Add Rishi's code here +async def Run5(): + await drive_base.straight(600) + await drive_base.straight(-100) + await drive_base.straight(150) + await drive_base.turn(60) + await drive_base.straight(100) + await drive_base.turn(-86) + await drive_base.straight(120) + await drive_base.turn(-45) + await drive_base.straight(-200) + await drive_base.turn(75) + +# Add - Adi's code here +async def Run6(): + await drive_base.straight(750) + await drive_base.straight(-650) + + +# Function to classify color based on HSV +def detect_color(h, s, v, reflected): + if reflected > 4: + if h < 4 or h > 350: # red + return "Red" + elif 3 < h < 40 and s > 70: # orange + return "Orange" + elif 47 < h < 56: # yellow + return "Yellow" + elif 70 < h < 160: # green - do it vertically not horizontally for accuracy + return "Green" + elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + return "Blue" + elif 260 < h < 350: # purple + return "Purple" + else: + return "Unknown" + return "Unknown" + + +async def main(): + while True: + h, s, v = await color_sensor.hsv() + reflected = await color_sensor.reflection() + color = detect_color(h, s, v, reflected) + + if color == "Red": + print('Running Mission 3') + await Run3() #red + elif color == "Orange": + print('Running Mission 6') + await Run6() #orange + elif color == "Yellow": + print('Running Mission 4') + await Run4() #yellow + elif color == "Green": + print('Running Mission 1') + await Run1() #green - vertically + elif color == "Blue": + print('Running Mission 5') + await Run5() #blue - vertically + elif color == "Purple": + print('Running Mission 2') + await Run2() #purple - vertically + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file diff --git a/codes_for_scrimmage/hazmat/tip the scale.py b/codes_for_scrimmage/hazmat/tip the scale.py new file mode 100644 index 0000000..200b1ea --- /dev/null +++ b/codes_for_scrimmage/hazmat/tip the scale.py @@ -0,0 +1,42 @@ +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(300,1000,300,200) + +#drive_base.use_gyro(True) + +async def main(): + 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) +run_task(main()) \ No newline at end of file diff --git a/final/main5.py b/final/main5.py index 2b0fcd6..7461d72 100644 --- a/final/main5.py +++ b/final/main5.py @@ -203,27 +203,29 @@ async def Run3(): 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 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 right_arm.run_angle(300, -400) + await arm_motor.run_angle(3000, 3000) await drive_base.straight(-150) - await drive_base.turn(125) + await drive_base.turn(135) 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 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(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) + await drive_base.turn(-5) + await arm_motor.run_angle(10000, 7000) + await drive_base.straight(-50) + await drive_base.turn(68) + await arm_motor.run_angle(10000, -6000) + await drive_base.straight(200) + await arm_motor.run_angle(10000, 4000) + await drive_base.turn(-40) + await arm_motor.run_angle(10000, 7000) async def Run5(): await drive_base.straight(420) diff --git a/missions/Boat_mission.py b/missions/Boat_mission.py new file mode 100644 index 0000000..6d87949 --- /dev/null +++ b/missions/Boat_mission.py @@ -0,0 +1,27 @@ +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(880,850,700,700) + +drive_base.use_gyro(True) + +first_run = True + +async def main(): + await drive_base.straight(750) + await drive_base.straight(-650) + +run_task(main()) \ No newline at end of file diff --git a/missions/HEAVY_LIFTING_UPD.py b/missions/HEAVY_LIFTING_UPD.py new file mode 100644 index 0000000..da0f004 --- /dev/null +++ b/missions/HEAVY_LIFTING_UPD.py @@ -0,0 +1,33 @@ +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(): + await drive_base.straight(200) + await drive_base.turn(-20) + await drive_base.straight(536) + await drive_base.turn(60, Stop.HOLD) + await drive_base.straight(30) + + await right_arm.run_angle(5000,3000) + await drive_base.straight(40) + await right_arm.run_angle(5000,-4000) + await drive_base.straight(-60) + + await drive_base.turn(-60) + await drive_base.straight(-670) +run_task(main()) \ No newline at end of file diff --git a/missions/Heavy lifting.py b/missions/Heavy lifting.py new file mode 100644 index 0000000..643e47d --- /dev/null +++ b/missions/Heavy lifting.py @@ -0,0 +1,37 @@ +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(): + await right_arm.run_angle(2000,1000) + + await drive_base.straight(200) + await drive_base.turn(-20) + await drive_base.straight(525) + await drive_base.turn(60) + await drive_base.straight(30) + + await right_arm.run_angle(2000,-1000) + await drive_base.straight(30) + await right_arm.run_angle(3000,1000) + await drive_base.straight(-60) + + await drive_base.turn(-60) + await drive_base.straight(-525) + await drive_base.turn(20) + await drive_base.straight(-200) + diff --git a/missions/Heavy_lifting_final.py b/missions/Heavy_lifting_final.py new file mode 100644 index 0000000..5a9dd1d --- /dev/null +++ b/missions/Heavy_lifting_final.py @@ -0,0 +1,36 @@ +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(): + #Get to mission + await drive_base.straight(200) + await drive_base.turn(-20) + await drive_base.straight(525) + await drive_base.turn(60) + #Solve mission + 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) + #Return home + await drive_base.straight(-100) + await drive_base.turn(-100) + await drive_base.straight(-600) +run_task(main()) \ No newline at end of file diff --git a/missions/Lift2.py b/missions/Lift2.py new file mode 100644 index 0000000..a5e1773 --- /dev/null +++ b/missions/Lift2.py @@ -0,0 +1,34 @@ +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(): + 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) +run_task(main()) \ No newline at end of file diff --git a/missions/New_MapReveal/New_MapReveal_Mineshaft.py b/missions/New_MapReveal/New_MapReveal_Mineshaft.py new file mode 100644 index 0000000..2113b9b --- /dev/null +++ b/missions/New_MapReveal/New_MapReveal_Mineshaft.py @@ -0,0 +1,38 @@ +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 +from pybricks.tools import wait, StopWatch +from pybricks.robotics import DriveBase +from pybricks.hubs import PrimeHub + +# Initialize hub and devices +hub = PrimeHub() +left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) +right_motor = Motor(Port.B) +left_arm = Motor(Port.C) +right_arm = Motor(Port.D) +lazer_ranger = UltrasonicSensor(Port.E) +color_sensor = ColorSensor(Port.F) + +# DriveBase configuration +WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) +AXLE_TRACK = 180 # mm (distance between wheels) +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(600, 500, 300, 200) +drive_base.use_gyro(True) + +WALL_DISTANCE = 200 # mm + +async def main(): + await drive_base.straight(700) + await drive_base.turn(-20) + await drive_base.straight(110) + await drive_base.straight(-220) + await drive_base.turn(63) + await drive_base.straight(130) + await right_arm.run_angle(1000, -1200) + await drive_base.straight(84) + await right_arm.run_angle(300, 1200) + await drive_base.straight(-875) + +run_task(main()) \ No newline at end of file diff --git a/missions/Sand Mission.py b/missions/Sand Mission.py index 73e1e16..368ba5c 100644 --- a/missions/Sand Mission.py +++ b/missions/Sand Mission.py @@ -20,10 +20,10 @@ drive_base.use_gyro(True) async def main(): - await drive_base.straight(420) - await right_arm.run_angle(300,-100) + await drive_base.straight(500) + await right_arm.run_angle(300,100) await drive_base.straight(-100) - await right_arm.run_angle(300, 100) + await right_arm.run_angle(300,-100) await drive_base.straight(-350) run_task(main()) \ No newline at end of file diff --git a/missions/Sand_mission.py b/missions/Sand_mission.py new file mode 100644 index 0000000..368ba5c --- /dev/null +++ b/missions/Sand_mission.py @@ -0,0 +1,29 @@ +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(400,500,100,100) + +drive_base.use_gyro(True) + + +async def main(): + await drive_base.straight(500) + await right_arm.run_angle(300,100) + await drive_base.straight(-100) + await right_arm.run_angle(300,-100) + await drive_base.straight(-350) + +run_task(main()) \ No newline at end of file diff --git a/missions/Send_Over.py b/missions/Send_Over.py index 97bed5f..61e2d37 100644 --- a/missions/Send_Over.py +++ b/missions/Send_Over.py @@ -23,9 +23,9 @@ async def main(): 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 left_arm.run_angle(10000,-4000) + await drive_base.straight(-90) + await drive_base.turn(80) await drive_base.straight(2000) - + run_task(main()) \ No newline at end of file diff --git a/missions/Send_Over_Final.py b/missions/Send_Over_Final.py new file mode 100644 index 0000000..db9a179 --- /dev/null +++ b/missions/Send_Over_Final.py @@ -0,0 +1,46 @@ +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) +lazer_ranger = UltrasonicSensor(Port.E) + +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(): + #Get to mission + await drive_base.straight(920) + await drive_base.turn(-90,Stop.HOLD) + await drive_base.straight(65) + #Solve mission + drive_base.turn(-10) + await left_arm.run_angle(10000,-4000) + #Get to Red Start + await drive_base.straight(-110) + await drive_base.turn(90) + await drive_base.straight(500) + while True: + distance_mm = await lazer_ranger.distance() + print('distancing...',distance_mm) + + if distance_mm < 300: + drive_base.stop + break + else: + drive_base.straight(300) + print('running...') + await wait(10) + +run_task(main()) \ No newline at end of file diff --git a/missions/Set2.py b/missions/Set2.py new file mode 100644 index 0000000..9298da3 --- /dev/null +++ b/missions/Set2.py @@ -0,0 +1,50 @@ +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(250) + await drive_base.turn(-5) + await arm_motor.run_angle(10000, 7000) + await drive_base.straight(-50) + await drive_base.turn(68) + await arm_motor.run_angle(10000, -6000) + await drive_base.straight(200) + await arm_motor.run_angle(10000, 4000) + await drive_base.turn(-40) + await arm_motor.run_angle(10000, 7000) +run_task(main()) \ No newline at end of file diff --git a/missions/tip the scale.py b/missions/tip the scale.py index 200b1ea..3a4264e 100644 --- a/missions/tip the scale.py +++ b/missions/tip the scale.py @@ -15,28 +15,34 @@ 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(300,1000,300,200) +drive_base.settings(600,500,300,200) -#drive_base.use_gyro(True) +drive_base.use_gyro(True) async def main(): - left_arm.run_angle(500,200) + left_arm.run_angle(600,200) right_arm.run_angle(500,200) await drive_base.straight(70) - await drive_base.turn(-55) + await drive_base.turn(-70) await drive_base.straight(900) - await drive_base.turn(92.5) + await drive_base.turn(115) await drive_base.straight(75) - await drive_base.straight(21) + await drive_base.straight(33) await right_arm.run_angle(500,-250) await right_arm.run_angle(500,250) - await drive_base.turn(55) + await drive_base.turn(66) + await drive_base.straight(7) - await left_arm.run_angle(300,-400) + await left_arm.run_angle(560,-390) #going down + print('turning now...') + await drive_base.turn(40) # turning right + await left_arm.run_angle(-410,-400) #lift a little bit - await drive_base.turn(46.5) + await drive_base.turn(-46.5) #ma din din din dun await drive_base.turn(-40) await drive_base.straight(900) -run_task(main()) \ No newline at end of file +run_task(main()) + + diff --git a/test_10_17_2025.py b/test_10_17_2025.py new file mode 100644 index 0000000..1d89281 --- /dev/null +++ b/test_10_17_2025.py @@ -0,0 +1,303 @@ +# Stuff from 10/15/2025 +# Atharv trying with no avail to add more colors to the color sensor logic 😭😭😭😭😭😭😭 +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 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(250) + await drive_base.turn(-5) + await arm_motor.run_angle(10000, 7000) + await drive_base.straight(-50) + await drive_base.turn(68) + await arm_motor.run_angle(10000, -6000) + await drive_base.straight(200) + await arm_motor.run_angle(10000, 4000) + await drive_base.turn(-40) + await arm_motor.run_angle(10000, 7000) + +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())