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:
Johannes
2025-11-06 01:44:09 +00:00
parent 411f46d55e
commit acc44dac22
1 changed files with 57 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
codes_for_scrimmage/hazmat/mainhazmatUPD.py
View File

@@ -45,35 +45,45 @@ async def monitor_distance():
# New Section # New Section
async def Run1(): # From M8_5.py async def Run1(): # From M8_5.py
left_arm.run_angle(1000, 300) right_arm.run_angle(1000,450)
right_arm.run_angle(1000,500) left_arm.run_angle(500,-80)
await drive_base.straight(320) await drive_base.straight(200)
await right_arm.run_angle(5000,-500, Stop.HOLD) await drive_base.turn(-40)
await right_arm.run_angle(5000,500, Stop.HOLD) await drive_base.straight(325)
await right_arm.run_angle(5000,-500, Stop.HOLD) await left_arm.run_angle(500,80)
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(-100)
await drive_base.straight(277) await drive_base.straight(50)
await drive_base.turn(20) await left_arm.run_angle(500,-170)
await drive_base.straight(65)
await drive_base.straight(-270)
await drive_base.turn(40)
await drive_base.straight(135)
left_arm.run_angle(1000,-670)
await right_arm.run_angle(5000,-450, Stop.HOLD)
await right_arm.run_angle(5000,450, Stop.HOLD)
await right_arm.run_angle(5000,-450, Stop.HOLD)
await right_arm.run_angle(5000,450, Stop.HOLD)
await right_arm.run_angle(5000,-450, Stop.HOLD)
right_arm.run_angle(5000,450, Stop.HOLD)
await drive_base.turn(-35)
await drive_base.straight(300)
await drive_base.turn(63)
await drive_base.straight(170)
await drive_base.turn(-80)
await drive_base.straight(87)
await drive_base.turn(-15)
await drive_base.straight(-120)
await drive_base.turn(-100)
await drive_base.straight(300)
await drive_base.turn(-45)
await drive_base.straight(500)
await drive_base.turn(-30)
right_arm.run_angle(70,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(-150)
await left_arm.run_angle(1000,450)
await drive_base.straight(27.67) #turns back to solve market place
await drive_base.turn(90) #Will solve market place
await drive_base.straight(-450)
await drive_base.turn(70)
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)
@@ -145,7 +155,28 @@ async def Run5():
# Add - Adi's code here # Add - Adi's code here
async def Run6(): async def Run6():
await drive_base.stop
# 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: while True:
h, s, v = await color_sensor.hsv() h, s, v = await color_sensor.hsv()
reflected = await color_sensor.reflection() reflected = await color_sensor.reflection()