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(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 arm_motor.run_angle(1000, -1200) await drive_base.straight(84) await arm_motor.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())