diff --git a/codes_for_scrimmage/hazmat/HazmatCodeChanges.py b/competition_codes/hazmat/HazmatCodeChanges.py similarity index 100% rename from codes_for_scrimmage/hazmat/HazmatCodeChanges.py rename to competition_codes/hazmat/HazmatCodeChanges.py diff --git a/codes_for_scrimmage/hazmat/Heavy_lifting_final.py b/competition_codes/hazmat/Heavy_lifting_final.py similarity index 100% rename from codes_for_scrimmage/hazmat/Heavy_lifting_final.py rename to competition_codes/hazmat/Heavy_lifting_final.py diff --git a/codes_for_scrimmage/hazmat/M8_5.py b/competition_codes/hazmat/M8_5.py similarity index 100% rename from codes_for_scrimmage/hazmat/M8_5.py rename to competition_codes/hazmat/M8_5.py diff --git a/codes_for_scrimmage/hazmat/Send_Over_Final.py b/competition_codes/hazmat/Send_Over_Final.py similarity index 100% rename from codes_for_scrimmage/hazmat/Send_Over_Final.py rename to competition_codes/hazmat/Send_Over_Final.py diff --git a/codes_for_scrimmage/hazmat/hazmat_main.py b/competition_codes/hazmat/hazmat_main.py similarity index 100% rename from codes_for_scrimmage/hazmat/hazmat_main.py rename to competition_codes/hazmat/hazmat_main.py diff --git a/codes_for_scrimmage/hazmat/mainhazmatUPD.py b/competition_codes/hazmat/mainhazmatUPD.py similarity index 100% rename from codes_for_scrimmage/hazmat/mainhazmatUPD.py rename to competition_codes/hazmat/mainhazmatUPD.py diff --git a/codes_for_scrimmage/hazmat/tip the scale.py b/competition_codes/hazmat/tip the scale.py similarity index 100% rename from codes_for_scrimmage/hazmat/tip the scale.py rename to competition_codes/hazmat/tip the scale.py diff --git a/codes_for_scrimmage/regional-final/Final_combined.py b/competition_codes/regional-final/Final_combined.py similarity index 100% rename from codes_for_scrimmage/regional-final/Final_combined.py rename to competition_codes/regional-final/Final_combined.py diff --git a/competition_codes/sectionals/sectional_main_dec_6.py b/competition_codes/sectionals/sectional_main_dec_6.py new file mode 100644 index 0000000..5a4bc39 --- /dev/null +++ b/competition_codes/sectionals/sectional_main_dec_6.py @@ -0,0 +1,372 @@ +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,Direction.CLOCKWISE) # Specify default direction +left_arm = Motor(Port.C, Direction.CLOCKWISE) # Specify default direction +right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration +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 drive_backward(): + """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 + drive_base.stop() + print(f"Wall detected at {distance}mm!") + break + if distance is None: + continue + + + # Small delay to prevent overwhelming the sensor + await wait(50) + +""" +Run#1 +- Removed forge and who lived here part +- What's on sale + Silo +- Green Key +""" +async def Run1(): + await solve_whats_on_sale() + await solve_silo() + + # return to base + await drive_base.straight(-90) + #await drive_base.turn(-100) + await drive_base.arc(200,None,-300) + drive_base.stop() + +async def solve_whats_on_sale(): + + right_arm.run_angle(500,30) + 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) + +async def solve_silo(): + 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(4000,-30, Stop.HOLD) + await right_arm.run_angle(4000,30, Stop.HOLD) + await right_arm.run_angle(4000,-30, Stop.HOLD) + await right_arm.run_angle(4000,30, Stop.HOLD) + await right_arm.run_angle(4000,-30, Stop.HOLD) + right_arm.run_angle(4000,30, Stop.HOLD) + + +""" +Run#2 +- This to solve forge, who lived here and heavy lifting combined +- Red Key +""" +async def Run2(): + await solve_forge() + await solve_heavy_lifting() + await solve_who_lived_here() + + # return to base + await drive_base.arc(-500,None,600) + drive_base.stop() + +async def solve_forge(): + await right_arm.run_target(50,50) + # await right_arm.run_angle(50,-30) + await drive_base.arc(350,113, None) + + await drive_base.straight(20) + await drive_base.turn(-60) + await drive_base.straight(-47) + +async def solve_heavy_lifting(): + await right_arm.run_angle(500,-160) # arm down + await wait(100) + await drive_base.turn(20) # turn right a little bit + await right_arm.run_angle(500,160) #arm up + await drive_base.turn(-20) #reset position + +async def solve_who_lived_here(): + await drive_base.straight(50) + await drive_base.turn(-15) + await drive_base.straight(50) + await drive_base.turn(-25) + await drive_base.straight(-50) + await drive_base.turn(-100) + +""" +Run#2.1 +- Alternate solution for Forge, Who lived here and Heavy Lifting combined +- Light Blue Key +- Different alignment +""" +async def Run2_1(): + await solve_forge_straight() + await solve_heavy_lifting() + await solve_who_lived_here() + + # return to base + await drive_base.arc(-500,None,600) + drive_base.stop() + +async def solve_forge_straight(): + await right_arm.run_target(50,50) + await right_arm.run_angle(50,-30) + await drive_base.straight(700) + # await drive_base.turn(-30) + # await drive_base.straight(20) + await drive_base.turn(-40) + await drive_base.straight(-30) + +""" +Run#3 +- Combined angler artifacts and tip the scale +- Yellow key +""" +async def Run3(): + await solve_angler_artifacts() + await solve_tip_the_scale() + + #cross over to red side + await multitask( + drive_forward(), + monitor_distance() + ) + +async def solve_angler_artifacts(): + await drive_base.straight(940) + await drive_base.turn(-90,Stop.HOLD) + await drive_base.straight(65) + #Solve + drive_base.turn(-10) + await left_arm.run_angle(10000,-3000) + await drive_base.straight(-110) + + await drive_base.turn(90) + await drive_base.arc(-150,-103, None) + +async def solve_tip_the_scale(): + await drive_base.turn(3) + await drive_base.straight(142.5) + await right_arm.run_angle(800,-150) + await right_arm.run_angle(900,150) + + await drive_base.straight(-100) + await drive_base.turn(-65) + await drive_base.straight(300,Stop.COAST_SMART) + await drive_base.arc(10,-47, None) + #await drive_base.turn(-23, Stop.COAST_SMART) + + + + + + +""" +Run #4 +- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal +- Blue Key +""" +async def Run4(): + await drive_base.straight(700) + await drive_base.turn(-18) + await drive_base.straight(120) + await drive_base.straight(-210) + await drive_base.turn(61) + await drive_base.straight(133) + await right_arm.run_angle(400, -200) + await drive_base.straight(90) + await right_arm.run_angle(100, 95) + await drive_base.straight(-875) + +async def solve_brush_reveal(): + await drive_base.straight(700) + await drive_base.turn(-20) + await drive_base.straight(110) + await drive_base.straight(-210) + +async def solve_mineshaft_explorer(): + await drive_base.turn(63) + await drive_base.straight(130) + await right_arm.run_angle(1000, -90) + await drive_base.straight(84) + await right_arm.run_angle(300, 90) + +""" +Run#5 +- Solves Salvage Operation + Statue Rebuild +- Orange Key +""" +async def Run5(): + await drive_base.straight(550) + await right_arm.run_angle(300,100) + await drive_base.straight(-75) + await right_arm.run_angle(300, -100) + await drive_base.straight(300) + await drive_base.straight(-200) + await drive_base.turn(-15) + #solving statue + await drive_base.straight(350) + await drive_base.turn(-104) + await drive_base.straight(-80) + await left_arm.run_angle(500, -900) + await drive_base.straight(120) + await drive_base.turn(5) + await left_arm.run_angle(500, 290) + await drive_base.turn(18) + await drive_base.straight(-100) + await drive_base.turn(-90) + await drive_base.straight(900) + drive_base.stop() + + +async def solve_salvage_operation(): + 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) + +async def solve_statue_rebuild(): + await drive_base.turn(-100) + await drive_base.straight(-80) + await left_arm.run_angle(500, -900) + await drive_base.straight(50) + await drive_base.straight(50) + await left_arm.run_angle(700, 250) + await drive_base.turn(30) + +""" +Run#6 +- Solve 2/3 Site Markings +- Run only if have time +- Purple Key +""" +async def Run6_7(): # experiment with ferris wheel for Site Markings + solve_site_mark_1() + solve_site_mark_2() + #return to base + await drive_base.straight(-300) + drive_base.stop() + +async def solve_site_mark_1(): + await drive_base.straight(500) + await right_arm.run_angle(100, -10) + await wait(50) + await drive_base.straight(-300) + await drive_base.arc(-150, -140, None) + +async def solve_site_mark_2(): + await drive_base.straight(-300) + await wait(50) + await right_arm.run_angle(50, 50) + + + +# 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 195 < h < 198: # light blue + return "Light_Blue" + 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() + print(color_sensor.color()) + print(h,s,v) + reflected = await color_sensor.reflection() + color = detect_color(h, s, v, reflected) + print(color) + + + if color == "Green": + print('Running Mission 1') + await Run1() + elif color == "Red": + print('Running Mission 2') + await Run2() + elif color == "Yellow": + print('Running Mission 3') + await Run3() + elif color == "Blue": + print('Running Mission 4') + await Run4() + elif color == "Orange": + print('Running Mission 5') + await Run5() + elif color == "Purple": + print('Running Mission 6') + await Run6_7() + elif color == "Light_Blue": + print("Running Mission 2_1") + await Run2_1() + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + await wait(10) +# Run the main function +run_task(main()) diff --git a/utils/tests/colorsensortest.py b/utils/tests/colorsensortest.py deleted file mode 100644 index 3d5a8d9..0000000 --- a/utils/tests/colorsensortest.py +++ /dev/null @@ -1,69 +0,0 @@ -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() - -color_sensor = ColorSensor(Port.F) - -# Color Settings -# https://docs.pybricks.com/en/latest/parameters/color.html -print("Default Detected Colors:", color_sensor.detectable_colors()) - -# Custom color Hue, Saturation, Brightness value for Lego bricks -Color.MAGENTA = Color(315,100,60) -Color.BLUE = Color(240,100,100) -Color.CYAN = Color(180,100,100) -Color.RED = Color(350, 100, 100) -LEGO_BRICKS_COLOR = [ - Color.BLUE, - Color.GREEN, - Color.WHITE, - Color.RED, - Color.YELLOW, - Color.MAGENTA, - Color.NONE -] -magenta_counter = 0 -stable_color = None -real_color = None -#Update Detectable colors -color_sensor.detectable_colors(LEGO_BRICKS_COLOR) -print(f'Yellow:{Color.YELLOW} : {Color.YELLOW.h}, {Color.YELLOW.s}, {Color.YELLOW.v}') -print("Updated Detected Colors:", color_sensor.detectable_colors()) -async def main(): - while True: - global magenta_counter, stable_color, real_color - color_reflected_percent = await color_sensor.reflection() - print("Reflection: ", color_reflected_percent) - if color_reflected_percent > 15: - color_detected = await color_sensor.color() - - if color_detected == Color.MAGENTA: - magenta_counter += 1 - else: - magenta_counter = 0 - stable_color = color_detected - - # Only accept magenta if it's been stable for a while - usually triggers before other colors so we gotta do this :| - if magenta_counter > 10: - stable_color = Color.MAGENTA - if stable_color != Color.MAGENTA: - stable_color = await color_sensor.color() - - real_color = stable_color - #if(color_detected != Color.NONE): - # return - - print("Magenta counter: ", magenta_counter) - if real_color is not None: - print(f'Detected color: {real_color} : {real_color.h}, {real_color.s}, {real_color.v}') - else: - print("No valid color detected yet.") - await wait(50) - -run_task(main()) \ No newline at end of file