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

dev #31

Merged
Atharv merged 136 commits from dev into ayaan_dev 2025-10-31 11:56:16 +00:00
Conversation 0 Commits 136 Files Changed 33 +80 -84
1 changed files with 80 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 2d2863726b - Show all commits

164
codes_for_scrimmage/hazmat/mainhazmatUPD.py
View File

@@ -7,95 +7,88 @@ 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)
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.settings(600, 500, 300, 200)
drive_base.use_gyro(True)
WALL_DISTANCE = 200 # mm
async def drive_forward():
"""Drive forward continuously using DriveBase."""
#await drive_base.straight(5000)
drive_base.drive(400,0)
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()
distance = lazer_ranger.distance()
if distance < WALL_DISTANCE:
# Stop the drivebase
await drive_base.turn(-180)
drive_base.brake
drive_base.brake()
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
# New Section
async def Run1(): # From M8_5.py
left_arm.run_angle(1000, 300)
right_arm.run_angle(1000,500)
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 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)
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 left_arm.run_angle(1000, -450)
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.turn(35)
await drive_base.straight(-600)
async def Run2(): #From Heavy_lifting_final.py
await drive_base.straight(200)
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 right_arm.run_angle(5000, 2900)
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.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
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)
left_arm.run_angle(500, 200)
right_arm.run_angle(500, 200)
await drive_base.straight(70)
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(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 left_arm.run_angle(300,-400)
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
drive_base.settings(600,500,300,200) #Reset it to the initial values
async def Run4(): #From Send_Over_Final.py
#Get to mission
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.turn(-90, Stop.HOLD)
await drive_base.straight(65)
#Solve mission
# Solve mission
drive_base.turn(-10)
await left_arm.run_angle(10000,-4000)
#Get to Red Start
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
# Add Rishi's code here
async def Run5():
await drive_base.straight(519)
await left_arm.run_angle(-10000, 300)
@@ -174,48 +156,62 @@ async def Run5():
await right_arm.run_angle(3000, 3000)
await drive_base.turn(-40)
#Add - Adi's code here
# Add - Adi's code here
async def Run6():
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 right_arm.run_angle(300, 100)
await drive_base.straight(-350)
# Function to classify color based on HSV
async def main:
def detect_color(h, s, v, reflected):
if reflected > 4:
if h < 4 or h > 350: #red
if h < 4 or h > 350: # red
return "Red"
print('Running Mission 3')
await Run3()
elif 3 < h < 40 and s > 70: #orange
elif 3 < h < 40 and s > 70: # orange
return "Orange"
print('Running Mission 6')
await Run6()
elif 47 < h < 56: #yellow
elif 47 < h < 56: # yellow
return "Yellow"
print('Running Mission 4')
await Run4()
elif 70 < h < 160: #green - do it vertically not horizontally for accuracy
elif 70 < h < 160: # green - do it vertically not horizontally for accuracy
return "Green"
print('Running Mission 1')
await Run1()
elif 210 < h < 225: #blue - do it vertically not horizontally for accuracy
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
return "Blue"
print('Running Mission 5')
await Run5()
elif 230 < h < 320: #purple
elif 260 < h < 350: # purple
return "Purple"
print('Running Mission 2')
await Run2()
else:
return "Unknown"
return "Unknown"
# Main loop
while True:
h, s, v = sensor.hsv()
reflected = sensor.reflection()
color = detect_color(h, s, v, reflected)
print(f"Detected Color: {color} (Hue: {h}, Sat: {s}, Val: {v})")
wait(1000)
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())