From ef0ff44f2be6ebde4d52c094ad9a410200d295ef Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Sat, 25 Oct 2025 02:06:49 +0000 Subject: [PATCH] HazmatCodeChanges This is the code we changed at the scrimmage. --- .../hazmat/HazmatCodeChanges.py | 227 ++++++++++++++++++ 1 file changed, 227 insertions(+) create mode 100644 codes_for_scrimmage/hazmat/HazmatCodeChanges.py diff --git a/codes_for_scrimmage/hazmat/HazmatCodeChanges.py b/codes_for_scrimmage/hazmat/HazmatCodeChanges.py new file mode 100644 index 0000000..c48de0b --- /dev/null +++ b/codes_for_scrimmage/hazmat/HazmatCodeChanges.py @@ -0,0 +1,227 @@ +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 = 300 # 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 + left_arm.run_angle(1000, -300) + 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 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) + 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(-145) + 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) + 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 69 + await drive_base.straight(900) + + +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.straight(65) + #Solve mission + drive_base.turn(-10) + await left_arm.run_angle(10000, 4000) + #Get to Red Start + await drive_base.straight(-110) + await drive_base.turn(90) + # 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 +async def Run5(): + await drive_base.straight(519) + await left_arm.run_angle(-10000, 300) + await left_arm.run_angle(10000, 600) + await drive_base.straight(160) + await drive_base.turn(-30) + await drive_base.straight(50) + await right_arm.run_angle(3000, 3000) + await drive_base.straight(-150) + await drive_base.turn(120) + await drive_base.straight(25) + await right_arm.run_angle(10000, -3000) + await drive_base.straight(-110) + await drive_base.turn(-43) + await right_arm.run_angle(10000, -3000) + await drive_base.straight(295) + await right_arm.run_angle(10000, 9000) + await drive_base.straight(-65) + await drive_base.turn(45) + await drive_base.straight(175) + await drive_base.turn(24.5) + await drive_base.straight(-100) + await right_arm.run_angle(10000, -8500) + await drive_base.straight(100) + await right_arm.run_angle(10000, 3500) + await drive_base.turn(-30) + await drive_base.straight(-300) + await drive_base.turn(-80) + await drive_base.straight(-700) + + +# Add - Adi's code here +async def Run6(): + await drive_base.straight(750) + await drive_base.straight(-650) + + +# 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()) \ No newline at end of file