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,-90) await drive_base.straight(200) await drive_base.turn(-40) await drive_base.straight(325) await left_arm.run_angle(500,90) await drive_base.straight(-100) await drive_base.straight(50) await left_arm.run_angle(500,-180) await drive_base.straight(-90) left_arm.run_angle(500,180) await drive_base.turn(-20) await drive_base.turn(15) await drive_base.straight(-173) await drive_base.turn(45) await drive_base.straight(120) 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(297) 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(-90) await drive_base.turn(-100) await drive_base.arc(-500,None,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 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(600) await drive_base.straight(-100) await drive_base.straight(150) await drive_base.turn(60) await drive_base.straight(100) await drive_base.turn(-86) await drive_base.straight(120) await drive_base.turn(-45) await drive_base.straight(-200) await drive_base.turn(75) # 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())