From 2d2863726b6296c9dca0c8c0a18be051e97dcccc Mon Sep 17 00:00:00 2001 From: Vickram <31kapoov@elmbrookstudents.org> Date: Fri, 24 Oct 2025 00:35:06 +0000 Subject: [PATCH] Update codes_for_scrimmage/hazmat/mainhazmatUPD.py --- codes_for_scrimmage/hazmat/mainhazmatUPD.py | 164 ++++++++++---------- 1 file changed, 80 insertions(+), 84 deletions(-) diff --git a/codes_for_scrimmage/hazmat/mainhazmatUPD.py b/codes_for_scrimmage/hazmat/mainhazmatUPD.py index 37a00a9..a243e83 100644 --- a/codes_for_scrimmage/hazmat/mainhazmatUPD.py +++ b/codes_for_scrimmage/hazmat/mainhazmatUPD.py @@ -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()) \ No newline at end of file