diff --git a/codes_for_scrimmage/hazmat/mainhazmatUPD.py b/codes_for_scrimmage/hazmat/mainhazmatUPD.py index daad2b2..6e785c7 100644 --- a/codes_for_scrimmage/hazmat/mainhazmatUPD.py +++ b/codes_for_scrimmage/hazmat/mainhazmatUPD.py @@ -145,256 +145,7 @@ async def Run5(): # Add - Adi's code here async def Run6(): -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) - -WALL_DISTANCE = 200 # mm - -async def drive_forward(): - """Drive forward continuously using DriveBase.""" - 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() - print('Distancing...',distance) - - if distance < WALL_DISTANCE: - # Stop the drivebase - await drive_base.stop - 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 - right_arm.run_angle(1000,450) - left_arm.run_angle(500,-80) - await drive_base.straight(200) - - await drive_base.turn(-40) - await drive_base.straight(325) - await left_arm.run_angle(500,80) - - await drive_base.straight(-100) - await drive_base.straight(50) - await left_arm.run_angle(500,-170) - - 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) - - -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 - left_arm.run_angle(600,-200) - right_arm.run_angle(500,200) - await drive_base.straight(70) - - await drive_base.turn(-70) - await drive_base.straight(900) - await drive_base.turn(115) - - await drive_base.straight(75) - await drive_base.straight(33) - await right_arm.run_angle(500,-250) - await right_arm.run_angle(500,250) - await drive_base.turn(66) - await drive_base.straight(7) - - await left_arm.run_angle(560,390) #going down - print('turning now...') - await drive_base.turn(40) # turning right - await left_arm.run_angle(410,-400) #lift a little bit - await drive_base.straight(80) - await drive_base.turn(-41) #ma din din din dun 67 41 21 - await drive_base.straight(900) - - -async def Run4(): # From Send_Over_Final.py - - await drive_base.straight(920) - await drive_base.turn(-90,Stop.HOLD) - await drive_base.straight(65) - #Solve - drive_base.turn(-10) - await left_arm.run_angle(10000,-4000) - await drive_base.straight(-110) - await drive_base.turn(90) - - await multitask( - drive_forward(), - monitor_distance() - ) - -# Add Rishi's code here -async def Run5(): - await drive_base.straight(700) - await drive_base.turn(-18) - await drive_base.straight(100) - await drive_base.straight(-205) - await drive_base.turn(63) - await drive_base.straight(125) - await right_arm.run_angle(1000, -1200) - await drive_base.straight(84) - await right_arm.run_angle(300, 1200) - await drive_base.straight(-875) - -# Add - Adi's code here -async def Run6(): - await drive_base.straight(500) - await right_arm.run_angle(300,500) - await drive_base.straight(-75) - await right_arm.run_angle(300, -900) - await drive_base.straight(-350) - await wait(1000) - await drive_base.straight(800) - await drive_base.straight(-200) - await drive_base.turn(-15) - await drive_base.straight(350) - await drive_base.turn(-94) - await drive_base.straight(-80) - await left_arm.run_angle(500, 900) - await drive_base.straight(50) - await drive_base.turn(-10) - await drive_base.straight(50) - await left_arm.run_angle(700, -200) - await drive_base.turn(30) - await drive_base.straight(-60) - await drive_base.turn(80) - await drive_base.straight(-900) - -# 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: - 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()) - -# 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: h, s, v = await color_sensor.hsv() reflected = await color_sensor.reflection()