FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed
Template
10
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

Update codes_for_scrimmage/hazmat/mainhazmatUPD.py

Browse Source
This commit is contained in:
Vickram
2025-10-24 00:35:06 +00:00
parent 12f3d97a9e
commit 2d2863726b
1 changed files with 80 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

164
codes_for_scrimmage/hazmat/mainhazmatUPD.py
View File

@@ -7,95 +7,88 @@ from pybricks.hubs import PrimeHub
# Initialize hub and devices # Initialize hub and devices
hub = PrimeHub() hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B) right_motor = Motor(Port.B)
left_arm = Motor(Port.C) left_arm = Motor(Port.C)
right_arm = Motor(Port.D) right_arm = Motor(Port.D)
lazer_ranger = UltrasonicSensor(Port.E) lazer_ranger = UltrasonicSensor(Port.E)
color_sensor = ColorSensor(Port.F) color_sensor = ColorSensor(Port.F)
# DriveBase configuration # DriveBase configuration
WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels) WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels)
AXLE_TRACK = 180 # mm (distance between wheels) AXLE_TRACK = 180 # mm (distance between wheels)
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
drive_base.settings(600,500,300,200) drive_base.settings(600, 500, 300, 200)
drive_base.use_gyro(True) drive_base.use_gyro(True)
WALL_DISTANCE = 200 # mm WALL_DISTANCE = 200 # mm
async def drive_forward(): async def drive_forward():
"""Drive forward continuously using DriveBase.""" """Drive forward continuously using DriveBase."""
#await drive_base.straight(5000) drive_base.drive(400, 0)
drive_base.drive(400,0)
async def monitor_distance(): async def monitor_distance():
"""Monitor ultrasonic sensor and stop when wall is detected.""" """Monitor ultrasonic sensor and stop when wall is detected."""
while True: while True:
distance = await lazer_ranger.distance() distance = lazer_ranger.distance()
if distance < WALL_DISTANCE: if distance < WALL_DISTANCE:
# Stop the drivebase # Stop the drivebase
await drive_base.turn(-180) await drive_base.turn(-180)
drive_base.brake drive_base.brake()
print(f"Wall detected at {distance}mm!") print(f"Wall detected at {distance}mm!")
break break
# Small delay to prevent overwhelming the sensor # Small delay to prevent overwhelming the sensor
await wait(50) await wait(50)
#New Section # New Section
async def Run1(): #From M8_5.py async def Run1(): # From M8_5.py
left_arm.run_angle(1000, 300) left_arm.run_angle(1000, 300)
right_arm.run_angle(1000,500) right_arm.run_angle(1000, 500)
await drive_base.straight(320) 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 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.turn(-20)
await drive_base.straight(277) await drive_base.straight(277)
await drive_base.turn(20) await drive_base.turn(20)
await drive_base.straight(65) await drive_base.straight(65)
await drive_base.turn(-30) await drive_base.turn(-30)
right_arm.run_angle(50,500) right_arm.run_angle(50, 500)
await drive_base.turn(45) await drive_base.turn(45)
await drive_base.straight(-145) await drive_base.straight(-145)
await drive_base.turn(-60) await drive_base.turn(-60)
await drive_base.straight(90) await drive_base.straight(90)
await left_arm.run_angle(1000,-450) await left_arm.run_angle(1000, -450)
await drive_base.straight(-145) await drive_base.straight(-145)
await left_arm.run_angle(1000,450) await left_arm.run_angle(1000, 450)
await drive_base.straight(10) await drive_base.straight(10)
await drive_base.turn(35) await drive_base.turn(35)
await drive_base.straight(-600) await drive_base.straight(-600)
async def Run2(): #From Heavy_lifting_final.py async def Run2(): # From Heavy_lifting_final.py
await drive_base.straight(200) await drive_base.straight(200)
await drive_base.turn(-20) await drive_base.turn(-20)
await drive_base.straight(536) await drive_base.straight(536)
await drive_base.turn(60, Stop.HOLD) await drive_base.turn(60, Stop.HOLD)
await drive_base.straight(30) await drive_base.straight(30)
await right_arm.run_angle(5000, 2900)
await right_arm.run_angle(5000,2900)
await drive_base.straight(40) await drive_base.straight(40)
await right_arm.run_angle(5000,-4000) await right_arm.run_angle(5000, -4000)
await drive_base.straight(-60) await drive_base.straight(-60)
await drive_base.turn(-60) await drive_base.turn(-60)
await drive_base.straight(-670) await drive_base.straight(-670)
async def Run3(): #tip the scale.py async def Run3(): # tip the scale.py
drive_base.settings(300,1000,300,200) #Custom drive_base setting based on Ayaan's code drive_base.settings(300, 1000, 300, 200) # Custom drive_base setting based on Ayaan's code
left_arm.run_angle(500,200) left_arm.run_angle(500, 200)
right_arm.run_angle(500,200) right_arm.run_angle(500, 200)
await drive_base.straight(70) await drive_base.straight(70)
await drive_base.turn(-55) await drive_base.turn(-55)
@@ -104,49 +97,38 @@ async def Run3(): #tip the scale.py
await drive_base.straight(75) await drive_base.straight(75)
await drive_base.straight(21) await drive_base.straight(21)
await right_arm.run_angle(500,-250) await right_arm.run_angle(500, -250)
await right_arm.run_angle(500,250) await right_arm.run_angle(500, 250)
await drive_base.turn(55) await drive_base.turn(55)
await left_arm.run_angle(300,-400) await left_arm.run_angle(300, -400)
await drive_base.turn(46.5) await drive_base.turn(46.5)
await drive_base.turn(-40) await drive_base.turn(-40)
await drive_base.straight(900) await drive_base.straight(900)
drive_base.settings(600, 500, 300, 200) # Reset it to the initial values
drive_base.settings(600,500,300,200) #Reset it to the initial values
async def Run4(): #From Send_Over_Final.py async def Run4(): # From Send_Over_Final.py
# Get to mission
#Get to mission
await drive_base.straight(920) await drive_base.straight(920)
await drive_base.turn(-90,Stop.HOLD) await drive_base.turn(-90, Stop.HOLD)
await drive_base.straight(65) await drive_base.straight(65)
#Solve mission # Solve mission
drive_base.turn(-10) drive_base.turn(-10)
await left_arm.run_angle(10000,-4000) await left_arm.run_angle(10000, -4000)
#Get to Red Start # Get to Red Start
await drive_base.straight(-110) await drive_base.straight(-110)
await drive_base.turn(90) await drive_base.turn(90)
await drive_base.straight(500) 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( await multitask(
drive_forward(), drive_forward(),
monitor_distance() monitor_distance()
) )
#Add Rishi's code here
# Add Rishi's code here
async def Run5(): async def Run5():
await drive_base.straight(519) await drive_base.straight(519)
await left_arm.run_angle(-10000, 300) await left_arm.run_angle(-10000, 300)
@@ -174,48 +156,62 @@ async def Run5():
await right_arm.run_angle(3000, 3000) await right_arm.run_angle(3000, 3000)
await drive_base.turn(-40) await drive_base.turn(-40)
#Add - Adi's code here
# Add - Adi's code here
async def Run6(): async def Run6():
await drive_base.straight(420) await drive_base.straight(420)
await right_arm.run_angle(300,-100) await right_arm.run_angle(300, -100)
await drive_base.straight(-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) await drive_base.straight(-350)
# Function to classify color based on HSV # Function to classify color based on HSV
async def main: def detect_color(h, s, v, reflected):
if reflected > 4: if reflected > 4:
if h < 4 or h > 350: #red if h < 4 or h > 350: # red
return "Red" return "Red"
print('Running Mission 3') elif 3 < h < 40 and s > 70: # orange
await Run3()
elif 3 < h < 40 and s > 70: #orange
return "Orange" return "Orange"
print('Running Mission 6') elif 47 < h < 56: # yellow
await Run6()
elif 47 < h < 56: #yellow
return "Yellow" return "Yellow"
print('Running Mission 4') elif 70 < h < 160: # green - do it vertically not horizontally for accuracy
await Run4()
elif 70 < h < 160: #green - do it vertically not horizontally for accuracy
return "Green" return "Green"
print('Running Mission 1') elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
await Run1()
elif 210 < h < 225: #blue - do it vertically not horizontally for accuracy
return "Blue" return "Blue"
print('Running Mission 5') elif 260 < h < 350: # purple
await Run5()
elif 230 < h < 320: #purple
return "Purple" return "Purple"
print('Running Mission 2')
await Run2()
else: else:
return "Unknown" return "Unknown"
return "Unknown"
# Main loop
while True: async def main():
h, s, v = sensor.hsv() while True:
reflected = sensor.reflection() h, s, v = await color_sensor.hsv()
color = detect_color(h, s, v, reflected) reflected = await color_sensor.reflection()
print(f"Detected Color: {color} (Hue: {h}, Sat: {s}, Val: {v})") color = detect_color(h, s, v, reflected)
wait(1000)
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())