solutions_season_unearthed/competition_codes/hazmat/HazmatCodeChanges.py

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())