Delete codes_for_scrimmage/hazmat/hazmat_main.py
This commit is contained in:
@@ -1,356 +0,0 @@
|
|||||||
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()
|
|
||||||
|
|
||||||
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
|
|
||||||
drive_base.settings(300,1000,300,200) #Custom drive_base setting based on Ayaan's code
|
|
||||||
|
|
||||||
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)
|
|
||||||
|
|
||||||
drive_base.settings(600,500,300,200) #Reset it to the initial values
|
|
||||||
|
|
||||||
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():
|
|
||||||
pass
|
|
||||||
|
|
||||||
#Add - Adi's code here
|
|
||||||
async def Run6():
|
|
||||||
pass
|
|
||||||
|
|
||||||
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 > 1: # 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())
|
|
||||||
Reference in New Issue
Block a user