From f389ff7101577e2caa92c7d67dd95e8d44a51b61 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Thu, 4 Dec 2025 01:46:03 +0000 Subject: [PATCH 01/75] Update competition_codes/sectionals/sectional_main_experimental.py --- competition_codes/sectionals/sectional_main_experimental.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/competition_codes/sectionals/sectional_main_experimental.py b/competition_codes/sectionals/sectional_main_experimental.py index 3deb707..d6accad 100644 --- a/competition_codes/sectionals/sectional_main_experimental.py +++ b/competition_codes/sectionals/sectional_main_experimental.py @@ -173,7 +173,7 @@ async def Run_3_4(): #combined angler artifacts and tip the scale - experimental await left_arm.run_angle(10000,-3000) await drive_base.straight(-110) await drive_base.turn(90) - await drive_base.arc(-150,-95, None) + await drive_base.arc(-150,-100, None) await drive_base.straight(100) await right_arm.run_angle(800,-150) From e8c25a4e906daf5dbbf6ae291cd6d2f7430f458a Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Thu, 4 Dec 2025 03:01:19 +0000 Subject: [PATCH 02/75] Update competition_codes/sectionals/sectional_main_experimental.py --- .../sectionals/sectional_main_experimental.py | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/competition_codes/sectionals/sectional_main_experimental.py b/competition_codes/sectionals/sectional_main_experimental.py index d6accad..715d7ba 100644 --- a/competition_codes/sectionals/sectional_main_experimental.py +++ b/competition_codes/sectionals/sectional_main_experimental.py @@ -21,7 +21,7 @@ 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 +WALL_DISTANCE = 200 # mm async def drive_forward(): """Drive forward continuously using DriveBase.""" @@ -113,14 +113,15 @@ async def Run2_1(): 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(-40) + await drive_base.straight(-47) await right_arm.run_angle(500,-90) # arm down await wait(100) await drive_base.turn(20) # turn right a little bit #await wait(100) - await right_arm.run_angle(500,90) #arm up + await right_arm.run_angle(500,140) #arm up await drive_base.turn(-20) #reset position await drive_base.straight(50) @@ -175,14 +176,14 @@ async def Run_3_4(): #combined angler artifacts and tip the scale - experimental await drive_base.turn(90) await drive_base.arc(-150,-100, None) - await drive_base.straight(100) + await drive_base.straight(135) await right_arm.run_angle(800,-150) await right_arm.run_angle(900,150) - await drive_base.straight(-80) - await drive_base.turn(-67) - await drive_base.straight(450,Stop.COAST_SMART) - await drive_base.arc(10,-25, None) + await drive_base.straight(-100) + await drive_base.turn(-65) + await drive_base.straight(320,Stop.COAST_SMART) + await drive_base.arc(10,-47, None) #await drive_base.turn(-23, Stop.COAST_SMART) await multitask( @@ -324,7 +325,7 @@ async def main(): if color == "Red": print('Running Mission 3') - await Run3() #red + await Run2_1() #red elif color == "Orange": print('Running Mission 6') await Run6_1() #orange @@ -342,9 +343,9 @@ async def main(): await Run6_7() #purple - vertically elif color == "Light_Blue": print("Running Mission 2_1") - await Run2_1() + await Run3() 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 +run_task(main()) From d235d602ce6ea39b58e3ea69a6865d62d1d3bb32 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Thu, 4 Dec 2025 23:33:57 +0000 Subject: [PATCH 03/75] Update competition_codes/sectionals/sectional_main_experimental.py --- .../sectionals/sectional_main_experimental.py | 265 +++++++++--------- 1 file changed, 132 insertions(+), 133 deletions(-) diff --git a/competition_codes/sectionals/sectional_main_experimental.py b/competition_codes/sectionals/sectional_main_experimental.py index 715d7ba..64952ff 100644 --- a/competition_codes/sectionals/sectional_main_experimental.py +++ b/competition_codes/sectionals/sectional_main_experimental.py @@ -50,8 +50,24 @@ async def monitor_distance(): # Small delay to prevent overwhelming the sensor await wait(50) -# Experimental - Removed forge and who lived here part -async def Run1(): # From M8_5.py +""" +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) @@ -69,6 +85,7 @@ async def Run1(): # From M8_5.py 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) @@ -81,91 +98,86 @@ async def Run1(): # From M8_5.py await right_arm.run_angle(4000,-30, Stop.HOLD) right_arm.run_angle(4000,30, 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) +""" +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() - await drive_base.straight(-90) - #await drive_base.turn(-100) - await drive_base.arc(200,None,-300) + # return to base + await drive_base.arc(-500,None,600) drive_base.stop() - -async def Run2(): # experiment with ferris wheel for silo - await drive_base.straight(410) - #await drive_base.turn(-20) - #await drive_base.straight(536) - #await drive_base.turn(60, Stop.HOLD) - #await drive_base.straight(30) - for i in range(3): - await right_arm.run_angle(10000, -360) - await drive_base.straight(-400) -# Experimental - This to solve forge, who lived here and heavy lifting combined -async def Run2_1(): +async def solve_forge(): await right_arm.run_target(50,50) - #await right_arm.re - #await multitask( 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,-90) # arm down await wait(100) await drive_base.turn(20) # turn right a little bit - #await wait(100) await right_arm.run_angle(500,140) #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) -async def Run3(): # tip the scale.py - right_arm.run_angle(500,30) - await drive_base.straight(800) - await drive_base.turn(90) - await drive_base.straight(86) - await right_arm.run_angle(800,-140) - await right_arm.run_angle(900,140) +""" +Run#3 +- Combined angler artifacts and tip the scale +- Yellow key +""" +async def Run3(): + await solve_angler_artifacts() + await solve_tip_the_scale() - await drive_base.straight(-100) - await drive_base.turn(90) - await drive_base.straight(800) - drive_base.stop() - - -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,-3000) - await drive_base.straight(-110) - await drive_base.turn(90) - + #cross over to red side await multitask( drive_forward(), monitor_distance() ) -async def Run_3_4(): #combined angler artifacts and tip the scale - experimental + +async def solve_angler_artifacts(): await drive_base.straight(920) await drive_base.turn(-90,Stop.HOLD) await drive_base.straight(65) @@ -173,9 +185,11 @@ async def Run_3_4(): #combined angler artifacts and tip the scale - experimental 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,-100, None) +async def solve_tip_the_scale(): await drive_base.straight(135) await right_arm.run_angle(800,-150) await right_arm.run_angle(900,150) @@ -186,27 +200,48 @@ async def Run_3_4(): #combined angler artifacts and tip the scale - experimental await drive_base.arc(10,-47, None) #await drive_base.turn(-23, Stop.COAST_SMART) - await multitask( - drive_forward(), - monitor_distance() - ) + +""" +Run #4 +- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal +- Blue Key +""" +async def Run4(): + await solve_brush_reveal() + await solve_mineshaft_explorer() + + #return to base + await drive_base.straight(-875) -# Add Rishi's code here -async def Run5(): +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) - await drive_base.straight(-875) -# Add - Adi's code here +""" +Run#5 +- Solves Salvage Operation + Statue Rebuild +- Orange Key +""" async def Run6(): + await solve_salvage_operation() + await solve_statue_rebuild() + + #return to base + await drive_base.straight(-60) + await drive_base.turn(80) + await drive_base.straight(-900) + +async def solve_salvage_operation(): await drive_base.straight(500) await right_arm.run_angle(300,500) await drive_base.straight(-75) @@ -217,6 +252,8 @@ async def Run6(): 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) @@ -224,71 +261,33 @@ async def Run6(): await drive_base.straight(50) await left_arm.run_angle(700, 250) await drive_base.turn(30) - await drive_base.straight(-60) - await drive_base.turn(80) - await drive_base.straight(-900) -async def arc_back(radius, angle): - #print(radius, angle) - drive_base.arc(radius, angle, None) - -async def lift_right_arm(): - await right_arm.run_angle(100, -90) - -# This is a refactor of Salvage operation mission without wait and realignment -async def Run6_1(): #experimental - - await drive_base.straight(500) - await right_arm.run_angle(100,90) - await drive_base.straight(-75) - - #ack back while raising arm - await multitask ( - lift_right_arm(), - arc_back(-215, -45) - ) - - #align to raise the ship - await drive_base.straight(-150) - await drive_base.turn(-45) - - #await drive_base.straight(-350) - #await wait(1000) - - #Solve ship - await drive_base.straight(800) - await drive_base.straight(-200) - await drive_base.turn(-15) - await drive_base.straight(350) - await drive_base.turn(-100) - await drive_base.straight(-80) - await left_arm.run_angle(500, -700) - await drive_base.straight(50) - await drive_base.straight(50) - await left_arm.run_angle(700, 250) - await drive_base.turn(15) - await drive_base.straight(-80) - await drive_base.turn(80) - await drive_base.straight(-900) +""" +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 Run6_7(): # experiment with ferris wheel for Site Markings +async def solve_site_mark_1(): await drive_base.straight(500) - await right_arm.run_angle(100, -10) await wait(50) - #await multitask ( - #lift_right_arm() - #await right_arm.run_angle(100, 10) - #await arc_back(-215, -140) 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) - #await drive_base.straight(-300) - drive_base.stop() + + # Function to classify color based on HSV def detect_color(h, s, v, reflected): @@ -323,27 +322,27 @@ async def main(): print(color) - if color == "Red": - print('Running Mission 3') - await Run2_1() #red - elif color == "Orange": - print('Running Mission 6') - await Run6_1() #orange - elif color == "Yellow": - print('Running Mission 4') - await Run_3_4() #yellow - elif color == "Green": + if color == "Green": print('Running Mission 1') - await Run1() #green - vertically - elif color == "Blue": - print('Running Mission 5') - await Run5() #blue - vertically - elif color == "Purple": + await Run1() + elif color == "Red": print('Running Mission 2') - await Run6_7() #purple - vertically + 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 Run3() + await Run2_1() else: print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") await wait(10) From 8bee9efe6be06154099a2c75faab8bcface6b79e Mon Sep 17 00:00:00 2001 From: Vickram <31kapoov@elmbrookstudents.org> Date: Thu, 4 Dec 2025 23:41:29 +0000 Subject: [PATCH 04/75] Add utils/tests/Diagnostics.py Returns all information about the robot --- utils/tests/Diagnostics.py | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 utils/tests/Diagnostics.py diff --git a/utils/tests/Diagnostics.py b/utils/tests/Diagnostics.py new file mode 100644 index 0000000..07d7766 --- /dev/null +++ b/utils/tests/Diagnostics.py @@ -0,0 +1,33 @@ +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 + +if hub.battery.voltage() > 7800: + print(f"Battery voltage is sufficient: {hub.battery.voltage()}") +elif hub.battery.voltage < 7800 : + print(f"Charging needed: {hub.battery.voltage()}") + +print(hub.battery.current()) +print(hub.imu.heading()) +print(hub.system.info()) \ No newline at end of file From d507a9c2040d4a3cf98059f0293f12c00c25d18a Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Thu, 4 Dec 2025 23:46:26 +0000 Subject: [PATCH 05/75] Update competition_codes/sectionals/sectional_main_experimental.py --- .../sectionals/sectional_main_experimental.py | 32 +++++++++++++------ 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/competition_codes/sectionals/sectional_main_experimental.py b/competition_codes/sectionals/sectional_main_experimental.py index 64952ff..a078cad 100644 --- a/competition_codes/sectionals/sectional_main_experimental.py +++ b/competition_codes/sectionals/sectional_main_experimental.py @@ -207,13 +207,17 @@ Run #4 - Blue Key """ async def Run4(): - await solve_brush_reveal() - await solve_mineshaft_explorer() - - #return to base + await drive_base.straight(700) + await drive_base.turn(-20) + await drive_base.straight(110) + await drive_base.straight(-210) + await drive_base.turn(60) + await drive_base.straight(130) + await right_arm.run_angle(1000, -200) + await drive_base.straight(90) + await right_arm.run_angle(300, 200) await drive_base.straight(-875) - async def solve_brush_reveal(): await drive_base.straight(700) await drive_base.turn(-20) @@ -233,10 +237,20 @@ Run#5 - Orange Key """ async def Run6(): - await solve_salvage_operation() - await solve_statue_rebuild() - - #return to base + 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) + await drive_base.straight(350) + await drive_base.turn(-103) + await drive_base.straight(-80) + await left_arm.run_angle(500, -900) + await drive_base.straight(101) + await left_arm.run_angle(700, 285) + await drive_base.turn(30) await drive_base.straight(-60) await drive_base.turn(80) await drive_base.straight(-900) From e8607d02ae89450a0f88d837376961d38628b88b Mon Sep 17 00:00:00 2001 From: Atharv <30nagava@elmbrookstudents.org> Date: Fri, 5 Dec 2025 16:28:00 +0000 Subject: [PATCH 06/75] Update utils/tests/ColorSensor.py --- utils/tests/{colorsensortest.py => ColorSensor.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename utils/tests/{colorsensortest.py => ColorSensor.py} (100%) diff --git a/utils/tests/colorsensortest.py b/utils/tests/ColorSensor.py similarity index 100% rename from utils/tests/colorsensortest.py rename to utils/tests/ColorSensor.py From 6fee096cf8082d8606ad228566b3075787d3a7b0 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sat, 6 Dec 2025 14:26:20 +0000 Subject: [PATCH 07/75] Update competition_codes/sectionals/sectional_main_experimental.py --- competition_codes/sectionals/sectional_main_experimental.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/competition_codes/sectionals/sectional_main_experimental.py b/competition_codes/sectionals/sectional_main_experimental.py index a078cad..21a9c0e 100644 --- a/competition_codes/sectionals/sectional_main_experimental.py +++ b/competition_codes/sectionals/sectional_main_experimental.py @@ -115,7 +115,7 @@ async def Run2(): async def solve_forge(): await right_arm.run_target(50,50) - await right_arm.run_angle(50,-30) + # await right_arm.run_angle(50,-30) await drive_base.arc(350,113, None) await drive_base.straight(20) From 8b2ab7b9c248c90c690bc682d2f6cfac8078562c Mon Sep 17 00:00:00 2001 From: Vickram <31kapoov@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:19:39 +0000 Subject: [PATCH 08/75] Upload files to "BatteryDiagnostics.py" --- BatteryDiagnostics.py/BatteryDiagnostics.py | 33 +++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 BatteryDiagnostics.py/BatteryDiagnostics.py diff --git a/BatteryDiagnostics.py/BatteryDiagnostics.py b/BatteryDiagnostics.py/BatteryDiagnostics.py new file mode 100644 index 0000000..07d7766 --- /dev/null +++ b/BatteryDiagnostics.py/BatteryDiagnostics.py @@ -0,0 +1,33 @@ +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 + +if hub.battery.voltage() > 7800: + print(f"Battery voltage is sufficient: {hub.battery.voltage()}") +elif hub.battery.voltage < 7800 : + print(f"Charging needed: {hub.battery.voltage()}") + +print(hub.battery.current()) +print(hub.imu.heading()) +print(hub.system.info()) \ No newline at end of file From ca0a9df8011789cce97000505994df211048d848 Mon Sep 17 00:00:00 2001 From: Vickram <31kapoov@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:20:14 +0000 Subject: [PATCH 09/75] Delete BatteryDiagnostics.py/BatteryDiagnostics.py --- BatteryDiagnostics.py/BatteryDiagnostics.py | 33 --------------------- 1 file changed, 33 deletions(-) delete mode 100644 BatteryDiagnostics.py/BatteryDiagnostics.py diff --git a/BatteryDiagnostics.py/BatteryDiagnostics.py b/BatteryDiagnostics.py/BatteryDiagnostics.py deleted file mode 100644 index 07d7766..0000000 --- a/BatteryDiagnostics.py/BatteryDiagnostics.py +++ /dev/null @@ -1,33 +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() -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 - -if hub.battery.voltage() > 7800: - print(f"Battery voltage is sufficient: {hub.battery.voltage()}") -elif hub.battery.voltage < 7800 : - print(f"Charging needed: {hub.battery.voltage()}") - -print(hub.battery.current()) -print(hub.imu.heading()) -print(hub.system.info()) \ No newline at end of file From 1a966fd553d1166fb594c86ae515ce2c24d879c0 Mon Sep 17 00:00:00 2001 From: Vickram <31kapoov@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:20:58 +0000 Subject: [PATCH 10/75] Upload files to "utils/tests/BatteryDiagnostics.py" --- .../BatteryDiagnostics.py | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 utils/tests/BatteryDiagnostics.py/BatteryDiagnostics.py diff --git a/utils/tests/BatteryDiagnostics.py/BatteryDiagnostics.py b/utils/tests/BatteryDiagnostics.py/BatteryDiagnostics.py new file mode 100644 index 0000000..07d7766 --- /dev/null +++ b/utils/tests/BatteryDiagnostics.py/BatteryDiagnostics.py @@ -0,0 +1,33 @@ +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 + +if hub.battery.voltage() > 7800: + print(f"Battery voltage is sufficient: {hub.battery.voltage()}") +elif hub.battery.voltage < 7800 : + print(f"Charging needed: {hub.battery.voltage()}") + +print(hub.battery.current()) +print(hub.imu.heading()) +print(hub.system.info()) \ No newline at end of file From 61d3dd3c06e0b04906455f3908928de7eae9bf62 Mon Sep 17 00:00:00 2001 From: Vickram <31kapoov@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:21:24 +0000 Subject: [PATCH 11/75] Delete utils/tests/BatteryDiagnostics.py/BatteryDiagnostics.py --- .../BatteryDiagnostics.py | 33 ------------------- 1 file changed, 33 deletions(-) delete mode 100644 utils/tests/BatteryDiagnostics.py/BatteryDiagnostics.py diff --git a/utils/tests/BatteryDiagnostics.py/BatteryDiagnostics.py b/utils/tests/BatteryDiagnostics.py/BatteryDiagnostics.py deleted file mode 100644 index 07d7766..0000000 --- a/utils/tests/BatteryDiagnostics.py/BatteryDiagnostics.py +++ /dev/null @@ -1,33 +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() -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 - -if hub.battery.voltage() > 7800: - print(f"Battery voltage is sufficient: {hub.battery.voltage()}") -elif hub.battery.voltage < 7800 : - print(f"Charging needed: {hub.battery.voltage()}") - -print(hub.battery.current()) -print(hub.imu.heading()) -print(hub.system.info()) \ No newline at end of file From 821924e8757f7a37f0e7f8acf370aac34fd0cd44 Mon Sep 17 00:00:00 2001 From: Vickram <31kapoov@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:22:50 +0000 Subject: [PATCH 12/75] Upload files to "utils/tests/FullDiagnostics" --- .../FullDiagnostics/BatteryDiagnostics.py | 33 +++++++++++++++++ .../tests/FullDiagnostics/FullDiagnostics.py | 35 +++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 utils/tests/FullDiagnostics/BatteryDiagnostics.py create mode 100644 utils/tests/FullDiagnostics/FullDiagnostics.py diff --git a/utils/tests/FullDiagnostics/BatteryDiagnostics.py b/utils/tests/FullDiagnostics/BatteryDiagnostics.py new file mode 100644 index 0000000..07d7766 --- /dev/null +++ b/utils/tests/FullDiagnostics/BatteryDiagnostics.py @@ -0,0 +1,33 @@ +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 + +if hub.battery.voltage() > 7800: + print(f"Battery voltage is sufficient: {hub.battery.voltage()}") +elif hub.battery.voltage < 7800 : + print(f"Charging needed: {hub.battery.voltage()}") + +print(hub.battery.current()) +print(hub.imu.heading()) +print(hub.system.info()) \ No newline at end of file diff --git a/utils/tests/FullDiagnostics/FullDiagnostics.py b/utils/tests/FullDiagnostics/FullDiagnostics.py new file mode 100644 index 0000000..af8a6b0 --- /dev/null +++ b/utils/tests/FullDiagnostics/FullDiagnostics.py @@ -0,0 +1,35 @@ +from pybricks.hubs import PrimeHub +from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor +from pybricks.parameters import Button, Color, Direction, Port, Side, Stop +from pybricks.robotics import DriveBase +from pybricks.tools import wait, StopWatch +hub = PrimeHub() +from BatteryDiagnostics import BatteryDiagnostics +battery = BatteryDiagnostics() +clearConfirmation = input("Do you want to clear the console before proceeding? Y/N (default: yes): ") +if(clearConfirmation == "Y" or clearConfirmation == "y" or clearConfirmation == "yes" or clearConfirmation == ""): + print("Clearing console... \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n") + +while True: + print("\nWhat diagnostic do you want to perform?") + print("Enter 'b' for Battery diagnostics") + print("Enter 'm' for Motor diagnostics") + print("Enter 'q' to Quit") + + choice = input("Your choice: ").strip().lower() + + if choice == "b": + print("-----------------------BATTERY DIAGNOSTICS-----------------------") + battery.printAll() + + elif choice == "m": + print("------------------------MOTOR DIAGNOSTICS------------------------") + # motor.printAll() # call your motor diagnostics here + print("Motor diagnostics would run here.") + + elif choice == "q": + print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!") + break + + else: + print("Invalid choice. Please enter 'b', 'm', or 'q'.") \ No newline at end of file From 1db4458e164fa644535d639109ac908ec6eb3a58 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:44:20 +0000 Subject: [PATCH 13/75] Delete utils/tests/FullDiagnostics/BatteryDiagnostics.py --- .../FullDiagnostics/BatteryDiagnostics.py | 33 ------------------- 1 file changed, 33 deletions(-) delete mode 100644 utils/tests/FullDiagnostics/BatteryDiagnostics.py diff --git a/utils/tests/FullDiagnostics/BatteryDiagnostics.py b/utils/tests/FullDiagnostics/BatteryDiagnostics.py deleted file mode 100644 index 07d7766..0000000 --- a/utils/tests/FullDiagnostics/BatteryDiagnostics.py +++ /dev/null @@ -1,33 +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() -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 - -if hub.battery.voltage() > 7800: - print(f"Battery voltage is sufficient: {hub.battery.voltage()}") -elif hub.battery.voltage < 7800 : - print(f"Charging needed: {hub.battery.voltage()}") - -print(hub.battery.current()) -print(hub.imu.heading()) -print(hub.system.info()) \ No newline at end of file From 63d4b27648b20a5d5f5e3c8ddc58dfccde53b93d Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:44:25 +0000 Subject: [PATCH 14/75] Delete utils/tests/FullDiagnostics/FullDiagnostics.py --- .../tests/FullDiagnostics/FullDiagnostics.py | 35 ------------------- 1 file changed, 35 deletions(-) delete mode 100644 utils/tests/FullDiagnostics/FullDiagnostics.py diff --git a/utils/tests/FullDiagnostics/FullDiagnostics.py b/utils/tests/FullDiagnostics/FullDiagnostics.py deleted file mode 100644 index af8a6b0..0000000 --- a/utils/tests/FullDiagnostics/FullDiagnostics.py +++ /dev/null @@ -1,35 +0,0 @@ -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor -from pybricks.parameters import Button, Color, Direction, Port, Side, Stop -from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch -hub = PrimeHub() -from BatteryDiagnostics import BatteryDiagnostics -battery = BatteryDiagnostics() -clearConfirmation = input("Do you want to clear the console before proceeding? Y/N (default: yes): ") -if(clearConfirmation == "Y" or clearConfirmation == "y" or clearConfirmation == "yes" or clearConfirmation == ""): - print("Clearing console... \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n") - -while True: - print("\nWhat diagnostic do you want to perform?") - print("Enter 'b' for Battery diagnostics") - print("Enter 'm' for Motor diagnostics") - print("Enter 'q' to Quit") - - choice = input("Your choice: ").strip().lower() - - if choice == "b": - print("-----------------------BATTERY DIAGNOSTICS-----------------------") - battery.printAll() - - elif choice == "m": - print("------------------------MOTOR DIAGNOSTICS------------------------") - # motor.printAll() # call your motor diagnostics here - print("Motor diagnostics would run here.") - - elif choice == "q": - print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!") - break - - else: - print("Invalid choice. Please enter 'b', 'm', or 'q'.") \ No newline at end of file From f926fbba795e13651a3a1127712690464f77ba4f Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:45:31 +0000 Subject: [PATCH 15/75] Delete utils/tests/ColorSensor.py --- utils/tests/ColorSensor.py | 69 -------------------------------------- 1 file changed, 69 deletions(-) delete mode 100644 utils/tests/ColorSensor.py diff --git a/utils/tests/ColorSensor.py b/utils/tests/ColorSensor.py deleted file mode 100644 index 3d5a8d9..0000000 --- a/utils/tests/ColorSensor.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 From 42ceb9f38e4ef88eb1b5d6edcb79df04558412cb Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:45:34 +0000 Subject: [PATCH 16/75] Delete utils/tests/Diagnostics.py --- utils/tests/Diagnostics.py | 33 --------------------------------- 1 file changed, 33 deletions(-) delete mode 100644 utils/tests/Diagnostics.py diff --git a/utils/tests/Diagnostics.py b/utils/tests/Diagnostics.py deleted file mode 100644 index 07d7766..0000000 --- a/utils/tests/Diagnostics.py +++ /dev/null @@ -1,33 +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() -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 - -if hub.battery.voltage() > 7800: - print(f"Battery voltage is sufficient: {hub.battery.voltage()}") -elif hub.battery.voltage < 7800 : - print(f"Charging needed: {hub.battery.voltage()}") - -print(hub.battery.current()) -print(hub.imu.heading()) -print(hub.system.info()) \ No newline at end of file From 59a4c42060fa8ea92f78caea146c2d416d1b75c8 Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Sat, 6 Dec 2025 17:47:05 +0000 Subject: [PATCH 17/75] Update competition_codes/sectionals/sectional_main_experimental.py --- .../sectionals/sectional_main_experimental.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/competition_codes/sectionals/sectional_main_experimental.py b/competition_codes/sectionals/sectional_main_experimental.py index 21a9c0e..5ed26d2 100644 --- a/competition_codes/sectionals/sectional_main_experimental.py +++ b/competition_codes/sectionals/sectional_main_experimental.py @@ -208,14 +208,14 @@ Run #4 """ async def Run4(): await drive_base.straight(700) - await drive_base.turn(-20) - await drive_base.straight(110) + await drive_base.turn(-18) + await drive_base.straight(120) await drive_base.straight(-210) - await drive_base.turn(60) - await drive_base.straight(130) - await right_arm.run_angle(1000, -200) + 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(300, 200) + await right_arm.run_angle(100, 95) await drive_base.straight(-875) async def solve_brush_reveal(): @@ -248,7 +248,7 @@ async def Run6(): await drive_base.turn(-103) await drive_base.straight(-80) await left_arm.run_angle(500, -900) - await drive_base.straight(101) + await drive_base.straight(105) await left_arm.run_angle(700, 285) await drive_base.turn(30) await drive_base.straight(-60) From ddc0f2ccca79cf3da96168d3899fe712a9690310 Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Sat, 6 Dec 2025 18:01:47 +0000 Subject: [PATCH 18/75] Update competition_codes/sectionals/sectional_main_dec_6.py --- .../{sectional_main_experimental.py => sectional_main_dec_6.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename competition_codes/sectionals/{sectional_main_experimental.py => sectional_main_dec_6.py} (99%) diff --git a/competition_codes/sectionals/sectional_main_experimental.py b/competition_codes/sectionals/sectional_main_dec_6.py similarity index 99% rename from competition_codes/sectionals/sectional_main_experimental.py rename to competition_codes/sectionals/sectional_main_dec_6.py index 5ed26d2..c0a04f1 100644 --- a/competition_codes/sectionals/sectional_main_experimental.py +++ b/competition_codes/sectionals/sectional_main_dec_6.py @@ -236,7 +236,7 @@ Run#5 - Solves Salvage Operation + Statue Rebuild - Orange Key """ -async def Run6(): +async def Run5(): await drive_base.straight(550) await right_arm.run_angle(300,100) await drive_base.straight(-75) From f1f800783f183df043019b7269797434d8e54592 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sat, 6 Dec 2025 20:16:37 +0000 Subject: [PATCH 19/75] Update competition_codes/sectionals/sectional_main_dec_6.py Updates for gear ratio change --- competition_codes/sectionals/sectional_main_dec_6.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/competition_codes/sectionals/sectional_main_dec_6.py b/competition_codes/sectionals/sectional_main_dec_6.py index c0a04f1..7aeacff 100644 --- a/competition_codes/sectionals/sectional_main_dec_6.py +++ b/competition_codes/sectionals/sectional_main_dec_6.py @@ -123,10 +123,10 @@ async def solve_forge(): await drive_base.straight(-47) async def solve_heavy_lifting(): - await right_arm.run_angle(500,-90) # arm down + 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,140) #arm up + await right_arm.run_angle(500,160) #arm up await drive_base.turn(-20) #reset position async def solve_who_lived_here(): From 27e1e2f7514e35528886d8fb5b04b95cb85e41fd Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sat, 6 Dec 2025 20:35:00 +0000 Subject: [PATCH 20/75] Update competition_codes/sectionals/sectional_main_dec_6.py --- competition_codes/sectionals/sectional_main_dec_6.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/competition_codes/sectionals/sectional_main_dec_6.py b/competition_codes/sectionals/sectional_main_dec_6.py index 7aeacff..c5f74ff 100644 --- a/competition_codes/sectionals/sectional_main_dec_6.py +++ b/competition_codes/sectionals/sectional_main_dec_6.py @@ -176,9 +176,8 @@ async def Run3(): monitor_distance() ) - async def solve_angler_artifacts(): - await drive_base.straight(920) + await drive_base.straight(940) await drive_base.turn(-90,Stop.HOLD) await drive_base.straight(65) #Solve @@ -187,20 +186,23 @@ async def solve_angler_artifacts(): await drive_base.straight(-110) await drive_base.turn(90) - await drive_base.arc(-150,-100, None) + await drive_base.arc(-150,-103, None) async def solve_tip_the_scale(): - await drive_base.straight(135) + 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(320,Stop.COAST_SMART) + 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 From f838d6b5664e2d57cff84b23992248177e313422 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sat, 6 Dec 2025 20:36:10 +0000 Subject: [PATCH 21/75] Update competition_codes/sectionals/sectional_main_dec_6.py --- competition_codes/sectionals/sectional_main_dec_6.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/competition_codes/sectionals/sectional_main_dec_6.py b/competition_codes/sectionals/sectional_main_dec_6.py index c5f74ff..9236075 100644 --- a/competition_codes/sectionals/sectional_main_dec_6.py +++ b/competition_codes/sectionals/sectional_main_dec_6.py @@ -203,6 +203,8 @@ async def solve_tip_the_scale(): + + """ Run #4 - Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal From a0bec55e97a108b1e459e59fc5d4ffb9ca543776 Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Sun, 7 Dec 2025 21:10:25 +0000 Subject: [PATCH 22/75] Update competition_codes/sectionals/sectional_main_dec_6.py --- .../sectionals/sectional_main_dec_6.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/competition_codes/sectionals/sectional_main_dec_6.py b/competition_codes/sectionals/sectional_main_dec_6.py index 9236075..e671f6c 100644 --- a/competition_codes/sectionals/sectional_main_dec_6.py +++ b/competition_codes/sectionals/sectional_main_dec_6.py @@ -252,12 +252,13 @@ async def Run5(): await drive_base.turn(-103) await drive_base.straight(-80) await left_arm.run_angle(500, -900) - await drive_base.straight(105) - await left_arm.run_angle(700, 285) - await drive_base.turn(30) - await drive_base.straight(-60) - await drive_base.turn(80) - await drive_base.straight(-900) + await drive_base.straight(120) + await drive_base.turn(10) + await left_arm.run_angle(700, 290) + await drive_base.turn(20) + await drive_base.straight(-100) + await drive_base.turn(-120) + await drive_base.straight(900) async def solve_salvage_operation(): await drive_base.straight(500) From 4f21cdc99ca88dd552ed64d74ca96802d8235707 Mon Sep 17 00:00:00 2001 From: nifrali Date: Mon, 8 Dec 2025 00:05:09 +0000 Subject: [PATCH 23/75] Update competition_codes/sectionals/sectional_main_dec_6.py Updated values --- .../sectionals/sectional_main_dec_6.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/competition_codes/sectionals/sectional_main_dec_6.py b/competition_codes/sectionals/sectional_main_dec_6.py index e671f6c..5a4bc39 100644 --- a/competition_codes/sectionals/sectional_main_dec_6.py +++ b/competition_codes/sectionals/sectional_main_dec_6.py @@ -248,17 +248,20 @@ async def Run5(): 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(-103) + 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(10) - await left_arm.run_angle(700, 290) - await drive_base.turn(20) + 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(-120) + await drive_base.turn(-90) await drive_base.straight(900) + drive_base.stop() + async def solve_salvage_operation(): await drive_base.straight(500) From 288c4eac26b984e43107ffd252c7a0c8b746f530 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Mon, 8 Dec 2025 17:29:02 +0000 Subject: [PATCH 24/75] Update README.md --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 3bb5d68..d843821 100644 --- a/README.md +++ b/README.md @@ -63,17 +63,17 @@ Repository ### Installation & Deployment - from the server - everyday -1. Download the file codes_for_scrimmage/regional-final/Final_combined.py +1. Download the file competition_codes/sectionals/sectional_main_dec_6.py - You can do this through the repo, by using cURL, or by using git. - - Repo - Go to [codes_for_scrimmage/regional-final/Final_combined.py](codes_for_scrimmage/regional-final/Final_combined.py) and click the "Download" button. + - Repo - Go to [competition_codes/sectionals/sectional_main_dec_6.py](competition_codes/sectionals/sectional_main_dec_6.py) and click the "Download" button. - cURL or another HTTP data transferrer - - ```curl -o Final_combined.py https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/main/codes_for_scrimmage/regional-final/Final_combined.py``` + ```curl -o Release-65266.py https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/main/competition_codes/sectionals/sectional_main_dec_6.py``` - git CLI - - ```git clone -b dev https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git && cd solutions_season_unearthed/codes_for_scrimmage/regional-final``` + ```git clone -b dev https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git && cd solutions_season_unearthed/competition_codes/sectionals``` - Then use Final_combined.py. + Then use sectional_main_dec_6.py. 2. Open https://code.pybricks.com/ and select the "Import a file" button on the top of the left bar. Import the .py file. Pair your robot via Bluetooth in Pybricks by selecting the Bluetooth button. From 4539ee361f44c63c1fae651547301f41452ea881 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Mon, 8 Dec 2025 19:13:17 +0000 Subject: [PATCH 25/75] Update README.md --- README.md | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index d843821..ad658ca 100644 --- a/README.md +++ b/README.md @@ -65,15 +65,12 @@ Repository 1. Download the file competition_codes/sectionals/sectional_main_dec_6.py - You can do this through the repo, by using cURL, or by using git. - - Repo - Go to [competition_codes/sectionals/sectional_main_dec_6.py](competition_codes/sectionals/sectional_main_dec_6.py) and click the "Download" button. - - cURL or another HTTP data transferrer - - - ```curl -o Release-65266.py https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/main/competition_codes/sectionals/sectional_main_dec_6.py``` + - Repo - Go to the latest release and click the "Download as ZIP" button to get the full repository. + - Single file - Go to the latest release and click the file link to get the master file. - git CLI - - - ```git clone -b dev https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git && cd solutions_season_unearthed/competition_codes/sectionals``` + ```git clone https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git``` - Then use sectional_main_dec_6.py. + Then use the master file. 2. Open https://code.pybricks.com/ and select the "Import a file" button on the top of the left bar. Import the .py file. Pair your robot via Bluetooth in Pybricks by selecting the Bluetooth button. From 4d94a35503be9df7ab3aa0f8f6623abf60f57ab5 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Mon, 8 Dec 2025 19:15:10 +0000 Subject: [PATCH 26/75] Update competition_codes/sectionals/mukwonago_sectionals_final.py --- .../{sectional_main_dec_6.py => mukwonago_sectionals_final.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename competition_codes/sectionals/{sectional_main_dec_6.py => mukwonago_sectionals_final.py} (100%) diff --git a/competition_codes/sectionals/sectional_main_dec_6.py b/competition_codes/sectionals/mukwonago_sectionals_final.py similarity index 100% rename from competition_codes/sectionals/sectional_main_dec_6.py rename to competition_codes/sectionals/mukwonago_sectionals_final.py From 5a805e75fca86e45efb296c412e99fdaa3401747 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Mon, 8 Dec 2025 19:16:10 +0000 Subject: [PATCH 27/75] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index ad658ca..0719e11 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ Repository 1. Download the file competition_codes/sectionals/sectional_main_dec_6.py - You can do this through the repo, by using cURL, or by using git. - Repo - Go to the latest release and click the "Download as ZIP" button to get the full repository. - - Single file - Go to the latest release and click the file link to get the master file. + - Single file - Go to the latest release and click the file link to get the raw master file. - git CLI - ```git clone https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git``` From f38a3cc62530fb06cdebbb2b78a1e1f01a45a7b2 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Mon, 8 Dec 2025 19:18:33 +0000 Subject: [PATCH 28/75] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 0719e11..86d54dc 100644 --- a/README.md +++ b/README.md @@ -65,7 +65,7 @@ Repository 1. Download the file competition_codes/sectionals/sectional_main_dec_6.py - You can do this through the repo, by using cURL, or by using git. - - Repo - Go to the latest release and click the "Download as ZIP" button to get the full repository. + - Repo - Go to the [latest release](https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/releases) and click the "Download as ZIP" button to get the full repository. - Single file - Go to the latest release and click the file link to get the raw master file. - git CLI - ```git clone https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git``` From 3014305d69b894a3abf0d6a73b4c2970c0abdfd0 Mon Sep 17 00:00:00 2001 From: nifrali Date: Tue, 9 Dec 2025 17:34:12 +0000 Subject: [PATCH 29/75] Update competition_codes/sectionals/mukwonago_sectionals_final.py Fixed return --- .../sectionals/mukwonago_sectionals_final.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/competition_codes/sectionals/mukwonago_sectionals_final.py b/competition_codes/sectionals/mukwonago_sectionals_final.py index 5a4bc39..084eb70 100644 --- a/competition_codes/sectionals/mukwonago_sectionals_final.py +++ b/competition_codes/sectionals/mukwonago_sectionals_final.py @@ -220,7 +220,11 @@ async def Run4(): await right_arm.run_angle(400, -200) await drive_base.straight(90) await right_arm.run_angle(100, 95) - await drive_base.straight(-875) + #await drive_base.straight(-875) + #return with arc + await drive_base.straight(-600) + await drive_base.arc(10,-47, None) + await drive_base.straight(-400,Stop.COAST_SMART) async def solve_brush_reveal(): await drive_base.straight(700) @@ -291,8 +295,15 @@ Run#6 - Purple Key """ async def Run6_7(): # experiment with ferris wheel for Site Markings - solve_site_mark_1() - solve_site_mark_2() + #solve_site_mark_1() + #solve_site_mark_2() + await drive_base.straight(640) + await drive_base.straight(-150) + await drive_base.turn(-8) + await left_arm.run_angle(500, -520) + await drive_base.straight(300) + await wait(50) + await left_arm.run_angle(500, 700) #return to base await drive_base.straight(-300) drive_base.stop() @@ -360,7 +371,7 @@ async def main(): print('Running Mission 5') await Run5() elif color == "Purple": - print('Running Mission 6') + print('Running Mission 6_7') await Run6_7() elif color == "Light_Blue": print("Running Mission 2_1") From a902f29952144dbcc8f24c3075cff1f3a2705802 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Mon, 22 Dec 2025 23:00:02 +0000 Subject: [PATCH 30/75] Add competition_codes/state/sunprarie_state_main.py --- .../state/sunprarie_state_main.py | 382 ++++++++++++++++++ 1 file changed, 382 insertions(+) create mode 100644 competition_codes/state/sunprarie_state_main.py diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py new file mode 100644 index 0000000..942e488 --- /dev/null +++ b/competition_codes/state/sunprarie_state_main.py @@ -0,0 +1,382 @@ +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, [[12,36]],[[12,20,24]] ) # 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 left_arm.run_until_stalled(1500 ,duty_limit=15) + left_arm.reset_angle(0) + 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,-119.5) + await drive_base.straight(180) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_silo(): + await drive_base.straight(-80) + await drive_base.turn(42) + await drive_base.straight(90) + + + await right_arm.run_angle(10000,-80, Stop.HOLD) + await right_arm.run_angle(10000,80, Stop.HOLD) + await right_arm.run_angle(10000,-80, Stop.HOLD) + await right_arm.run_angle(10000,80, Stop.HOLD) + await right_arm.run_angle(10000,-80, Stop.HOLD) + await right_arm.run_angle(10000,80, Stop.HOLD) + await right_arm.run_angle(10000,-80, Stop.HOLD) + right_arm.run_angle(4000,60, 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(2000,-160) # arm down + await wait(100) + await drive_base.turn(30) # turn right a little bit + await right_arm.run_angle(2000,160) #arm up + await drive_base.turn(-30) #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(-40) + await drive_base.straight(50) + right_arm.run_angle(1000,-160) + await drive_base.turn(-60) + await right_arm.run_angle(2000,160) + +""" +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()) From 5a5b6d2d16d2eedff3c6709ccecc720cf4eb798e Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Wed, 24 Dec 2025 16:34:38 +0000 Subject: [PATCH 31/75] Update competition_codes/state/sunprarie_state_main.py Code optimization and additions to 1st run. --- .../state/sunprarie_state_main.py | 41 +++++++++++++------ 1 file changed, 29 insertions(+), 12 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 942e488..29f4509 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -50,6 +50,9 @@ async def monitor_distance(): # Small delay to prevent overwhelming the sensor await wait(50) + +def set_default_speed(): + drive_base.settings(600, 500, 300, 200) """ Run#1 - Removed forge and who lived here part @@ -58,11 +61,16 @@ Run#1 """ async def Run1(): - await left_arm.run_until_stalled(1500 ,duty_limit=15) + # Fast approach to near-stall position + await left_arm.run_angle(2000, 180) # Fast movement upward + + # Gentle stall detection (shorter distance = faster) + await left_arm.run_until_stalled(1500, duty_limit=15) left_arm.reset_angle(0) + await solve_whats_on_sale() await solve_silo() - + # return to base await drive_base.straight(-90) #await drive_base.turn(-100) @@ -94,14 +102,14 @@ async def solve_silo(): await drive_base.turn(42) await drive_base.straight(90) + SPEED = 10000 # speed in degree per second + SWING_ANGLE = 80 # the angle! + + # Repeat this motion 4 times + for _ in range(4): + await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.COAST) # Swing up + await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.COAST) # Swing down - await right_arm.run_angle(10000,-80, Stop.HOLD) - await right_arm.run_angle(10000,80, Stop.HOLD) - await right_arm.run_angle(10000,-80, Stop.HOLD) - await right_arm.run_angle(10000,80, Stop.HOLD) - await right_arm.run_angle(10000,-80, Stop.HOLD) - await right_arm.run_angle(10000,80, Stop.HOLD) - await right_arm.run_angle(10000,-80, Stop.HOLD) right_arm.run_angle(4000,60, Stop.HOLD) @@ -319,7 +327,16 @@ async def solve_site_mark_2(): await wait(50) await right_arm.run_angle(50, 50) - +async def Run10(): # experiment with ferris wheel for Site Markings + + await drive_base.straight(680) + drive_base.settings(150, 750, 50, 750) + await drive_base.turn(-45) + await drive_base.straight(200) + set_default_speed() + await drive_base.straight(-100) + await drive_base.turn(45) + drive_base.stop() # Function to classify color based on HSV def detect_color(h, s, v, reflected): @@ -371,7 +388,7 @@ async def main(): await Run5() elif color == "Purple": print('Running Mission 6') - await Run6_7() + await Run10() elif color == "Light_Blue": print("Running Mission 2_1") await Run2_1() @@ -379,4 +396,4 @@ async def main(): print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") await wait(10) # Run the main function -run_task(main()) +run_task(main()) \ No newline at end of file From d9dec685c3e266f1447fcefe18dd419426e07184 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sun, 28 Dec 2025 01:55:38 +0000 Subject: [PATCH 32/75] Update competition_codes/state/sunprarie_state_main.py Slight updates to Send over and the first mission run. --- .../state/sunprarie_state_main.py | 32 ++++++++----------- 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 29f4509..7fd5991 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -25,7 +25,7 @@ WALL_DISTANCE = 200 # mm async def drive_forward(): """Drive forward continuously using DriveBase.""" - drive_base.drive(400, 0) + drive_base.drive(1000,0) async def drive_backward(): """Drive forward continuously using DriveBase.""" @@ -80,7 +80,11 @@ async def Run1(): async def solve_whats_on_sale(): right_arm.run_angle(500,30) - left_arm.run_angle(500,-119.5) + + #Automated inconsistency + #left_arm.run_angle(500,-119.5) + left_arm.run_target(500,-121.5, Stop.HOLD) + await drive_base.straight(180) await drive_base.turn(-40) @@ -195,28 +199,20 @@ async def Run3(): ) async def solve_angler_artifacts(): - await drive_base.straight(940) + await drive_base.straight(890) await drive_base.turn(-90,Stop.HOLD) - await drive_base.straight(65) + await drive_base.straight(45) #Solve drive_base.turn(-10) - await left_arm.run_angle(10000,-3000) - await drive_base.straight(-110) + await left_arm.run_angle(10000,-750) + await drive_base.straight(-130) - await drive_base.turn(90) - await drive_base.arc(-150,-103, None) + await drive_base.turn(67) 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) + await drive_base.straight(-200) + await drive_base.straight(60) + await drive_base.turn(22) From 6755e783f49c550475b7ebdf1e541cb5af423c63 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Mon, 29 Dec 2025 05:44:11 +0000 Subject: [PATCH 33/75] Add competition_codes/state/run10_experimental.py --- competition_codes/state/run10_experimental.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 competition_codes/state/run10_experimental.py diff --git a/competition_codes/state/run10_experimental.py b/competition_codes/state/run10_experimental.py new file mode 100644 index 0000000..95dd75b --- /dev/null +++ b/competition_codes/state/run10_experimental.py @@ -0,0 +1,16 @@ +await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(260) + left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-80) + await drive_base.turn(30) + await drive_base.straight(-300) + await drive_base.straight(400) + #await left_arm.run_angle(50,120) + await drive_base.straight(-200) + await left_arm.run_angle(300,-215) + await drive_base.straight(-600) + drive_base.stop() From d5e7e5816a08baecb4cb49caa43230e5595ae919 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Mon, 29 Dec 2025 05:44:44 +0000 Subject: [PATCH 34/75] Add competition_codes/state/run11_experimental.py --- competition_codes/state/run11_experimental.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 competition_codes/state/run11_experimental.py diff --git a/competition_codes/state/run11_experimental.py b/competition_codes/state/run11_experimental.py new file mode 100644 index 0000000..c342307 --- /dev/null +++ b/competition_codes/state/run11_experimental.py @@ -0,0 +1,16 @@ +await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(250) + #left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-80) + await drive_base.turn(30) + await drive_base.straight(-180) + #await drive_base.straight(400) + #await left_arm.run_angle(50,120) + #await drive_base.straight(-200) + await left_arm.run_angle(300,-300) + await drive_base.straight(-600) + drive_base.stop() From aa69eed5344307b6dbce26f456177b2c8105a6c6 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Mon, 29 Dec 2025 19:25:24 +0000 Subject: [PATCH 35/75] Update competition_codes/state/sunprarie_state_main.py Updates to run 10. --- .../state/sunprarie_state_main.py | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 7fd5991..6d96528 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -325,13 +325,21 @@ async def solve_site_mark_2(): async def Run10(): # experiment with ferris wheel for Site Markings - await drive_base.straight(680) - drive_base.settings(150, 750, 50, 750) - await drive_base.turn(-45) - await drive_base.straight(200) + await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(250) + #left_arm.run_angle(300,218) + set_default_speed() - await drive_base.straight(-100) - await drive_base.turn(45) + await drive_base.straight(-80) + await drive_base.turn(30) + await drive_base.straight(-180) + #await drive_base.straight(400) + #await left_arm.run_angle(50,120) + #await drive_base.straight(-200) + await left_arm.run_angle(300,-300) + await drive_base.straight(-600) drive_base.stop() # Function to classify color based on HSV From 54aa537877cf09f3b8d925edc2cc3d745430e12a Mon Sep 17 00:00:00 2001 From: Vickram <31kapoov@elmbrookstudents.org> Date: Mon, 29 Dec 2025 22:45:02 +0000 Subject: [PATCH 36/75] Fixed motor angles for the left arm --- competition_codes/state/sunprarie_state_main.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 6d96528..568d5ad 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -266,10 +266,10 @@ async def Run5(): await drive_base.straight(350) await drive_base.turn(-104) await drive_base.straight(-80) - await left_arm.run_angle(500, -900) + await left_arm.run_angle(500, -300) await drive_base.straight(120) await drive_base.turn(5) - await left_arm.run_angle(500, 290) + await left_arm.run_angle(500, 120) await drive_base.turn(18) await drive_base.straight(-100) await drive_base.turn(-90) From 6dbcfbe146750b79312e248b79a476d1b5388be9 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sat, 3 Jan 2026 18:38:23 +0000 Subject: [PATCH 37/75] Update competition_codes/state/sunprarie_state_main.py Changes to Run 12 --- .../state/sunprarie_state_main.py | 72 +++++++++++++++++-- 1 file changed, 66 insertions(+), 6 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 568d5ad..95646be 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -323,7 +323,26 @@ async def solve_site_mark_2(): await wait(50) await right_arm.run_angle(50, 50) -async def Run10(): # experiment with ferris wheel for Site Markings +async def Run10(): # experimental map reveal attachment + + await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(260) + left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-80) + await drive_base.turn(30) + await drive_base.straight(-300) + await drive_base.straight(400) + #await left_arm.run_angle(50,120) + await drive_base.straight(-200) + await left_arm.run_angle(300,-215) + await drive_base.straight(-600) + drive_base.stop() + +async def Run11(): # experimental surface brushing attachment await drive_base.straight(600) drive_base.settings(150, 750, 50, 500) @@ -342,6 +361,46 @@ async def Run10(): # experiment with ferris wheel for Site Markings await drive_base.straight(-600) drive_base.stop() +async def Run12(): # experimental careful recovery attachment + + right_arm.reset_angle(0) + # This raises the left arm to avoid entanglement when turning + await left_arm.run_angle(2000, 180) # Fast movement upward + # Gentle stall detection (shorter distance = faster) + await left_arm.run_until_stalled(1500, duty_limit=15) + left_arm.reset_angle(0) + + # Drive the robot to the wall + await drive_base.straight(900) + await drive_base.turn(83) # Align robot to Mineshaft entrance + + # This change robot movement to slow + drive_base.settings(100, 100, 50, 500) + + # Bring down left arm to position + await left_arm.run_angle(2000, -180) + await left_arm.run_until_stalled(-1500,duty_limit=15) + left_arm.reset_angle(0) + + # Bring down right arm to position + await right_arm.run_target(2000,-120) + right_arm.reset_angle(0) + + await drive_base.straight(190) # Slowly move straight to mineshaft 190 mm + + await right_arm.run_angle(100,95,Stop.HOLD) # Raise mineshaft + await wait(50) + await left_arm.run_angle(100,60) # recover artifact by lifting arm 60 degrees + + # Moving back + await drive_base.straight(-200) + + # Return home + set_default_speed() # change movement robot movement to fast + await drive_base.turn(100) + await drive_base.straight(700) + drive_base.stop() + # Function to classify color based on HSV def detect_color(h, s, v, reflected): if reflected > 4: @@ -368,11 +427,11 @@ def detect_color(h, s, v, reflected): async def main(): while True: h, s, v = await color_sensor.hsv() - print(color_sensor.color()) - print(h,s,v) + #print(color_sensor.color()) + #print(h,s,v) reflected = await color_sensor.reflection() color = detect_color(h, s, v, reflected) - print(color) + #print(color) if color == "Green": @@ -392,12 +451,13 @@ async def main(): await Run5() elif color == "Purple": print('Running Mission 6') - await Run10() + await Run11() elif color == "Light_Blue": print("Running Mission 2_1") - await Run2_1() + await Run12() else: print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + #pass await wait(10) # Run the main function run_task(main()) \ No newline at end of file From 4233cc5f742193b52aebac3f8e55b402b5fdd0f9 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sun, 4 Jan 2026 18:09:47 +0000 Subject: [PATCH 38/75] Update competition_codes/state/sunprarie_state_main.py Various updates to a few of the codes. --- competition_codes/state/sunprarie_state_main.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 95646be..16e2bc3 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -1,3 +1,5 @@ +#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge. + 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 @@ -104,7 +106,7 @@ async def solve_whats_on_sale(): async def solve_silo(): await drive_base.straight(-80) await drive_base.turn(42) - await drive_base.straight(90) + await drive_base.straight(95) SPEED = 10000 # speed in degree per second SWING_ANGLE = 80 # the angle! @@ -199,7 +201,7 @@ async def Run3(): ) async def solve_angler_artifacts(): - await drive_base.straight(890) + await drive_base.straight(870) await drive_base.turn(-90,Stop.HOLD) await drive_base.straight(45) #Solve @@ -386,14 +388,14 @@ async def Run12(): # experimental careful recovery attachment await right_arm.run_target(2000,-120) right_arm.reset_angle(0) - await drive_base.straight(190) # Slowly move straight to mineshaft 190 mm + await drive_base.straight(180) # Slowly move straight to mineshaft 180 mm await right_arm.run_angle(100,95,Stop.HOLD) # Raise mineshaft await wait(50) await left_arm.run_angle(100,60) # recover artifact by lifting arm 60 degrees # Moving back - await drive_base.straight(-200) + await drive_base.straight(-190) # Return home set_default_speed() # change movement robot movement to fast From 3efdf8ecbcfaf41fe0e9288908b35826182192cf Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Mon, 5 Jan 2026 01:24:12 +0000 Subject: [PATCH 39/75] Update competition_codes/state/sunprarie_state_main.py Slight changes to motor angles in Run_5. --- competition_codes/state/sunprarie_state_main.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 16e2bc3..46d8021 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -257,21 +257,24 @@ Run#5 - Orange Key """ async def Run5(): + # Getting the sand down await drive_base.straight(550) await right_arm.run_angle(300,100) await drive_base.straight(-75) await right_arm.run_angle(300, -100) + # Shoving the boat into place await drive_base.straight(300) await drive_base.straight(-200) await drive_base.turn(-15) - #solving statue + # Solving statue await drive_base.straight(350) await drive_base.turn(-104) await drive_base.straight(-80) await left_arm.run_angle(500, -300) await drive_base.straight(120) await drive_base.turn(5) - await left_arm.run_angle(500, 120) + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) await drive_base.turn(18) await drive_base.straight(-100) await drive_base.turn(-90) @@ -297,7 +300,7 @@ async def solve_statue_rebuild(): 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 left_arm.run_angle(700,200) await drive_base.turn(30) """ @@ -391,7 +394,6 @@ async def Run12(): # experimental careful recovery attachment await drive_base.straight(180) # Slowly move straight to mineshaft 180 mm await right_arm.run_angle(100,95,Stop.HOLD) # Raise mineshaft - await wait(50) await left_arm.run_angle(100,60) # recover artifact by lifting arm 60 degrees # Moving back From 4942f28e67c9f88faee75fc5d564500fdf653555 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Tue, 6 Jan 2026 22:23:43 +0000 Subject: [PATCH 40/75] Update competition_codes/state/sunprarie_state_main.py Battery diagnostics added. --- .../state/sunprarie_state_main.py | 91 +++++++++++++++---- 1 file changed, 71 insertions(+), 20 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 46d8021..fd889af 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -1,5 +1,10 @@ -#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge. +Francisco Liwa +7:58 AM (8 hours ago) +to me + +#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge. +import umath 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 @@ -23,8 +28,24 @@ 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 +""" +Debugging helps +""" +DEBUG = 0 # Enable when you want to show logs +# Example conversion function (adjust min/max values as needed for your hub) +async def get_battery_percentage(voltage_mV:float): + max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us + min_voltage = 5000.0 # min battery level + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100 + return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100 +async def wait_button_release(): + """Wait for all buttons to be released""" + while hub.buttons.pressed(): + await wait(500) + await wait(1000) # Debounce delay + +WALL_DISTANCE = 200 # mm async def drive_forward(): """Drive forward continuously using DriveBase.""" drive_base.drive(1000,0) @@ -47,14 +68,18 @@ async def monitor_distance(): break if distance is None: continue - - + # Small delay to prevent overwhelming the sensor await wait(50) - +# Use this to set default def set_default_speed(): drive_base.settings(600, 500, 300, 200) + +# Use this to change drive base movement +def set_speed(straight_speed, st_acc, turn_speed, turn_acc): + drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc) + """ Run#1 - Removed forge and who lived here part @@ -69,8 +94,9 @@ async def Run1(): # Gentle stall detection (shorter distance = faster) await left_arm.run_until_stalled(1500, duty_limit=15) left_arm.reset_angle(0) + print(f"Initial left arm angle : {left_arm.angle()}") - await solve_whats_on_sale() + await solve_whats_on_sale_v2() await solve_silo() # return to base @@ -78,7 +104,7 @@ async def Run1(): #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) @@ -86,6 +112,33 @@ async def solve_whats_on_sale(): #Automated inconsistency #left_arm.run_angle(500,-119.5) left_arm.run_target(500,-121.5, Stop.HOLD) + print(f"Position left arm angle : {left_arm.angle()}") + + await drive_base.straight(180) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_whats_on_sale_v2(): + + right_arm.run_angle(500,30) + + # Bring down left arm to position + await left_arm.run_angle(2000, -120) + #await left_arm.run_until_stalled(-500,duty_limit=15) + print(f"Position left arm angle : {left_arm.angle()}") + left_arm.reset_angle(0) await drive_base.straight(180) @@ -109,12 +162,14 @@ async def solve_silo(): await drive_base.straight(95) SPEED = 10000 # speed in degree per second - SWING_ANGLE = 80 # the angle! + SWING_ANGLE = 60 # the angle! + REBOUND_ADJ = 20 # Repeat this motion 4 times for _ in range(4): - await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.COAST) # Swing up - await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.COAST) # Swing down + await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up + await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down + right_arm.run_angle(4000,60, Stop.HOLD) @@ -430,12 +485,15 @@ def detect_color(h, s, v, reflected): async def main(): while True: + pressed = hub.buttons.pressed() 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 DEBUG : + #print(color_sensor.color()) + #print(h,s,v) + #print(color) + print(f"button pressed: {pressed}") if color == "Green": @@ -458,10 +516,3 @@ async def main(): await Run11() elif color == "Light_Blue": print("Running Mission 2_1") - await Run12() - else: - print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") - #pass - await wait(10) -# Run the main function -run_task(main()) \ No newline at end of file From a116a9b3bbb77f8d9eb8ce4727265cfdc8856593 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Tue, 6 Jan 2026 22:24:05 +0000 Subject: [PATCH 41/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index fd889af..4c5ac54 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -1,8 +1,3 @@ - -Francisco Liwa -7:58 AM (8 hours ago) -to me - #Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge. import umath from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor From 50d125a09e29161ab8a47616e1e26378f0bb3d7a Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Wed, 7 Jan 2026 00:17:23 +0000 Subject: [PATCH 42/75] Update competition_codes/state/sunprarie_state_main.py Forgot to add run task before --- .../state/sunprarie_state_main.py | 25 ++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 4c5ac54..4f79446 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -26,7 +26,7 @@ drive_base.use_gyro(True) """ Debugging helps """ -DEBUG = 0 # Enable when you want to show logs +DEBUG = 1 # Enable when you want to show logs # Example conversion function (adjust min/max values as needed for your hub) async def get_battery_percentage(voltage_mV:float): max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us @@ -511,3 +511,26 @@ async def main(): await Run11() elif color == "Light_Blue": print("Running Mission 2_1") + await Run12() + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + #pass + + # Show battery % for debugging + if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = hub.battery.voltage() + # Use the function with your voltage reading + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file From 8b152a371d3d3e46cff0c2b1da9a6a1b4ea367c7 Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Thu, 8 Jan 2026 23:11:34 +0000 Subject: [PATCH 43/75] Add competition_codes/state/Updated state.py --- competition_codes/state/Updated state.py | 513 +++++++++++++++++++++++ 1 file changed, 513 insertions(+) create mode 100644 competition_codes/state/Updated state.py diff --git a/competition_codes/state/Updated state.py b/competition_codes/state/Updated state.py new file mode 100644 index 0000000..4b0cebb --- /dev/null +++ b/competition_codes/state/Updated state.py @@ -0,0 +1,513 @@ +#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge. +import umath +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, [[12,36]],[[12,20,24]] ) # 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) + +""" +Debugging helps +""" +DEBUG = 1 # Enable when you want to show logs +# Example conversion function (adjust min/max values as needed for your hub) +async def get_battery_percentage(voltage_mV:float): + max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us + min_voltage = 5000.0 # min battery level + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100 + return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100 + +async def wait_button_release(): + """Wait for all buttons to be released""" + while hub.buttons.pressed(): + await wait(500) + await wait(1000) # Debounce delay + +WALL_DISTANCE = 200 # mm +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(1000,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) + +# Use this to set default +def set_default_speed(): + drive_base.settings(600, 500, 300, 200) + +# Use this to change drive base movement +def set_speed(straight_speed, st_acc, turn_speed, turn_acc): + drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc) + +""" +Run#1 +- Removed forge and who lived here part +- What's on sale + Silo +- Green Key +""" +async def Run1(): + + # Fast approach to near-stall position + await left_arm.run_angle(2000, 180) # Fast movement upward + + # Gentle stall detection (shorter distance = faster) + await left_arm.run_until_stalled(1500, duty_limit=15) + left_arm.reset_angle(0) + print(f"Initial left arm angle : {left_arm.angle()}") + + await solve_whats_on_sale_v2() + 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) + + #Automated inconsistency + #left_arm.run_angle(500,-119.5) + left_arm.run_target(500,-121.5, Stop.HOLD) + print(f"Position left arm angle : {left_arm.angle()}") + + await drive_base.straight(180) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_whats_on_sale_v2(): + + right_arm.run_angle(500,30) + + # Bring down left arm to position + await left_arm.run_angle(2000, -120) + #await left_arm.run_until_stalled(-500,duty_limit=15) + print(f"Position left arm angle : {left_arm.angle()}") + left_arm.reset_angle(0) + + await drive_base.straight(180) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_silo(): + await drive_base.straight(-80) + await drive_base.turn(42) + await drive_base.straight(95) + + SPEED = 10000 # speed in degree per second + SWING_ANGLE = 60 # the angle! + REBOUND_ADJ = 20 + + # Repeat this motion 4 times + for _ in range(4): + await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up + await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down + + + right_arm.run_angle(4000,60, 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(2000,-160) # arm down + await wait(100) + await drive_base.turn(30) # turn right a little bit + await right_arm.run_angle(2000,160) #arm up + await drive_base.turn(-30) #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(-40) + await drive_base.straight(50) + right_arm.run_angle(1000,-160) + await drive_base.turn(-60) + await right_arm.run_angle(2000,160) + +""" +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(870) + await drive_base.turn(-90,Stop.HOLD) + await drive_base.straight(45) + #Solve + drive_base.turn(-10) + await left_arm.run_angle(10000,-750) + await drive_base.straight(-130) + + await drive_base.turn(67) + +async def solve_tip_the_scale(): + await drive_base.straight(-200) + await drive_base.straight(60) + await drive_base.turn(22) + + + + + + +""" +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(): + # Getting the sand down + await drive_base.straight(550) + await right_arm.run_angle(300,100) + await drive_base.straight(-75) + await right_arm.run_angle(300, -100) + # Shoving the boat into place + 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, -300) + await drive_base.straight(120) + await drive_base.turn(5) + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) + 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,200) + 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) + +async def Run10(): # experimental map reveal attachment + + await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(260) + left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-80) + await drive_base.turn(30) + await drive_base.straight(-300) + await drive_base.straight(400) + #await left_arm.run_angle(50,120) + await drive_base.straight(-200) + await left_arm.run_angle(300,-215) + await drive_base.straight(-600) + drive_base.stop() + +async def Run11(): # experimental surface brushing attachment + + await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(250) + #left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-100) + await drive_base.turn(30) + await drive_base.straight(-190) + #await drive_base.straight(400) + #await left_arm.run_angle(50,120) + #await drive_base.straight(-200) + await left_arm.run_angle(300,-300) + await drive_base.straight(-600) + drive_base.stop() + +async def Run12(): + await drive_base.straight(900) + await drive_base.turn(83) + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + drive_base.settings(150, 50, 50, 50) + await drive_base.straight(120) + left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await drive_base.straight(-100) + drive_base.settings(950, 750, 750, 750) + await drive_base.turn(110) + await drive_base.straight(1000) + + +# 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: + pressed = hub.buttons.pressed() + h, s, v = await color_sensor.hsv() + reflected = await color_sensor.reflection() + color = detect_color(h, s, v, reflected) + if DEBUG : + #print(color_sensor.color()) + #print(h,s,v) + #print(color) + print(f"button pressed: {pressed}") + + + 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 Run11() + elif color == "Light_Blue": + print("Running Mission 2_1") + await Run12() + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + #pass + + # Show battery % for debugging + if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = hub.battery.voltage() + # Use the function with your voltage reading + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file From fa28fbb0864a841e40c7d2939ef1ce5a5f8e4e87 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Thu, 8 Jan 2026 23:21:51 +0000 Subject: [PATCH 44/75] Update competition_codes/state/sunprarie_state_main.py --- .../state/sunprarie_state_main.py | 51 +++++-------------- 1 file changed, 14 insertions(+), 37 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 4f79446..4b0cebb 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -406,9 +406,9 @@ async def Run11(): # experimental surface brushing attachment #left_arm.run_angle(300,218) set_default_speed() - await drive_base.straight(-80) + await drive_base.straight(-100) await drive_base.turn(30) - await drive_base.straight(-180) + await drive_base.straight(-190) #await drive_base.straight(400) #await left_arm.run_angle(50,120) #await drive_base.straight(-200) @@ -416,44 +416,21 @@ async def Run11(): # experimental surface brushing attachment await drive_base.straight(-600) drive_base.stop() -async def Run12(): # experimental careful recovery attachment - - right_arm.reset_angle(0) - # This raises the left arm to avoid entanglement when turning - await left_arm.run_angle(2000, 180) # Fast movement upward - # Gentle stall detection (shorter distance = faster) - await left_arm.run_until_stalled(1500, duty_limit=15) - left_arm.reset_angle(0) - - # Drive the robot to the wall +async def Run12(): await drive_base.straight(900) - await drive_base.turn(83) # Align robot to Mineshaft entrance - - # This change robot movement to slow - drive_base.settings(100, 100, 50, 500) - - # Bring down left arm to position - await left_arm.run_angle(2000, -180) - await left_arm.run_until_stalled(-1500,duty_limit=15) + await drive_base.turn(83) + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + drive_base.settings(150, 50, 50, 50) + await drive_base.straight(120) left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await drive_base.straight(-100) + drive_base.settings(950, 750, 750, 750) + await drive_base.turn(110) + await drive_base.straight(1000) - # Bring down right arm to position - await right_arm.run_target(2000,-120) - right_arm.reset_angle(0) - - await drive_base.straight(180) # Slowly move straight to mineshaft 180 mm - - await right_arm.run_angle(100,95,Stop.HOLD) # Raise mineshaft - await left_arm.run_angle(100,60) # recover artifact by lifting arm 60 degrees - - # Moving back - await drive_base.straight(-190) - - # Return home - set_default_speed() # change movement robot movement to fast - await drive_base.turn(100) - await drive_base.straight(700) - drive_base.stop() # Function to classify color based on HSV def detect_color(h, s, v, reflected): From a395daba025c7d292032f371e29f4d38b7e21dab Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 01:29:36 +0000 Subject: [PATCH 45/75] Delete RoshoDaGoat.py --- RoshoDaGoat.py | 42 ------------------------------------------ 1 file changed, 42 deletions(-) delete mode 100644 RoshoDaGoat.py diff --git a/RoshoDaGoat.py b/RoshoDaGoat.py deleted file mode 100644 index 7d84ee2..0000000 --- a/RoshoDaGoat.py +++ /dev/null @@ -1,42 +0,0 @@ -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor -from pybricks.parameters import Button, Color, Direction, Port, Side, Stop -from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch, run_task, multitask - -hub = PrimeHub() - -# Initialize both motors. In this example, the motor on the -# left must turn counterclockwise to make the robot go forward. -left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) -right_motor = Motor(Port.B) - -arm_motor = Motor(Port.D, Direction.CLOCKWISE) -arm_motor_left= Motor(Port.C, Direction.CLOCKWISE) -# Initialize the drive base. In this example, the wheel diameter is 56mm. -# The distance between the two wheel-ground contact points is 112mm. -drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180) - -print('The default settings are: ' + str(drive_base.settings())) -drive_base.settings(300,1000,300,750) -# Optionally, uncomment the line below to use the gyro for improved accuracy. -drive_base.use_gyro(True) - -async def main(): - await drive_base.straight(519) - await arm_motor_left.run_angle(-10000, 300) - await arm_motor_left.run_angle(10000, 600) - await drive_base.straight(160) - await drive_base.turn(-30) - await drive_base.straight(50) - await arm_motor.run_angle(3000, 3000) - await drive_base.straight(-150) - await drive_base.turn(135) - await drive_base.straight(50) - await arm_motor.run_angle(10000, -3000) - await drive_base.straight(-100) - await drive_base.turn(-54) - await arm_motor.run_angle(10000, -3000) - await drive_base.straight(200) - await arm_motor.run_angle(10000, 10000) -run_task(main()) \ No newline at end of file From 44bbcf176dac4ef80a65c709367032eec504393d Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 01:29:48 +0000 Subject: [PATCH 46/75] Delete config.py --- config.py | 40 ---------------------------------------- 1 file changed, 40 deletions(-) delete mode 100644 config.py diff --git a/config.py b/config.py deleted file mode 100644 index f59fbfb..0000000 --- a/config.py +++ /dev/null @@ -1,40 +0,0 @@ -# config.py - Robot configuration shared across all modules -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor -from pybricks.parameters import Port - -# Initialize hub -hub = PrimeHub() - -# Robot hardware configuration -ROBOT_CONFIG = { - # Drive motors - 'left_motor': Motor(Port.A), - 'right_motor': Motor(Port.B), - - # Attachment motors - 'attachment_motor': Motor(Port.C), - 'lift_motor': Motor(Port.D), - - # Sensors - #'color_left': ColorSensor(Port.E), - 'color_back': ColorSensor(Port.F), - 'ultrasonic': UltrasonicSensor(Port.E), - - # Hub reference - 'hub': hub -} - -# Speed and distance constants -SPEEDS = { - 'FAST': 400, - 'NORMAL': 250, - 'SLOW': 100, - 'PRECISE': 50 -} - -DISTANCES = { - 'TILE_SIZE': 300, # mm per field tile - 'ROBOT_LENGTH': 180, # mm - 'ROBOT_WIDTH': 140 # mm -} \ No newline at end of file From b93ab2f7881305e97f8d2b5e3d0330a4ef5f534a Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 01:30:03 +0000 Subject: [PATCH 47/75] Delete test_10_17_2025.py --- test_10_17_2025.py | 303 --------------------------------------------- 1 file changed, 303 deletions(-) delete mode 100644 test_10_17_2025.py diff --git a/test_10_17_2025.py b/test_10_17_2025.py deleted file mode 100644 index 1d89281..0000000 --- a/test_10_17_2025.py +++ /dev/null @@ -1,303 +0,0 @@ -# Stuff from 10/15/2025 -# Atharv trying with no avail to add more colors to the color sensor logic 😭😭😭😭😭😭😭 -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor -from pybricks.parameters import Port, Stop, Color, Direction -from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch, multitask, run_task - -hub = PrimeHub() -left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) -right_motor = Motor(Port.B) -left_arm = Motor(Port.C)#, Direction.COUNTERCLOCKWISE) -right_arm = Motor(Port.D) -drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180) -drive_base.settings(550,700,100,100) -drive_base.use_gyro(True) -color_sensor = ColorSensor(Port.F) -Color.ORANGE = Color(28, 61, 61) -Color.BLUE = Color(230,100,100) -Color.YELLOW = Color(37, 85, 95) -Color.PURPLE = Color(326, 60, 87) -color_sensor.detectable_colors([Color.ORANGE, Color.BLUE, Color.GREEN, Color.WHITE, Color.RED, Color.YELLOW, Color.NONE, Color.PURPLE]) -hub.speaker.volume(50) # Set the volume of the speaker -color_sensor.detectable_colors() -# Celebration sound types -class CelebrationSound: - VICTORY_FANFARE = 0 - LEVEL_UP = 1 - SUCCESS_CHIME = 2 - TA_DA = 3 - POWER_UP = 4 - RICKROLL_INSPIRED = 5 - -async def play_victory_fanfare(): - """Classic victory fanfare""" - notes = [ - (262, 200), # C4 - (262, 200), # C4 - (262, 200), # C4 - (349, 600), # F4 - ] - - for freq, duration in notes: - await hub.speaker.beep(freq, duration) - await wait(50) -async def play_level_up(): - """Upward scale for level completion""" - notesold = [ - (262, 100), # C4 - (294, 100), # D4 - (330, 100), # E4 - (349, 100), # F4 - (392, 100), # G4 - (440, 100), # A4 - (494, 100), # B4 - (523, 300), # C5 - ] - notes = [ - (277, 100), - (330, 100), - (277, 100), - (554, 100), - (277, 100), - (413, 100), - (330, 100), - (277, 100), - (413, 100), - (277, 100), - (554, 100), - (413, 100), - (277, 100), - (413, 100), - (554, 100), - (413, 100) - ] - - for freq, duration in notes: - await hub.speaker.beep(freq, duration) - #await wait(20) -async def play_success_chime(): - """Simple success notification""" - notes = [ - (523, 150), # C5 - (659, 150), # E5 - (784, 300), # G5 - ] - - for freq, duration in notes: - await hub.speaker.beep(freq, duration) - await wait(50) -async def play_ta_da(): - """Classic "ta-da!" sound""" - notes = [ - (392, 200), # G4 - (523, 400), # C5 - ] - - for freq, duration in notes: - await hub.speaker.beep(freq, duration) - await wait(100) -async def play_power_up(): - """Rising power-up sound""" - for freq in range(200, 800, 50): - await hub.speaker.beep(freq, 50) - await wait(10) - await hub.speaker.beep(1000, 200) -async def play_rickroll_inspired(): - """Fun 80s-style dance beat inspired sound""" - # Upbeat bouncy rhythm - pattern = [ - (392, 200), (440, 200), (494, 200), (523, 200), - (440, 200), (392, 200), (349, 200), (392, 300), - (440, 200), (392, 200), (349, 200), (330, 400), - ] - - for freq, duration in pattern: - await hub.speaker.beep(freq, duration) - await wait(50) -async def celebrate_mission_complete(sound_type=CelebrationSound.SUCCESS_CHIME): - """ - Main celebration function to call after completing a mission. - Plays a sound and shows light animation. - - Args: - sound_type: CelebrationSound enum value (default: SUCCESS_CHIME) - """ - # Light show - hub.light.on(Color.GREEN) - - # Play the selected celebration sound - if sound_type == CelebrationSound.VICTORY_FANFARE: - await play_victory_fanfare() - elif sound_type == CelebrationSound.LEVEL_UP: - await play_level_up() - elif sound_type == CelebrationSound.SUCCESS_CHIME: - await play_success_chime() - elif sound_type == CelebrationSound.TA_DA: - await play_ta_da() - elif sound_type == CelebrationSound.POWER_UP: - await play_power_up() - elif sound_type == CelebrationSound.RICKROLL_INSPIRED: - await play_rickroll_inspired() - else: - await play_success_chime() # Default fallback - - # Blink the light - for _ in range(3): - hub.light.off() - await wait(100) - hub.light.on(Color.GREEN) - await wait(100) - - hub.light.off() - -async def Run1(): - 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(-700) - -async def Run2(): - await drive_base.straight(200) - await drive_base.turn(-20) - await drive_base.straight(525) - await drive_base.turn(60) - - await drive_base.straight(50) - await right_arm.run_angle(2000,1000) - await drive_base.straight(-50) - await drive_base.turn(45) - await drive_base.straight(50) - await right_arm.run_angle(350,-1000) - await drive_base.straight(-100) - await drive_base.turn(-100) - await drive_base.straight(-750) - - -async def Run3(): - await drive_base.straight(920) - await drive_base.turn(-90) - await drive_base.straight(60) - drive_base.turn(-10) - await left_arm.run_angle(10000,-4000) - await drive_base.straight(-110) - await drive_base.turn(90) - await drive_base.straight(2000) - -async def Run4(): - await drive_base.straight(519) - await drive_base.straight(519) - await arm_motor_left.run_angle(-10000, 300) - await arm_motor_left.run_angle(10000, 600) - await drive_base.straight(160) - await drive_base.turn(-30) - await drive_base.straight(50) - await arm_motor.run_angle(3000, 3000) - await drive_base.straight(-150) - await drive_base.turn(135) - await drive_base.straight(50) - await arm_motor.run_angle(10000, -3000) - await drive_base.straight(-100) - await drive_base.turn(-54) - await arm_motor.run_angle(10000, -3000) - await drive_base.straight(250) - await drive_base.turn(-5) - await arm_motor.run_angle(10000, 7000) - await drive_base.straight(-50) - await drive_base.turn(68) - await arm_motor.run_angle(10000, -6000) - await drive_base.straight(200) - await arm_motor.run_angle(10000, 4000) - await drive_base.turn(-40) - await arm_motor.run_angle(10000, 7000) - -async def Run5(): - await drive_base.straight(420) - await right_arm.run_angle(300,-100) - await drive_base.straight(-100) - await right_arm.run_angle(300, 100) - await drive_base.straight(-350) - - -async def Run6(): - left_arm.run_angle(500,200) - right_arm.run_angle(500,200) - await drive_base.straight(70) - - await drive_base.turn(-55) - await drive_base.straight(900) - await drive_base.turn(92.5) - - await drive_base.straight(75) - await drive_base.straight(21) - await right_arm.run_angle(500,-250) - await right_arm.run_angle(500,250) - await drive_base.turn(55) - - await left_arm.run_angle(300,-400) - - await drive_base.turn(46.5) - await drive_base.turn(-40) - await drive_base.straight(900) -async def main(): - while True: - - #await celebrate_mission_complete(CelebrationSound.LEVEL_UP) - color_reflected_percent = await color_sensor.reflection() - print(color_reflected_percent) - - color_detected = await color_sensor.color() - print(f'Detected color: {color_detected.h}, {color_detected.s}, {color_detected.v}') - if color_reflected_percent > 1: - if color_detected == Color.GREEN: - print('Running Mission 1') - await Run1() - #await celebrate_mission_complete(CelebrationSound.VICTORY_FANFARE) - elif color_detected == Color.WHITE: - print('Running Mission 2') - await Run2() - #await celebrate_mission_complete(CelebrationSound.RICKROLL_INSPIRED) - elif color_detected == Color.YELLOW: - print('Running Mission 3') - await Run3() - #await celebrate_mission_complete(CelebrationSound.SUCCESS_CHIME ) - elif color_detected == Color.BLUE: - print('Running Mission 4') - await Run4() - #await celebrate_mission_complete(CelebrationSound.POWER_UP) - elif color_detected == Color.ORANGE: - print('Running Mission 5') - await Run5() - #await celebrate_mission_complete(CelebrationSound.LEVEL_UP) - elif color_detected == Color.PURPLE: - print('Running Mission 6 (this is ayaan\'s code)') - await Run6() - #await celebrate_mission_complete(CelebrationSound.LEVEL_UP) - else: - hub.light.off() - left_motor.stop() - right_motor.stop() - await wait(1000) #prevent loop from iterating fast -# Main execution loop -run_task(main()) From 79158d0f15af2766ddf1e0dcf6877c2ca129c93e Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:22:24 +0000 Subject: [PATCH 48/75] Update competition_codes/regional-final/RegionalFinalOld.py --- .../regional-final/RegionalFinalOld.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename RegionalFinal.py => competition_codes/regional-final/RegionalFinalOld.py (100%) diff --git a/RegionalFinal.py b/competition_codes/regional-final/RegionalFinalOld.py similarity index 100% rename from RegionalFinal.py rename to competition_codes/regional-final/RegionalFinalOld.py From 3bbf3fb2e6dcfebf0667a75577aa6fc51920df0b Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:22:47 +0000 Subject: [PATCH 49/75] Update competition_codes/twist_scrimmagetwist_scrimmage.py --- .../twist_scrimmagetwist_scrimmage.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename twist_scrimmage.py => competition_codes/twist_scrimmagetwist_scrimmage.py (100%) diff --git a/twist_scrimmage.py b/competition_codes/twist_scrimmagetwist_scrimmage.py similarity index 100% rename from twist_scrimmage.py rename to competition_codes/twist_scrimmagetwist_scrimmage.py From ba3e83343404b97433f7ec1d733cee0fa53fd300 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:22:52 +0000 Subject: [PATCH 50/75] Update competition_codes/twist_scrimmage/twist_scrimmage.py --- .../twist_scrimmage.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename competition_codes/{twist_scrimmagetwist_scrimmage.py => twist_scrimmage/twist_scrimmage.py} (100%) diff --git a/competition_codes/twist_scrimmagetwist_scrimmage.py b/competition_codes/twist_scrimmage/twist_scrimmage.py similarity index 100% rename from competition_codes/twist_scrimmagetwist_scrimmage.py rename to competition_codes/twist_scrimmage/twist_scrimmage.py From b3ab479bc43c65f9fb590ac8c563f18f3fd669a9 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:24:33 +0000 Subject: [PATCH 51/75] Update final/old_combined/1main.py --- final/{ => old_combined}/1main.py | 1 - 1 file changed, 1 deletion(-) rename final/{ => old_combined}/1main.py (99%) diff --git a/final/1main.py b/final/old_combined/1main.py similarity index 99% rename from final/1main.py rename to final/old_combined/1main.py index f535603..4912045 100644 --- a/final/1main.py +++ b/final/old_combined/1main.py @@ -1,4 +1,3 @@ - from pybricks.hubs import PrimeHub from pybricks.pupdevices import Motor, ColorSensor from pybricks.parameters import Port, Stop, Color, Direction From dbf5fc4880176662854bf5bc2559154427d9b0ee Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:24:59 +0000 Subject: [PATCH 52/75] Update old_combined/1main.py --- {final/old_combined => old_combined}/1main.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {final/old_combined => old_combined}/1main.py (100%) diff --git a/final/old_combined/1main.py b/old_combined/1main.py similarity index 100% rename from final/old_combined/1main.py rename to old_combined/1main.py From b0fd33d6b063e2b36471043e0d0fbece7930ad44 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:25:06 +0000 Subject: [PATCH 53/75] Update old_combined/2main.py --- {final => old_combined}/2main.py | 1 - 1 file changed, 1 deletion(-) rename {final => old_combined}/2main.py (99%) diff --git a/final/2main.py b/old_combined/2main.py similarity index 99% rename from final/2main.py rename to old_combined/2main.py index 7c0940d..892e31c 100644 --- a/final/2main.py +++ b/old_combined/2main.py @@ -1,4 +1,3 @@ - from pybricks.hubs import PrimeHub from pybricks.pupdevices import Motor, ColorSensor from pybricks.parameters import Port, Stop, Color, Direction From 664defae7347bab73914b5ce205502dd2b26a51d Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:25:14 +0000 Subject: [PATCH 54/75] Update old_combined/3main.py --- {final => old_combined}/3main.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {final => old_combined}/3main.py (100%) diff --git a/final/3main.py b/old_combined/3main.py similarity index 100% rename from final/3main.py rename to old_combined/3main.py From ccd30af870c0ddb48a2e0007e8bdf6e82497956d Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:27:28 +0000 Subject: [PATCH 55/75] Update old_combined/4main.py --- {final => old_combined}/4main.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {final => old_combined}/4main.py (100%) diff --git a/final/4main.py b/old_combined/4main.py similarity index 100% rename from final/4main.py rename to old_combined/4main.py From 65373a16d3a78a7a82125a957cebbcb58e6d0130 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:27:35 +0000 Subject: [PATCH 56/75] Update old_combined/main5.py --- {final => old_combined}/main5.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {final => old_combined}/main5.py (100%) diff --git a/final/main5.py b/old_combined/main5.py similarity index 100% rename from final/main5.py rename to old_combined/main5.py From 3e4dab3c9c38905589d55e3404fe2ce304e0ad50 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:28:45 +0000 Subject: [PATCH 57/75] Delete utils/color_sensor_navi.py --- utils/color_sensor_navi.py | 41 -------------------------------------- 1 file changed, 41 deletions(-) delete mode 100644 utils/color_sensor_navi.py diff --git a/utils/color_sensor_navi.py b/utils/color_sensor_navi.py deleted file mode 100644 index 54bb542..0000000 --- a/utils/color_sensor_navi.py +++ /dev/null @@ -1,41 +0,0 @@ -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor -from pybricks.parameters import Button, Color, Direction, Port, Side, Stop -from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch - -hub = PrimeHub() - -# Robot hardware configuration
ROBOT_CONFIG = {
 # Drive motors
 'left_motor': Motor(Port.A),
 'right_motor': Motor(Port.B),
 
 # Attachment motors
 'attachment_motor': Motor(Port.C),
 'lift_motor': Motor(Port.D),
 
 # Sensors
 'color_left': ColorSensor(Port.E),
 'color_right': ColorSensor(Port.F),
 'ultrasonic': UltrasonicSensor(Port.S1),
 
 # Hub reference
 'hub': hub
}

# Speed and distance constants
SPEEDS = {
 'FAST': 400,
 'NORMAL': 250,
 'SLOW': 100,
 'PRECISE': 50
}

DISTANCES = {
 'TILE_SIZE': 300, # mm per field tile
 'ROBOT_LENGTH': 180, # mm
 'ROBOT_WIDTH': 140 # mm
} - - -def mission_run_1(): - print('Running missions in Run 1') - -def mission_run_2(): - print('Running missions in Run 2') - -def mission_run_3(): - print('Running missions in Run 3') - -# In main.py - sensor-based navigation -def main(self): - """Use color sensor to select runs""" - print("Place colored object in front of sensor:") - print("RED=Run1, GREEN=Run2, BLUE=Run3, YELLOW=Test") - while True: - color = ROBOT_CONFIG['color_left'].color() - if color == Color.RED: - mission_run_1() - break - elif color == Color.GREEN: - mission_run_2() - break - elif color == Color.BLUE: - mission_run_3() - break - elif color == Color.YELLOW: - self.test_mode() - break - wait(1000) -main() From aed13bedff07ad165620ff3924fe3847e4841041 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:29:06 +0000 Subject: [PATCH 58/75] Delete utils/constants.py --- utils/constants.py | 13 ------------- 1 file changed, 13 deletions(-) delete mode 100644 utils/constants.py diff --git a/utils/constants.py b/utils/constants.py deleted file mode 100644 index f74d325..0000000 --- a/utils/constants.py +++ /dev/null @@ -1,13 +0,0 @@ -#Speed and distance constants -SPEEDS = { - 'FAST': 400, - 'NORMAL': 250, - 'SLOW': 100, - 'PRECISE': 50 -} - -DISTANCES = { - 'TILE_SIZE': 300, # mm per field tile - 'ROBOT_LENGTH': 180, # mm - 'ROBOT_WIDTH': 140 # mm -} \ No newline at end of file From e306b7dce164b9a85008a2bcb052dc7580506a9f Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Fri, 9 Jan 2026 13:30:08 +0000 Subject: [PATCH 59/75] Delete utils/starter_drivebase_code.py --- utils/starter_drivebase_code.py | 36 --------------------------------- 1 file changed, 36 deletions(-) delete mode 100644 utils/starter_drivebase_code.py diff --git a/utils/starter_drivebase_code.py b/utils/starter_drivebase_code.py deleted file mode 100644 index d7cb0b7..0000000 --- a/utils/starter_drivebase_code.py +++ /dev/null @@ -1,36 +0,0 @@ -from pybricks.hubs import PrimeHub -from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor -from pybricks.parameters import Button, Color, Direction, Port, Side, Stop -from pybricks.robotics import DriveBase -from pybricks.tools import wait, StopWatch - -hub = PrimeHub() - -# Initialize both motors. In this example, the motor on the -# left must turn counterclockwise to make the robot go forward. -left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) -right_motor = Motor(Port.B) - -arm_motor = Motor(Port.E, Direction.CLOCKWISE) -arm_motor.run_angle(299,90, Stop.HOLD) -# Initialize the drive base. In this example, the wheel diameter is 56mm. -# The distance between the two wheel-ground contact points is 112mm. -drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=112) - -print('The default settings are: ' + str(drive_base.settings())) -drive_base.settings(100,1000,166,750) -# Optionally, uncomment the line below to use the gyro for improved accuracy. -drive_base.use_gyro(True) - -# Drive forward by 500mm (half a meter). -drive_base.straight(500) - -# Turn around clockwise by 180 degrees. -drive_base.turn(180) - -# Drive forward again to get back to the start. -drive_base.straight(500) - -# Turn around counterclockwise. -drive_base.turn(-180) -arm_motor.run_angle(299,-90, Stop.HOLD) \ No newline at end of file From 0e009d2da4371a57bbd72e1e26bb5db1364c8616 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Fri, 9 Jan 2026 14:33:43 +0000 Subject: [PATCH 60/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 4b0cebb..e9e6fb0 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -84,14 +84,14 @@ Run#1 async def Run1(): # Fast approach to near-stall position - await left_arm.run_angle(2000, 180) # Fast movement upward + await left_arm.run_angle(2000, -190) # Fast movement downward # Gentle stall detection (shorter distance = faster) - await left_arm.run_until_stalled(1500, duty_limit=15) + await left_arm.run_until_stalled(-1500, duty_limit=15) left_arm.reset_angle(0) print(f"Initial left arm angle : {left_arm.angle()}") - await solve_whats_on_sale_v2() + await solve_whats_on_sale_v3() await solve_silo() # return to base @@ -100,13 +100,13 @@ async def Run1(): await drive_base.arc(200,None,-300) drive_base.stop() -async def solve_whats_on_sale(): +async def solve_whats_on_sale_v3(): right_arm.run_angle(500,30) #Automated inconsistency #left_arm.run_angle(500,-119.5) - left_arm.run_target(500,-121.5, Stop.HOLD) + left_arm.run_target(500,70, Stop.HOLD) print(f"Position left arm angle : {left_arm.angle()}") await drive_base.straight(180) @@ -412,6 +412,7 @@ async def Run11(): # experimental surface brushing attachment #await drive_base.straight(400) #await left_arm.run_angle(50,120) #await drive_base.straight(-200) + await wait(67) await left_arm.run_angle(300,-300) await drive_base.straight(-600) drive_base.stop() From 70c4df4a12fec062207b50fc4a62c5acd4cd86b4 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sat, 10 Jan 2026 03:01:48 +0000 Subject: [PATCH 61/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index e9e6fb0..765521e 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -408,12 +408,9 @@ async def Run11(): # experimental surface brushing attachment set_default_speed() await drive_base.straight(-100) await drive_base.turn(30) - await drive_base.straight(-190) #await drive_base.straight(400) #await left_arm.run_angle(50,120) #await drive_base.straight(-200) - await wait(67) - await left_arm.run_angle(300,-300) await drive_base.straight(-600) drive_base.stop() From e46897548b30e12d1f03e2ccd6cd19660a40f54b Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sat, 10 Jan 2026 03:02:02 +0000 Subject: [PATCH 62/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 765521e..981a6e9 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -84,7 +84,7 @@ Run#1 async def Run1(): # Fast approach to near-stall position - await left_arm.run_angle(2000, -190) # Fast movement downward + await left_arm.run_angle(2000, -210) # Fast movement downward # Gentle stall detection (shorter distance = faster) await left_arm.run_until_stalled(-1500, duty_limit=15) @@ -106,7 +106,8 @@ async def solve_whats_on_sale_v3(): #Automated inconsistency #left_arm.run_angle(500,-119.5) - left_arm.run_target(500,70, Stop.HOLD) + await left_arm.run_angle(500, 75,Stop.HOLD) + #await left_arm.run_target(500,90,Stop.HOLD) print(f"Position left arm angle : {left_arm.angle()}") await drive_base.straight(180) @@ -153,7 +154,7 @@ async def solve_whats_on_sale_v2(): async def solve_silo(): await drive_base.straight(-80) - await drive_base.turn(42) + await drive_base.turn(43) await drive_base.straight(95) SPEED = 10000 # speed in degree per second From 54dd1db320f93c5c859b5b4644c0c1b875575b08 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sun, 11 Jan 2026 00:47:37 +0000 Subject: [PATCH 63/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 981a6e9..a085862 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -84,7 +84,7 @@ Run#1 async def Run1(): # Fast approach to near-stall position - await left_arm.run_angle(2000, -210) # Fast movement downward + await left_arm.run_angle(2000, -190) # Fast movement downward # Gentle stall detection (shorter distance = faster) await left_arm.run_until_stalled(-1500, duty_limit=15) @@ -106,8 +106,7 @@ async def solve_whats_on_sale_v3(): #Automated inconsistency #left_arm.run_angle(500,-119.5) - await left_arm.run_angle(500, 75,Stop.HOLD) - #await left_arm.run_target(500,90,Stop.HOLD) + left_arm.run_target(500,70, Stop.HOLD) print(f"Position left arm angle : {left_arm.angle()}") await drive_base.straight(180) @@ -154,7 +153,7 @@ async def solve_whats_on_sale_v2(): async def solve_silo(): await drive_base.straight(-80) - await drive_base.turn(43) + await drive_base.turn(42) await drive_base.straight(95) SPEED = 10000 # speed in degree per second @@ -407,12 +406,14 @@ async def Run11(): # experimental surface brushing attachment #left_arm.run_angle(300,218) set_default_speed() - await drive_base.straight(-100) + await drive_base.straight(-120) await drive_base.turn(30) #await drive_base.straight(400) #await left_arm.run_angle(50,120) #await drive_base.straight(-200) - await drive_base.straight(-600) + await drive_base.straight(-129) + await left_arm.run_angle(300, -75) + await drive_base.straight(-400) drive_base.stop() async def Run12(): From eb9b2ca30fc37654217fa2a0035a9a04b1aad645 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sun, 11 Jan 2026 00:57:25 +0000 Subject: [PATCH 64/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index a085862..f91001f 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -406,12 +406,12 @@ async def Run11(): # experimental surface brushing attachment #left_arm.run_angle(300,218) set_default_speed() - await drive_base.straight(-120) + await drive_base.straight(-210) await drive_base.turn(30) #await drive_base.straight(400) #await left_arm.run_angle(50,120) #await drive_base.straight(-200) - await drive_base.straight(-129) + await drive_base.straight(-119) await left_arm.run_angle(300, -75) await drive_base.straight(-400) drive_base.stop() From 2d57f65a58977bd3dfac8fd45c37e49b2de5b991 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sun, 11 Jan 2026 01:19:39 +0000 Subject: [PATCH 65/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index f91001f..5d2eb14 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -411,9 +411,9 @@ async def Run11(): # experimental surface brushing attachment #await drive_base.straight(400) #await left_arm.run_angle(50,120) #await drive_base.straight(-200) - await drive_base.straight(-119) + await drive_base.straight(-80) await left_arm.run_angle(300, -75) - await drive_base.straight(-400) + await drive_base.straight(-600) drive_base.stop() async def Run12(): From 67c6554c61c00ba290878d50d6188961da314c0b Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sun, 11 Jan 2026 20:16:08 +0000 Subject: [PATCH 66/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 5d2eb14..4f6560d 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -412,7 +412,7 @@ async def Run11(): # experimental surface brushing attachment #await left_arm.run_angle(50,120) #await drive_base.straight(-200) await drive_base.straight(-80) - await left_arm.run_angle(300, -75) + #await left_arm.run_angle(300, -75) await drive_base.straight(-600) drive_base.stop() From ea9723a67d01ed9ad27096ef8bc7cffa6c90de4e Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Mon, 12 Jan 2026 03:30:25 +0000 Subject: [PATCH 67/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 4f6560d..974a4d1 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -412,8 +412,8 @@ async def Run11(): # experimental surface brushing attachment #await left_arm.run_angle(50,120) #await drive_base.straight(-200) await drive_base.straight(-80) - #await left_arm.run_angle(300, -75) - await drive_base.straight(-600) + await left_arm.run_angle(300, -75) + await drive_base.arc(550,None,800) drive_base.stop() async def Run12(): From edd49e0a9bbcea62d6b26627860f2ef88ab20421 Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Mon, 12 Jan 2026 03:36:10 +0000 Subject: [PATCH 68/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index 974a4d1..d7beea2 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -411,9 +411,9 @@ async def Run11(): # experimental surface brushing attachment #await drive_base.straight(400) #await left_arm.run_angle(50,120) #await drive_base.straight(-200) - await drive_base.straight(-80) - await left_arm.run_angle(300, -75) - await drive_base.arc(550,None,800) + #await drive_base.straight(-80) + #await left_arm.run_angle(300, -75) + await drive_base.arc(-650,None,-800) drive_base.stop() async def Run12(): From 782b6ec78f51b11398aa69788a7863aae1dedd0a Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Mon, 12 Jan 2026 03:37:32 +0000 Subject: [PATCH 69/75] Update competition_codes/state/sunprarie_state_main.py --- competition_codes/state/sunprarie_state_main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main.py index d7beea2..2507054 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main.py @@ -406,7 +406,7 @@ async def Run11(): # experimental surface brushing attachment #left_arm.run_angle(300,218) set_default_speed() - await drive_base.straight(-210) + await drive_base.straight(-230) await drive_base.turn(30) #await drive_base.straight(400) #await left_arm.run_angle(50,120) From ba9ddf6373ed8e04254660a4ad18a8ff543bd247 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Wed, 14 Jan 2026 17:32:46 +0000 Subject: [PATCH 70/75] Update competition_codes/state/sunprarie_state_main_FINAL.py --- ...{sunprarie_state_main.py => sunprarie_state_main_FINAL.py} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename competition_codes/state/{sunprarie_state_main.py => sunprarie_state_main_FINAL.py} (99%) diff --git a/competition_codes/state/sunprarie_state_main.py b/competition_codes/state/sunprarie_state_main_FINAL.py similarity index 99% rename from competition_codes/state/sunprarie_state_main.py rename to competition_codes/state/sunprarie_state_main_FINAL.py index 2507054..04d244c 100644 --- a/competition_codes/state/sunprarie_state_main.py +++ b/competition_codes/state/sunprarie_state_main_FINAL.py @@ -24,9 +24,9 @@ drive_base.settings(600, 500, 300, 200) drive_base.use_gyro(True) """ -Debugging helps +Debugging helps :) """ -DEBUG = 1 # Enable when you want to show logs +DEBUG = 0 # Enable when you want to show logs # Example conversion function (adjust min/max values as needed for your hub) async def get_battery_percentage(voltage_mV:float): max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us From 0abada1c4b12dcc5baf57864e1d65c8f8f37b95b Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Sat, 11 Apr 2026 00:32:43 +0000 Subject: [PATCH 71/75] Add competition_codes/AROC/Nationals_Main.py New code for Nationals. Still subject to change. Red side unfinished, Rishi pls code. APDS is good, actually. --- competition_codes/AROC/Nationals_Main.py | 533 +++++++++++++++++++++++ 1 file changed, 533 insertions(+) create mode 100644 competition_codes/AROC/Nationals_Main.py diff --git a/competition_codes/AROC/Nationals_Main.py b/competition_codes/AROC/Nationals_Main.py new file mode 100644 index 0000000..7e7cad1 --- /dev/null +++ b/competition_codes/AROC/Nationals_Main.py @@ -0,0 +1,533 @@ +#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge. +import umath +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, [[12,36]],[[12,20,24]] ) # 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) + +""" +Debugging helps +""" +DEBUG = 1 # Enable when you want to show logs +# Example conversion function (adjust min/max values as needed for your hub) +async def get_battery_percentage(voltage_mV:float): + max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us + min_voltage = 5000.0 # min battery level + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100 + return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100 + +async def wait_button_release(): + """Wait for all buttons to be released""" + while hub.buttons.pressed(): + await wait(500) + await wait(1000) # Debounce delay + +WALL_DISTANCE = 300 # mm +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(1000,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) + +# Use this to set default +def set_default_speed(): + drive_base.settings(600, 500, 300, 200) + +# Use this to change drive base movement +def set_speed(straight_speed, st_acc, turn_speed, turn_acc): + drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc) + +""" +Run#1 +- Removed forge and who lived here part +- What's on sale + Silo +- Green Key +""" +async def Run1(): + + # Fast approach to near-stall position + await left_arm.run_angle(2000, -210) # Fast movement downward + + # Gentle stall detection (shorter distance = faster) + await left_arm.run_until_stalled(-1500, duty_limit=15) + left_arm.reset_angle(0) + print(f"Initial left arm angle : {left_arm.angle()}") + + await solve_whats_on_sale_v3() + 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_v3(): + + right_arm.run_angle(500,30) + + #Automated inconsistency + #left_arm.run_angle(500,-119.5) + await left_arm.run_angle(500, 75,Stop.HOLD) + #await left_arm.run_target(500,90,Stop.HOLD) + print(f"Position left arm angle : {left_arm.angle()}") + + await drive_base.straight(190) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_whats_on_sale_v2(): + + right_arm.run_angle(500,30) + + # Bring down left arm to position + await left_arm.run_angle(2000, -120) + #await left_arm.run_until_stalled(-500,duty_limit=15) + print(f"Position left arm angle : {left_arm.angle()}") + left_arm.reset_angle(0) + + await drive_base.straight(180) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_silo(): + await drive_base.straight(-80) + await drive_base.turn(45) + await drive_base.straight(120) + + SPEED = 10000 # speed in degree per second + SWING_ANGLE = 60 # the angle! + REBOUND_ADJ = 20 + + # Repeat this motion 4 times + for _ in range(4): + await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up + await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down + + + right_arm.run_angle(4000,60, 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() + await solve_flag() + + # return to base + await drive_base.turn(55) + await drive_base.straight(-190) + await drive_base.turn(30) + await drive_base.straight(-720) + drive_base.stop() + +async def solve_forge(): + left_arm.run_angle(100,90) + await right_arm.run_target(50,50) + await wait(800) + # await right_arm.run_angle(50,-30) + await drive_base.straight(50) + await drive_base.turn(-17) + await drive_base.straight(650) + await drive_base.turn(50) + + await drive_base.straight(90) + await drive_base.turn(-70) + await drive_base.straight(-60) + +async def solve_heavy_lifting(): + await right_arm.run_angle(2000,-160) # arm down + await wait(100) + await drive_base.turn(30) # turn right a little bit + await right_arm.run_angle(2000,160) #arm up + await drive_base.turn(-30) #reset position + +async def solve_who_lived_here(): + await drive_base.straight(35) + await drive_base.turn(-20) + await drive_base.straight(50) + await drive_base.turn(-25) + await drive_base.straight(-100) + await drive_base.turn(-5) + await drive_base.straight(300) + await drive_base.turn(60) + +async def solve_flag(): + await drive_base.straight(85) + await left_arm.run_angle(70,-90) + await wait(500) + await left_arm.run_angle(100,120) + await drive_base.straight(-45) + await drive_base.turn(-80) + await drive_base.straight(-20) + await left_arm.run_angle(250,-90) + await left_arm.run_angle(250,90) + + +""" +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_tip_the_scale() + await solve_angler_artifacts() + + #cross over to red side + await multitask( + drive_forward(), + monitor_distance() + ) + +async def solve_tip_the_scale(): + drive_base.straight(20) + await drive_base.arc(-275,None,365) + await drive_base.straight(280) + await drive_base.straight(-80) + await drive_base.turn(-50) + await drive_base.straight(80) + await drive_base.turn(40) + await drive_base.straight(295) + await drive_base.turn(-90) + +async def solve_angler_artifacts(): + await drive_base.straight(55) + await drive_base.turn(-10) + await left_arm.run_angle(10000,-800) + await drive_base.straight(-120) + await drive_base.turn(110) + await drive_base.turn(-25) + + + + + +""" +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(): + # Getting the sand down + await drive_base.straight(550) + await right_arm.run_angle(300,100) + await drive_base.straight(-75) + await right_arm.run_angle(300, -100) + # Shoving the boat into place + 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, -300) + await drive_base.straight(120) + await drive_base.turn(5) + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) + 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,200) + 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) + +async def Run10(): # experimental map reveal attachment + + await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(260) + left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-80) + await drive_base.turn(30) + await drive_base.straight(-300) + await drive_base.straight(400) + #await left_arm.run_angle(50,120) + await drive_base.straight(-200) + await left_arm.run_angle(300,-215) + await drive_base.straight(-600) + drive_base.stop() + +async def Run11(): # experimental surface brushing attachment + + await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(250) + #left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-100) + await drive_base.turn(30) + #await drive_base.straight(400) + #await left_arm.run_angle(50,120) + #await drive_base.straight(-200) + await drive_base.straight(-600) + drive_base.stop() + +async def Run12(): + await drive_base.straight(900) + await drive_base.turn(83) + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + drive_base.settings(150, 50, 50, 50) + await drive_base.straight(120) + left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await drive_base.straight(-100) + drive_base.settings(950, 750, 750, 750) + await drive_base.turn(110) + await drive_base.straight(1000) + + +# 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: + pressed = hub.buttons.pressed() + h, s, v = await color_sensor.hsv() + reflected = await color_sensor.reflection() + color = detect_color(h, s, v, reflected) + if DEBUG : + #print(color_sensor.color()) + #print(h,s,v) + #print(color) + print(f"button pressed: {pressed}") + + + 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 Run11() + elif color == "Light_Blue": + print("Running Mission 2_1") + await Run12() + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + #pass + + # Show battery % for debugging + if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = hub.battery.voltage() + # Use the function with your voltage reading + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file From 63bae21b8853880a695d68278ea76716acfefbe5 Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Sat, 9 May 2026 01:08:23 +0000 Subject: [PATCH 72/75] The-South_Awakens Has an advanced PID module, already implemented in RED Side. --- competition_codes/AROC/The_South-Awakens.py | 745 ++++++++++++++++++++ 1 file changed, 745 insertions(+) create mode 100644 competition_codes/AROC/The_South-Awakens.py diff --git a/competition_codes/AROC/The_South-Awakens.py b/competition_codes/AROC/The_South-Awakens.py new file mode 100644 index 0000000..93b3e4e --- /dev/null +++ b/competition_codes/AROC/The_South-Awakens.py @@ -0,0 +1,745 @@ +#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge. +import umath +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, [[12,36]],[[12,20,24]] ) # 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) + +""" +Debugging helps +""" +DEBUG = 1 # Enable when you want to show logs +# Example conversion function (adjust min/max values as needed for your hub) +async def get_battery_percentage(voltage_mV:float): + max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us + min_voltage = 5000.0 # min battery level + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100 + return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100 + +async def wait_button_release(): + """Wait for all buttons to be released""" + while hub.buttons.pressed(): + await wait(500) + await wait(1000) # Debounce delay + +WALL_DISTANCE = 300 # mm +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(1000,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) + +# Use this to set default +def set_default_speed(): + drive_base.settings(600, 500, 300, 200) + +# Use this to change drive base movement +def set_speed(straight_speed, st_acc, turn_speed, turn_acc): + drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc) + +# ───────────────────────────────────────────────────────────── +# Advanced PID Turn Controller +# ───────────────────────────────────────────────────────────── +# Tuning guide: +# Kp – start at 3.0, increase if turn is sluggish +# Ki – keep small (0.05–0.15); fights steady-state drift only +# Kd – start at 0.8; increase to kill overshoot +# DEADBAND – degrees of "close enough" before hard brake (2–4°) +# MAX_TURN_SPEED – degrees/sec cap; 200–400 for normal turns +# INTEGRAL_CLAMP – prevents windup from accumulated small errors + +PID_Kp = 3.0 +PID_Ki = 0.08 +PID_Kd = 1.2 +DEADBAND = 2.5 # degrees +MAX_TURN_SPEED = 350 # deg/s +INTEGRAL_CLAMP = 25 # max integral contribution in deg/s +SETTLE_MS = 60 # ms to confirm hold inside deadband + +def _clamp(val: float, lo: float, hi: float) -> float: + return max(lo, min(hi, val)) + +def _shortest_angle(error: float) -> float: + """Wrap error to [-180, 180] so turns never go the long way.""" + while error > 180: + error -= 360 + while error < -180: + error += 360 + return error + +async def gyro_turn(target_degrees: float, timeout_ms: int = 3000): + """ + Turn the robot to an absolute heading using a PID loop + driven by the hub IMU. + + Args: + target_degrees: absolute heading to reach (degrees) + timeout_ms: failsafe — stops if turn takes too long + """ + hub.imu.reset_heading(0) + current_heading = 0.0 + + integral = 0.0 + prev_error = 0.0 + settled_ms = 0 + + clock = StopWatch() + dt_watch = StopWatch() + clock.reset() + dt_watch.reset() + + while clock.time() < timeout_ms: + # ── 1. Read heading ───────────────────────────────── + current_heading = hub.imu.heading() + error = _shortest_angle(target_degrees - current_heading) + + # ── 2. Deadband check ─────────────────────────────── + if abs(error) < DEADBAND: + settled_ms += 10 + if settled_ms >= SETTLE_MS: + break # confirmed stable inside target + drive_base.drive(0, 0) + await wait(10) + continue + else: + settled_ms = 0 # reset settle counter on drift + + # ── 3. dt in seconds (for consistent Ki/Kd scaling) ─ + dt = dt_watch.time() / 1000.0 + dt_watch.reset() + if dt <= 0 or dt > 0.5: # skip bad first/stall tick + dt = 0.02 + + # ── 4. P term ─────────────────────────────────────── + P = PID_Kp * error + + # ── 5. I term with anti-windup clamp ──────────────── + integral += error * dt + integral = _clamp(integral, -INTEGRAL_CLAMP / PID_Ki, + INTEGRAL_CLAMP / PID_Ki) + I = PID_Ki * integral + + # ── 6. D term (derivative-on-measurement, not error) ─ + # Using (error - prev_error)/dt avoids "derivative kick" + # when the target suddenly changes. + D = PID_Kd * (error - prev_error) / dt + prev_error = error + + # ── 7. Sum and clamp total output ─────────────────── + output = _clamp(P + I + D, + -MAX_TURN_SPEED, + MAX_TURN_SPEED) + + # ── 8. Drive with turn-only command ───────────────── + drive_base.drive(0, output) + + await wait(10) # 100 Hz loop + + # Hard brake — holds position with motor hold + drive_base.stop() + await wait(50) # brief hold for gyro to settle + +# ───────────────────────────────────────────────────────────── +# Advanced PID Straight Controller +# ───────────────────────────────────────────────────────────── +# Tuning guide: +# STRAIGHT_Kp – main speed push, start at 2.0 +# STRAIGHT_Ki – fights distance undershoot, keep tiny +# STRAIGHT_Kd – smooths deceleration, start at 0.5 +# HEADING_Kp – how hard it steers to stay straight +# STRAIGHT_DEADBAND – mm of "close enough" before braking + +STRAIGHT_Kp = 2.0 +STRAIGHT_Ki = 0.02 +STRAIGHT_Kd = 0.5 +HEADING_Kp = 3.5 # steering correction gain +STRAIGHT_DEADBAND = 5 # mm +STRAIGHT_CLAMP = 20 # anti-windup cap +MAX_STRAIGHT_SPEED = 800 # mm/s cap + +async def gyro_straight(distance_mm: float, speed: int = 600, timeout_ms: int = 5000): + """ + Drive straight a precise distance using dual PID: + - Distance PID → controls forward speed + - Heading PID → corrects steering drift in real time + + Args: + distance_mm: how far to travel (negative = backward) + speed: max forward speed in mm/s + timeout_ms: failsafe cutoff + """ + # ── Reset everything ──────────────────────────────────── + hub.imu.reset_heading(0) + left_motor.reset_angle(0) + right_motor.reset_angle(0) + + target_heading = 0.0 + integral = 0.0 + prev_dist_error = 0.0 + settled_ms = 0 + + clock = StopWatch() + dt_watch = StopWatch() + clock.reset() + dt_watch.reset() + + # Convert distance to motor degrees + wheel_circumference = umath.pi * WHEEL_DIAMETER # mm per full rotation + target_deg = (distance_mm / wheel_circumference) * 360.0 + + while clock.time() < timeout_ms: + + # ── 1. Current position from motor encoders ────────── + avg_angle = (left_motor.angle() + right_motor.angle()) / 2.0 + dist_error = target_deg - avg_angle + + # ── 2. Deadband check ──────────────────────────────── + dist_error_mm = dist_error / 360.0 * wheel_circumference + if abs(dist_error_mm) < STRAIGHT_DEADBAND: + settled_ms += 10 + if settled_ms >= 60: + break + left_motor.hold() + right_motor.hold() + await wait(10) + continue + else: + settled_ms = 0 + + # ── 3. dt ──────────────────────────────────────────── + dt = dt_watch.time() / 1000.0 + dt_watch.reset() + if dt <= 0 or dt > 0.5: + dt = 0.02 + + # ── 4. Distance PID ────────────────────────────────── + P_dist = STRAIGHT_Kp * dist_error_mm + + integral += dist_error_mm * dt + integral = _clamp(integral, + -STRAIGHT_CLAMP / STRAIGHT_Ki, + STRAIGHT_CLAMP / STRAIGHT_Ki) + I_dist = STRAIGHT_Ki * integral + + D_dist = STRAIGHT_Kd * (dist_error_mm - prev_dist_error) / dt + prev_dist_error = dist_error_mm + + forward_speed = _clamp(P_dist + I_dist + D_dist, + -speed, speed) + + # ── 5. Heading PID (steering correction) ───────────── + heading_error = target_heading - hub.imu.heading() + heading_error = _shortest_angle(heading_error) + steering = HEADING_Kp * heading_error + + # ── 6. Mix forward + steering into each motor ──────── + # Positive steering correction speeds up left, slows right + left_speed = _clamp(forward_speed + steering, + -MAX_STRAIGHT_SPEED, MAX_STRAIGHT_SPEED) + right_speed = _clamp(forward_speed - steering, + -MAX_STRAIGHT_SPEED, MAX_STRAIGHT_SPEED) + + left_motor.run(left_speed) + right_motor.run(right_speed) + + await wait(10) # 100 Hz loop + + # ── Hard brake ─────────────────────────────────────────── + left_motor.hold() + right_motor.hold() + await wait(50) + +""" +Run#1 +- Removed forge and who lived here part +- What's on sale + Silo +- Green Key +""" +async def Run1(): + + # Fast approach to near-stall position + await left_arm.run_angle(2000, -210) # Fast movement downward + + # Gentle stall detection (shorter distance = faster) + await left_arm.run_until_stalled(-1500, duty_limit=15) + left_arm.reset_angle(0) + print(f"Initial left arm angle : {left_arm.angle()}") + + await solve_whats_on_sale_v3() + 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_v3(): + + right_arm.run_angle(500,30) + + #Automated inconsistency + #left_arm.run_angle(500,-119.5) + await left_arm.run_angle(500, 75,Stop.HOLD) + #await left_arm.run_target(500,90,Stop.HOLD) + print(f"Position left arm angle : {left_arm.angle()}") + + await drive_base.straight(190) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_whats_on_sale_v2(): + + right_arm.run_angle(500,30) + + # Bring down left arm to position + await left_arm.run_angle(2000, -120) + #await left_arm.run_until_stalled(-500,duty_limit=15) + print(f"Position left arm angle : {left_arm.angle()}") + left_arm.reset_angle(0) + + await drive_base.straight(180) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_silo(): + await drive_base.straight(-80) + await drive_base.turn(45) + await drive_base.straight(120) + + SPEED = 10000 # speed in degree per second + SWING_ANGLE = 60 # the angle! + REBOUND_ADJ = 20 + + # Repeat this motion 4 times + for _ in range(4): + await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up + await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down + + + right_arm.run_angle(4000,60, 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() + await solve_flag() + + # return to base + await drive_base.turn(55) + await drive_base.straight(-190) + await drive_base.turn(30) + await drive_base.straight(-720) + drive_base.stop() + +async def solve_forge(): + left_arm.run_angle(100,90) + await right_arm.run_target(50,50) + await wait(800) + # await right_arm.run_angle(50,-30) + await drive_base.straight(50) + await drive_base.turn(-17) + await drive_base.straight(650) + await drive_base.turn(50) + + await drive_base.straight(90) + await drive_base.turn(-70) + await drive_base.straight(-60) + +async def solve_heavy_lifting(): + await right_arm.run_angle(2000,-160) # arm down + await wait(100) + await drive_base.turn(30) # turn right a little bit + await right_arm.run_angle(2000,160) #arm up + await drive_base.turn(-30) #reset position + +async def solve_who_lived_here(): + await drive_base.straight(35) + await drive_base.turn(-20) + await drive_base.straight(50) + await drive_base.turn(-25) + await drive_base.straight(-100) + await drive_base.turn(-5) + await drive_base.straight(300) + await drive_base.turn(60) + +async def solve_flag(): + await drive_base.straight(85) + await left_arm.run_angle(70,-90) + await wait(500) + await left_arm.run_angle(100,120) + await drive_base.straight(-45) + await drive_base.turn(-80) + await drive_base.straight(-20) + await left_arm.run_angle(250,-90) + await left_arm.run_angle(250,90) + + +""" +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_tip_the_scale() + await solve_angler_artifacts() + + #cross over to red side + await multitask( + drive_forward(), + monitor_distance() + ) + +async def solve_tip_the_scale(): + drive_base.straight(20) + await drive_base.arc(-275,None,365) + await drive_base.straight(280) + await drive_base.straight(-80) + await drive_base.turn(-50) + await drive_base.straight(80) + await drive_base.turn(40) + await drive_base.straight(295) + await drive_base.turn(-90) + +async def solve_angler_artifacts(): + await drive_base.straight(55) + await drive_base.turn(-10) + await left_arm.run_angle(10000,-800) + await drive_base.straight(-120) + await drive_base.turn(110) + await drive_base.turn(-25) + + + + + +""" +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(): + # Getting the sand down + await gyro_straight(550) + await right_arm.run_angle(300,100) + await gyro_straight(-75) + await right_arm.run_angle(300, -100) + # Shoving the boat into place + await gyro_straight(300) + await gyro_straight(-200) + await gyro_turn(-15) + # Solving statue + await gyro_straight(350) + await gyro_turn(-104) + await gyro_straight(-80) + await left_arm.run_angle(500, -300) + await gyro_straight(120) + await gyro_turn(5) + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) + 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,200) + 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) + +async def Run10(): # experimental map reveal attachment + + await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(260) + left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-80) + await drive_base.turn(30) + await drive_base.straight(-300) + await drive_base.straight(400) + #await left_arm.run_angle(50,120) + await drive_base.straight(-200) + await left_arm.run_angle(300,-215) + await drive_base.straight(-600) + drive_base.stop() + +async def Run11(): # experimental surface brushing attachment + + await gyro_straight(600) + drive_base.settings(150, 750, 50, 500) + await gyro_turn(-30) + await gyro_straight(250) + #left_arm.run_angle(300,218) + + set_default_speed() + await gyro_straight(-100) + await drive_base.turn(30) + #await drive_base.straight(400) + #await left_arm.run_angle(50,120) + #await drive_base.straight(-200) + await gyro_straight(-600) + drive_base.stop() + +async def Run12(): + await gyro_straight(900) + await gyro_turn(83) + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + drive_base.settings(150, 50, 50, 50) + await gyro_straight(120) + left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await gyro_straight(-100) + drive_base.settings(950, 750, 750, 750) + await gyro_turn(110) + await gyro_straight(1000) + + +# 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: + pressed = hub.buttons.pressed() + h, s, v = await color_sensor.hsv() + reflected = await color_sensor.reflection() + color = detect_color(h, s, v, reflected) + if DEBUG : + #print(color_sensor.color()) + #print(h,s,v) + #print(color) + print(f"button pressed: {pressed}") + + + 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 Run11() + elif color == "Light_Blue": + print("Running Mission 2_1") + await Run12() + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + #pass + + # Show battery % for debugging + if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = hub.battery.voltage() + # Use the function with your voltage reading + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file From 8871e16b4eda6a41376d10921b9988205a1926b0 Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Fri, 15 May 2026 12:49:43 +0000 Subject: [PATCH 73/75] Update competition_codes/AROC/The_South-Awakens.py Yes, it has an extra 500 lines of code, but its APID. --- competition_codes/AROC/The_South-Awakens.py | 1032 ++++++++++++------- 1 file changed, 652 insertions(+), 380 deletions(-) diff --git a/competition_codes/AROC/The_South-Awakens.py b/competition_codes/AROC/The_South-Awakens.py index 93b3e4e..fba12b4 100644 --- a/competition_codes/AROC/The_South-Awakens.py +++ b/competition_codes/AROC/The_South-Awakens.py @@ -10,282 +10,585 @@ 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, [[12,36]],[[12,20,24]] ) # Specify default direction -right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration +right_motor = Motor(Port.B, Direction.CLOCKWISE) +left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12, 36]], [[12, 20, 24]]) +right_arm = Motor(Port.D, Direction.CLOCKWISE, [[12, 36], [12, 20, 24]]) 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) +WHEEL_DIAMETER = 68.8 # mm +AXLE_TRACK = 180 # mm drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) drive_base.settings(600, 500, 300, 200) drive_base.use_gyro(True) +# ============================================================ +# ADVANCED PID MODULE +# ============================================================ + +class PIDController: + """ + Advanced PID Controller with: + - Anti-windup (integral clamping) + - Derivative filtering (low-pass) + - Deadband tolerance + - Output saturation + - Setpoint ramping + - Error history tracking for diagnostics + """ + + def __init__( + self, + kp: float, + ki: float, + kd: float, + setpoint: float = 0.0, + output_min: float = -100.0, + output_max: float = 100.0, + integral_limit: float = 1000.0, + deadband: float = 0.5, + derivative_filter: float = 0.2, + name: str = "PID", + ): + self.kp = kp + self.ki = ki + self.kd = kd + self.setpoint = setpoint + self.output_min = output_min + self.output_max = output_max + self.integral_limit = integral_limit + self.deadband = deadband + self.derivative_filter = derivative_filter + self._integral = 0.0 + self._prev_error = 0.0 + self._prev_derivative = 0.0 + self._timer = StopWatch() + self._first_update = True + self.name = name + self._last_output = 0.0 + self._last_p = 0.0 + self._last_i = 0.0 + self._last_d = 0.0 + self._error_history = [] + self._max_history = 50 + + def reset(self): + self._integral = 0.0 + self._prev_error = 0.0 + self._prev_derivative = 0.0 + self._first_update = True + self._timer.reset() + self._last_output = 0.0 + self._last_p = 0.0 + self._last_i = 0.0 + self._last_d = 0.0 + self._error_history = [] + + def set_setpoint(self, setpoint: float): + self.setpoint = setpoint + + def set_gains(self, kp: float, ki: float, kd: float): + self.kp = kp + self.ki = ki + self.kd = kd + + def _clamp(self, value: float, low: float, high: float) -> float: + if value < low: + return low + if value > high: + return high + return value + + def update(self, current_value: float) -> float: + dt_ms = self._timer.time() + self._timer.reset() + + if self._first_update or dt_ms <= 0: + dt = 0.02 + self._first_update = False + else: + dt = dt_ms / 1000.0 + + error = self.setpoint - current_value + + if abs(error) < self.deadband: + error = 0.0 + + p_term = self.kp * error + + self._integral += error * dt + self._integral = self._clamp( + self._integral, -self.integral_limit, self.integral_limit + ) + i_term = self.ki * self._integral + + if dt > 0: + raw_derivative = (error - self._prev_error) / dt + else: + raw_derivative = 0.0 + + alpha = self.derivative_filter + filtered_derivative = ( + alpha * self._prev_derivative + (1.0 - alpha) * raw_derivative + ) + d_term = self.kd * filtered_derivative + + output = p_term + i_term + d_term + output = self._clamp(output, self.output_min, self.output_max) + + self._prev_error = error + self._prev_derivative = filtered_derivative + self._last_output = output + self._last_p = p_term + self._last_i = i_term + self._last_d = d_term + + if len(self._error_history) >= self._max_history: + self._error_history.pop(0) + self._error_history.append(error) + + return output + + def is_settled(self, tolerance: float = 1.0, samples: int = 5) -> bool: + if len(self._error_history) < samples: + return False + recent = self._error_history[-samples:] + return all(abs(e) < tolerance for e in recent) + + def diagnostics(self) -> str: + return ( + f"[{self.name}] P={self._last_p:.2f} I={self._last_i:.2f} " + f"D={self._last_d:.2f} Out={self._last_output:.2f} " + f"Err={self._prev_error:.2f}" + ) + + +# ============================================================ +# PID-BASED DRIVE VARIABLE +# ============================================================ + +class PIDDrive: + """ + High-level PID driving interface that wraps the gyro and motors. + """ + + def __init__( + self, + hub_ref, + left: Motor, + right: Motor, + drivebase: DriveBase, + ultrasonic: UltrasonicSensor, + wheel_diameter: float = 68.8, + axle_track: float = 180.0, + ): + self.hub = hub_ref + self.left = left + self.right = right + self.db = drivebase + self.ultrasonic = ultrasonic + self.wheel_diameter = wheel_diameter + self.axle_track = axle_track + self.mm_per_degree = (umath.pi * wheel_diameter) / 360.0 + + self.straight_pid = PIDController( + kp=3.0, ki=0.05, kd=1.5, + output_min=-150, output_max=150, + integral_limit=500, deadband=0.3, + derivative_filter=0.3, name="Straight", + ) + + self.turn_pid = PIDController( + kp=4.0, ki=0.1, kd=2.0, + output_min=-400, output_max=400, + integral_limit=800, deadband=0.5, + derivative_filter=0.25, name="Turn", + ) + + self.distance_pid = PIDController( + kp=1.5, ki=0.02, kd=1.0, + output_min=-300, output_max=300, + integral_limit=500, deadband=2.0, + derivative_filter=0.3, name="Distance", + ) + + self.accel_rate = 5 + self.decel_distance = 80 + + def _get_heading(self) -> float: + return self.hub.imu.heading() + + def _reset_heading(self): + self.hub.imu.reset_heading(0) + + def _reset_motors(self): + self.left.reset_angle(0) + self.right.reset_angle(0) + + def _get_distance_traveled(self) -> float: + left_deg = abs(self.left.angle()) + right_deg = abs(self.right.angle()) + avg_deg = (left_deg + right_deg) / 2.0 + return avg_deg * self.mm_per_degree + + def _drive_motors(self, left_speed: float, right_speed: float): + self.left.run(left_speed) + self.right.run(right_speed) + + def _stop_motors(self, stop_type=Stop.BRAKE): + self.left.stop() + self.right.stop() + if stop_type == Stop.HOLD: + self.left.hold() + self.right.hold() + + async def pid_straight( + self, + distance_mm: float, + base_speed: float = 400, + settle_tolerance: float = 2.0, + settle_samples: int = 3, + timeout_ms: int = 8000, + ): + self._reset_heading() + self._reset_motors() + self.straight_pid.reset() + self.straight_pid.set_setpoint(0) + + direction = 1 if distance_mm >= 0 else -1 + target_dist = abs(distance_mm) + timer = StopWatch() + + while True: + traveled = self._get_distance_traveled() + remaining = target_dist - traveled + + if remaining <= 0: + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Straight] Timeout after {timeout_ms}ms") + break + + ramp_up_speed = min(base_speed, 100 + traveled * self.accel_rate) + if remaining < self.decel_distance: + decel_speed = max(80, base_speed * (remaining / self.decel_distance)) + else: + decel_speed = base_speed + speed = min(ramp_up_speed, decel_speed) + + current_heading = self._get_heading() + correction = self.straight_pid.update(current_heading) + + left_speed = direction * speed + correction + right_speed = direction * speed - correction + + self._drive_motors(left_speed, right_speed) + + if DEBUG and timer.time() % 200 < 20: + print( + f"[Straight] dist={traveled:.0f}/{target_dist:.0f} " + f"spd={speed:.0f} {self.straight_pid.diagnostics()}" + ) + + await wait(10) + + self._stop_motors(Stop.BRAKE) + if DEBUG: + print( + f"[PID Straight] Complete: {self._get_distance_traveled():.1f}mm " + f"of {distance_mm}mm" + ) + + async def pid_turn( + self, + angle_deg: float, + max_speed: float = 300, + settle_tolerance: float = 1.0, + settle_samples: int = 5, + timeout_ms: int = 5000, + ): + self._reset_heading() + self.turn_pid.reset() + self.turn_pid.set_setpoint(angle_deg) + + timer = StopWatch() + + while True: + current_heading = self._get_heading() + output = self.turn_pid.update(current_heading) + + if output > max_speed: + output = max_speed + elif output < -max_speed: + output = -max_speed + + if abs(output) < 20 and abs(angle_deg - current_heading) > settle_tolerance: + if output > 0: + output = 20 + elif output < 0: + output = -20 + + self._drive_motors(output, -output) + + if self.turn_pid.is_settled(settle_tolerance, settle_samples): + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Turn] Timeout after {timeout_ms}ms") + break + + if DEBUG and timer.time() % 200 < 20: + print( + f"[Turn] heading={current_heading:.1f}/{angle_deg:.1f} " + f"{self.turn_pid.diagnostics()}" + ) + + await wait(10) + + self._stop_motors(Stop.HOLD) + if DEBUG: + final = self._get_heading() + print( + f"[PID Turn] Complete: {final:.1f}° of {angle_deg:.1f}° " + f"(error={angle_deg - final:.2f}°)" + ) + + async def pid_drive_until( + self, + base_speed: float = 300, + stop_distance_mm: float = 100, + timeout_ms: int = 10000, + ): + self._reset_heading() + self.straight_pid.reset() + self.straight_pid.set_setpoint(0) + + timer = StopWatch() + + while True: + distance = await self.ultrasonic.distance() + + if distance is not None and distance < stop_distance_mm: + if DEBUG: + print(f"[PID DriveUntil] Object at {distance}mm — stopping") + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID DriveUntil] Timeout") + break + + heading = self._get_heading() + correction = self.straight_pid.update(heading) + + if distance is not None and distance < stop_distance_mm * 2: + speed_factor = distance / (stop_distance_mm * 2) + speed = max(80, base_speed * speed_factor) + else: + speed = base_speed + + self._drive_motors(speed + correction, speed - correction) + + await wait(10) + + self._stop_motors(Stop.BRAKE) + + async def pid_align_to_wall( + self, + target_distance_mm: float = 100, + base_speed: float = 200, + tolerance: float = 3.0, + timeout_ms: int = 5000, + ): + self._reset_heading() + self.straight_pid.reset() + self.straight_pid.set_setpoint(0) + self.distance_pid.reset() + self.distance_pid.set_setpoint(target_distance_mm) + + timer = StopWatch() + + while True: + distance = await self.ultrasonic.distance() + + if distance is None: + await wait(20) + continue + + speed = -self.distance_pid.update(distance) + + heading = self._get_heading() + correction = self.straight_pid.update(heading) + + self._drive_motors(speed + correction, speed - correction) + + if self.distance_pid.is_settled(tolerance, 5): + if DEBUG: + print(f"[PID Align] Settled at {distance}mm") + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Align] Timeout") + break + + await wait(10) + + self._stop_motors(Stop.HOLD) + + async def pid_arc( + self, + radius_mm: float, + angle_deg: float, + base_speed: float = 300, + timeout_ms: int = 8000, + ): + self._reset_heading() + self.turn_pid.reset() + self._reset_motors() + + if radius_mm == 0: + await self.pid_turn(angle_deg, base_speed) + return + + abs_radius = abs(radius_mm) + outer_radius = abs_radius + (self.axle_track / 2.0) + inner_radius = abs_radius - (self.axle_track / 2.0) + + if inner_radius < 0: + inner_radius = 0 + + speed_ratio = inner_radius / outer_radius if outer_radius > 0 else 0 + + target_heading = angle_deg + self.turn_pid.set_setpoint(target_heading) + + direction = 1 if angle_deg >= 0 else -1 + timer = StopWatch() + + while True: + heading = self._get_heading() + correction = self.turn_pid.update(heading) + + if abs(target_heading) > 0: + progress = abs(heading) / abs(target_heading) + else: + progress = 1.0 + + if progress > 0.8: + speed = max(60, base_speed * (1.0 - progress) * 5) + else: + speed = base_speed + + outer_speed = direction * speed + inner_speed = direction * speed * speed_ratio + + if radius_mm > 0: + left_speed = outer_speed + correction * 0.3 + right_speed = inner_speed - correction * 0.3 + else: + left_speed = inner_speed + correction * 0.3 + right_speed = outer_speed - correction * 0.3 + + self._drive_motors(left_speed, right_speed) + + if self.turn_pid.is_settled(1.5, 4): + break + + if timer.time() > timeout_ms: + if DEBUG: + print(f"[PID Arc] Timeout") + break + + await wait(10) + + self._stop_motors(Stop.HOLD) + if DEBUG: + print(f"[PID Arc] Final heading: {self._get_heading():.1f}° / {angle_deg}°") + + +# ============================================================ +# INSTANTIATE THE PID DRIVE VARIABLE +# ============================================================ + +drive = PIDDrive( + hub_ref=hub, + left=left_motor, + right=right_motor, + drivebase=drive_base, + ultrasonic=lazer_ranger, + wheel_diameter=WHEEL_DIAMETER, + axle_track=AXLE_TRACK, +) + +# ============================================================ +# TUNING PRESETS +# ============================================================ + +def set_precise_mode(): + drive.straight_pid.set_gains(kp=4.0, ki=0.08, kd=2.0) + drive.turn_pid.set_gains(kp=5.0, ki=0.15, kd=2.5) + drive.accel_rate = 3 + drive.decel_distance = 100 + +def set_fast_mode(): + drive.straight_pid.set_gains(kp=2.5, ki=0.03, kd=1.0) + drive.turn_pid.set_gains(kp=3.0, ki=0.05, kd=1.5) + drive.accel_rate = 8 + drive.decel_distance = 60 + +def set_default_pid_mode(): + drive.straight_pid.set_gains(kp=3.0, ki=0.05, kd=1.5) + drive.turn_pid.set_gains(kp=4.0, ki=0.1, kd=2.0) + drive.accel_rate = 5 + drive.decel_distance = 80 + + """ Debugging helps """ -DEBUG = 1 # Enable when you want to show logs -# Example conversion function (adjust min/max values as needed for your hub) -async def get_battery_percentage(voltage_mV:float): - max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us - min_voltage = 5000.0 # min battery level - percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100 - return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100 +DEBUG = 1 +async def get_battery_percentage(voltage_mV: float): + max_voltage = 8400.0 + min_voltage = 5000.0 + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage)) * 100 + return max(0, min(100, percentage)) async def wait_button_release(): - """Wait for all buttons to be released""" while hub.buttons.pressed(): await wait(500) - await wait(1000) # Debounce delay + await wait(1000) + +WALL_DISTANCE = 300 -WALL_DISTANCE = 300 # mm async def drive_forward(): - """Drive forward continuously using DriveBase.""" - drive_base.drive(1000,0) + drive_base.drive(1000, 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) - + 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) -# Use this to set default def set_default_speed(): drive_base.settings(600, 500, 300, 200) -# Use this to change drive base movement def set_speed(straight_speed, st_acc, turn_speed, turn_acc): - drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc) + drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc) -# ───────────────────────────────────────────────────────────── -# Advanced PID Turn Controller -# ───────────────────────────────────────────────────────────── -# Tuning guide: -# Kp – start at 3.0, increase if turn is sluggish -# Ki – keep small (0.05–0.15); fights steady-state drift only -# Kd – start at 0.8; increase to kill overshoot -# DEADBAND – degrees of "close enough" before hard brake (2–4°) -# MAX_TURN_SPEED – degrees/sec cap; 200–400 for normal turns -# INTEGRAL_CLAMP – prevents windup from accumulated small errors - -PID_Kp = 3.0 -PID_Ki = 0.08 -PID_Kd = 1.2 -DEADBAND = 2.5 # degrees -MAX_TURN_SPEED = 350 # deg/s -INTEGRAL_CLAMP = 25 # max integral contribution in deg/s -SETTLE_MS = 60 # ms to confirm hold inside deadband - -def _clamp(val: float, lo: float, hi: float) -> float: - return max(lo, min(hi, val)) - -def _shortest_angle(error: float) -> float: - """Wrap error to [-180, 180] so turns never go the long way.""" - while error > 180: - error -= 360 - while error < -180: - error += 360 - return error - -async def gyro_turn(target_degrees: float, timeout_ms: int = 3000): - """ - Turn the robot to an absolute heading using a PID loop - driven by the hub IMU. - - Args: - target_degrees: absolute heading to reach (degrees) - timeout_ms: failsafe — stops if turn takes too long - """ - hub.imu.reset_heading(0) - current_heading = 0.0 - - integral = 0.0 - prev_error = 0.0 - settled_ms = 0 - - clock = StopWatch() - dt_watch = StopWatch() - clock.reset() - dt_watch.reset() - - while clock.time() < timeout_ms: - # ── 1. Read heading ───────────────────────────────── - current_heading = hub.imu.heading() - error = _shortest_angle(target_degrees - current_heading) - - # ── 2. Deadband check ─────────────────────────────── - if abs(error) < DEADBAND: - settled_ms += 10 - if settled_ms >= SETTLE_MS: - break # confirmed stable inside target - drive_base.drive(0, 0) - await wait(10) - continue - else: - settled_ms = 0 # reset settle counter on drift - - # ── 3. dt in seconds (for consistent Ki/Kd scaling) ─ - dt = dt_watch.time() / 1000.0 - dt_watch.reset() - if dt <= 0 or dt > 0.5: # skip bad first/stall tick - dt = 0.02 - - # ── 4. P term ─────────────────────────────────────── - P = PID_Kp * error - - # ── 5. I term with anti-windup clamp ──────────────── - integral += error * dt - integral = _clamp(integral, -INTEGRAL_CLAMP / PID_Ki, - INTEGRAL_CLAMP / PID_Ki) - I = PID_Ki * integral - - # ── 6. D term (derivative-on-measurement, not error) ─ - # Using (error - prev_error)/dt avoids "derivative kick" - # when the target suddenly changes. - D = PID_Kd * (error - prev_error) / dt - prev_error = error - - # ── 7. Sum and clamp total output ─────────────────── - output = _clamp(P + I + D, - -MAX_TURN_SPEED, - MAX_TURN_SPEED) - - # ── 8. Drive with turn-only command ───────────────── - drive_base.drive(0, output) - - await wait(10) # 100 Hz loop - - # Hard brake — holds position with motor hold - drive_base.stop() - await wait(50) # brief hold for gyro to settle - -# ───────────────────────────────────────────────────────────── -# Advanced PID Straight Controller -# ───────────────────────────────────────────────────────────── -# Tuning guide: -# STRAIGHT_Kp – main speed push, start at 2.0 -# STRAIGHT_Ki – fights distance undershoot, keep tiny -# STRAIGHT_Kd – smooths deceleration, start at 0.5 -# HEADING_Kp – how hard it steers to stay straight -# STRAIGHT_DEADBAND – mm of "close enough" before braking - -STRAIGHT_Kp = 2.0 -STRAIGHT_Ki = 0.02 -STRAIGHT_Kd = 0.5 -HEADING_Kp = 3.5 # steering correction gain -STRAIGHT_DEADBAND = 5 # mm -STRAIGHT_CLAMP = 20 # anti-windup cap -MAX_STRAIGHT_SPEED = 800 # mm/s cap - -async def gyro_straight(distance_mm: float, speed: int = 600, timeout_ms: int = 5000): - """ - Drive straight a precise distance using dual PID: - - Distance PID → controls forward speed - - Heading PID → corrects steering drift in real time - - Args: - distance_mm: how far to travel (negative = backward) - speed: max forward speed in mm/s - timeout_ms: failsafe cutoff - """ - # ── Reset everything ──────────────────────────────────── - hub.imu.reset_heading(0) - left_motor.reset_angle(0) - right_motor.reset_angle(0) - - target_heading = 0.0 - integral = 0.0 - prev_dist_error = 0.0 - settled_ms = 0 - - clock = StopWatch() - dt_watch = StopWatch() - clock.reset() - dt_watch.reset() - - # Convert distance to motor degrees - wheel_circumference = umath.pi * WHEEL_DIAMETER # mm per full rotation - target_deg = (distance_mm / wheel_circumference) * 360.0 - - while clock.time() < timeout_ms: - - # ── 1. Current position from motor encoders ────────── - avg_angle = (left_motor.angle() + right_motor.angle()) / 2.0 - dist_error = target_deg - avg_angle - - # ── 2. Deadband check ──────────────────────────────── - dist_error_mm = dist_error / 360.0 * wheel_circumference - if abs(dist_error_mm) < STRAIGHT_DEADBAND: - settled_ms += 10 - if settled_ms >= 60: - break - left_motor.hold() - right_motor.hold() - await wait(10) - continue - else: - settled_ms = 0 - - # ── 3. dt ──────────────────────────────────────────── - dt = dt_watch.time() / 1000.0 - dt_watch.reset() - if dt <= 0 or dt > 0.5: - dt = 0.02 - - # ── 4. Distance PID ────────────────────────────────── - P_dist = STRAIGHT_Kp * dist_error_mm - - integral += dist_error_mm * dt - integral = _clamp(integral, - -STRAIGHT_CLAMP / STRAIGHT_Ki, - STRAIGHT_CLAMP / STRAIGHT_Ki) - I_dist = STRAIGHT_Ki * integral - - D_dist = STRAIGHT_Kd * (dist_error_mm - prev_dist_error) / dt - prev_dist_error = dist_error_mm - - forward_speed = _clamp(P_dist + I_dist + D_dist, - -speed, speed) - - # ── 5. Heading PID (steering correction) ───────────── - heading_error = target_heading - hub.imu.heading() - heading_error = _shortest_angle(heading_error) - steering = HEADING_Kp * heading_error - - # ── 6. Mix forward + steering into each motor ──────── - # Positive steering correction speeds up left, slows right - left_speed = _clamp(forward_speed + steering, - -MAX_STRAIGHT_SPEED, MAX_STRAIGHT_SPEED) - right_speed = _clamp(forward_speed - steering, - -MAX_STRAIGHT_SPEED, MAX_STRAIGHT_SPEED) - - left_motor.run(left_speed) - right_motor.run(right_speed) - - await wait(10) # 100 Hz loop - - # ── Hard brake ─────────────────────────────────────────── - left_motor.hold() - right_motor.hold() - await wait(50) """ Run#1 @@ -293,75 +596,59 @@ Run#1 - What's on sale + Silo - Green Key """ -async def Run1(): - - # Fast approach to near-stall position - await left_arm.run_angle(2000, -210) # Fast movement downward - - # Gentle stall detection (shorter distance = faster) +async def Run1(): + await left_arm.run_angle(2000, -210) await left_arm.run_until_stalled(-1500, duty_limit=15) left_arm.reset_angle(0) print(f"Initial left arm angle : {left_arm.angle()}") await solve_whats_on_sale_v3() await solve_silo() - + # return to base await drive_base.straight(-90) - #await drive_base.turn(-100) - await drive_base.arc(200,None,-300) + await drive_base.arc(200, None, -300) drive_base.stop() async def solve_whats_on_sale_v3(): - - right_arm.run_angle(500,30) - - #Automated inconsistency - #left_arm.run_angle(500,-119.5) - await left_arm.run_angle(500, 75,Stop.HOLD) - #await left_arm.run_target(500,90,Stop.HOLD) + right_arm.run_angle(500, 30) + await left_arm.run_angle(500, 75, Stop.HOLD) print(f"Position left arm angle : {left_arm.angle()}") - - await drive_base.straight(190) + await drive_base.straight(190) await drive_base.turn(-40) await drive_base.straight(335) - await left_arm.run_angle(500,-20) + await left_arm.run_angle(500, -20) await drive_base.straight(-100) await drive_base.straight(60) - await left_arm.run_angle(500,50) + await left_arm.run_angle(500, 50) await drive_base.straight(-100) - left_arm.run_angle(500,-50) + left_arm.run_angle(500, -50) await drive_base.turn(-20) - left_arm.run_angle(1000,180) + left_arm.run_angle(1000, 180) await drive_base.turn(15) async def solve_whats_on_sale_v2(): - - right_arm.run_angle(500,30) - - # Bring down left arm to position + right_arm.run_angle(500, 30) await left_arm.run_angle(2000, -120) - #await left_arm.run_until_stalled(-500,duty_limit=15) print(f"Position left arm angle : {left_arm.angle()}") left_arm.reset_angle(0) - - await drive_base.straight(180) + await drive_base.straight(180) await drive_base.turn(-40) await drive_base.straight(335) - await left_arm.run_angle(500,-20) + await left_arm.run_angle(500, -20) await drive_base.straight(-100) await drive_base.straight(60) - await left_arm.run_angle(500,50) + await left_arm.run_angle(500, 50) await drive_base.straight(-100) - left_arm.run_angle(500,-50) + left_arm.run_angle(500, -50) await drive_base.turn(-20) - left_arm.run_angle(1000,180) + left_arm.run_angle(1000, 180) await drive_base.turn(15) async def solve_silo(): @@ -369,17 +656,14 @@ async def solve_silo(): await drive_base.turn(45) await drive_base.straight(120) - SPEED = 10000 # speed in degree per second - SWING_ANGLE = 60 # the angle! - REBOUND_ADJ = 20 + SPEED = 10000 + SWING_ANGLE = 60 - # Repeat this motion 4 times for _ in range(4): - await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up - await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down - + await right_arm.run_angle(SPEED, SWING_ANGLE, Stop.HOLD) + await right_arm.run_angle(SPEED, (-1 * SWING_ANGLE), Stop.HOLD) - right_arm.run_angle(4000,60, Stop.HOLD) + right_arm.run_angle(4000, 60, Stop.HOLD) """ @@ -387,7 +671,7 @@ Run#2 - This to solve forge, who lived here and heavy lifting combined - Red Key """ -async def Run2(): +async def Run2(): await solve_forge() await solve_heavy_lifting() await solve_who_lived_here() @@ -401,10 +685,9 @@ async def Run2(): drive_base.stop() async def solve_forge(): - left_arm.run_angle(100,90) - await right_arm.run_target(50,50) + left_arm.run_angle(100, 90) + await right_arm.run_target(50, 50) await wait(800) - # await right_arm.run_angle(50,-30) await drive_base.straight(50) await drive_base.turn(-17) await drive_base.straight(650) @@ -415,11 +698,11 @@ async def solve_forge(): await drive_base.straight(-60) async def solve_heavy_lifting(): - await right_arm.run_angle(2000,-160) # arm down + await right_arm.run_angle(2000, -160) await wait(100) - await drive_base.turn(30) # turn right a little bit - await right_arm.run_angle(2000,160) #arm up - await drive_base.turn(-30) #reset position + await drive_base.turn(30) + await right_arm.run_angle(2000, 160) + await drive_base.turn(-30) async def solve_who_lived_here(): await drive_base.straight(35) @@ -433,40 +716,36 @@ async def solve_who_lived_here(): async def solve_flag(): await drive_base.straight(85) - await left_arm.run_angle(70,-90) + await left_arm.run_angle(70, -90) await wait(500) - await left_arm.run_angle(100,120) + await left_arm.run_angle(100, 120) await drive_base.straight(-45) await drive_base.turn(-80) await drive_base.straight(-20) - await left_arm.run_angle(250,-90) - await left_arm.run_angle(250,90) + await left_arm.run_angle(250, -90) + await left_arm.run_angle(250, 90) """ Run#2.1 -- Alternate solution for Forge, Who lived here and Heavy Lifting combined - Light Blue Key -- Different alignment """ -async def Run2_1(): +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) + 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 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 @@ -475,8 +754,7 @@ Run#3 async def Run3(): await solve_tip_the_scale() await solve_angler_artifacts() - - #cross over to red side + await multitask( drive_forward(), monitor_distance() @@ -484,7 +762,7 @@ async def Run3(): async def solve_tip_the_scale(): drive_base.straight(20) - await drive_base.arc(-275,None,365) + await drive_base.arc(-275, None, 365) await drive_base.straight(280) await drive_base.straight(-80) await drive_base.turn(-50) @@ -496,18 +774,14 @@ async def solve_tip_the_scale(): async def solve_angler_artifacts(): await drive_base.straight(55) await drive_base.turn(-10) - await left_arm.run_angle(10000,-800) + await left_arm.run_angle(10000, -800) await drive_base.straight(-120) await drive_base.turn(110) await drive_base.turn(-25) - - - """ Run #4 -- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal - Blue Key """ async def Run4(): @@ -535,40 +809,44 @@ async def solve_mineshaft_explorer(): await drive_base.straight(84) await right_arm.run_angle(300, 90) + """ Run#5 - Solves Salvage Operation + Statue Rebuild - Orange Key +- *** USES PID DRIVE *** """ async def Run5(): # Getting the sand down - await gyro_straight(550) - await right_arm.run_angle(300,100) - await gyro_straight(-75) + await drive.pid_straight(550) + await right_arm.run_angle(300, 100) + await drive.pid_straight(-75) await right_arm.run_angle(300, -100) # Shoving the boat into place - await gyro_straight(300) - await gyro_straight(-200) - await gyro_turn(-15) + await drive.pid_straight(300) + await drive.pid_straight(-200) + await drive.pid_turn(-15) # Solving statue - await gyro_straight(350) - await gyro_turn(-104) - await gyro_straight(-80) + await drive.pid_straight(350) + await drive.pid_turn(-104) + await drive.pid_straight(-80) await left_arm.run_angle(500, -300) - await gyro_straight(120) - await gyro_turn(5) + await drive.pid_straight(120) + await drive.pid_turn(5) # Lift up statue await left_arm.run_angle(500, 100, Stop.HOLD) - await drive_base.turn(18) - await drive_base.straight(-100) - await drive_base.turn(-90) - await drive_base.straight(900) + await drive.pid_turn(18) + await drive.pid_straight(-100) + await drive.pid_turn(-90) + set_fast_mode() + await drive.pid_straight(900) + set_default_pid_mode() drive_base.stop() async def solve_salvage_operation(): await drive_base.straight(500) - await right_arm.run_angle(300,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) @@ -584,19 +862,17 @@ async def solve_statue_rebuild(): await left_arm.run_angle(500, -900) await drive_base.straight(50) await drive_base.straight(50) - await left_arm.run_angle(700,200) + await left_arm.run_angle(700, 200) 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 +async def Run6_7(): solve_site_mark_1() solve_site_mark_2() - #return to base await drive_base.straight(-300) drive_base.stop() @@ -612,76 +888,80 @@ async def solve_site_mark_2(): await wait(50) await right_arm.run_angle(50, 50) -async def Run10(): # experimental map reveal attachment - +async def Run10(): await drive_base.straight(600) drive_base.settings(150, 750, 50, 500) await drive_base.turn(-30) await drive_base.straight(260) - left_arm.run_angle(300,218) - + left_arm.run_angle(300, 218) + set_default_speed() await drive_base.straight(-80) await drive_base.turn(30) await drive_base.straight(-300) await drive_base.straight(400) - #await left_arm.run_angle(50,120) await drive_base.straight(-200) - await left_arm.run_angle(300,-215) + await left_arm.run_angle(300, -215) await drive_base.straight(-600) drive_base.stop() -async def Run11(): # experimental surface brushing attachment - - await gyro_straight(600) - drive_base.settings(150, 750, 50, 500) - await gyro_turn(-30) - await gyro_straight(250) - #left_arm.run_angle(300,218) - - set_default_speed() - await gyro_straight(-100) - await drive_base.turn(30) - #await drive_base.straight(400) - #await left_arm.run_angle(50,120) - #await drive_base.straight(-200) - await gyro_straight(-600) + +""" +Run#11 +- *** USES PID DRIVE *** +""" +async def Run11(): + await drive.pid_straight(600) + set_precise_mode() + await drive.pid_turn(-30) + await drive.pid_straight(250) + + set_default_pid_mode() + await drive.pid_straight(-100) + await drive.pid_turn(30) + await drive.pid_straight(-600) drive_base.stop() + +""" +Run#12 +- *** USES PID DRIVE *** +""" async def Run12(): - await gyro_straight(900) - await gyro_turn(83) + set_fast_mode() + await drive.pid_straight(900) + await drive.pid_turn(83) await left_arm.run_angle(3000, -300) await right_arm.run_angle(1100, -180) - drive_base.settings(150, 50, 50, 50) - await gyro_straight(120) + set_precise_mode() + await drive.pid_straight(120) left_arm.reset_angle(0) await left_arm.run_angle(50, 50) await right_arm.run_angle(50, 90) - await gyro_straight(-100) - drive_base.settings(950, 750, 750, 750) - await gyro_turn(110) - await gyro_straight(1000) + await drive.pid_straight(-100) + set_fast_mode() + await drive.pid_turn(110) + await drive.pid_straight(1000) + set_default_pid_mode() # Function to classify color based on HSV def detect_color(h, s, v, reflected): if reflected > 4: - if h < 4 or h > 350: # red + if h < 4 or h > 350: return "Red" - elif 3 < h < 40 and s > 70: # orange + elif 3 < h < 40 and s > 70: return "Orange" - elif 47 < h < 56: # yellow + elif 47 < h < 56: return "Yellow" - elif 70 < h < 160: # green - do it vertically not horizontally for accuracy + elif 70 < h < 160: return "Green" - elif 195 < h < 198: # light blue + elif 195 < h < 198: return "Light_Blue" - elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + elif 210 < h < 225: return "Blue" - elif 260 < h < 350: # purple + elif 260 < h < 350: return "Purple" - else: return "Unknown" return "Unknown" @@ -689,16 +969,12 @@ def detect_color(h, s, v, reflected): async def main(): while True: - pressed = hub.buttons.pressed() + pressed = hub.buttons.pressed() h, s, v = await color_sensor.hsv() reflected = await color_sensor.reflection() color = detect_color(h, s, v, reflected) - if DEBUG : - #print(color_sensor.color()) - #print(h,s,v) - #print(color) + if DEBUG: print(f"button pressed: {pressed}") - if color == "Green": print('Running Mission 1') @@ -717,19 +993,15 @@ async def main(): await Run5() elif color == "Purple": print('Running Mission 6') - await Run11() + await Run11() elif color == "Light_Blue": print("Running Mission 2_1") await Run12() else: print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") - #pass - # Show battery % for debugging - if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor - # Get the battery voltage in millivolts (mV) + if Button.BLUETOOTH in pressed: battery_voltage_mV = hub.battery.voltage() - # Use the function with your voltage reading percentage = await get_battery_percentage(float(battery_voltage_mV)) if DEBUG: print(f"Battery voltage: {battery_voltage_mV} mV") @@ -741,5 +1013,5 @@ async def main(): continue await wait(10) -# Run the main function + run_task(main()) \ No newline at end of file From 4102f776f67838f1ba9e98c6aea7a1ceba932712 Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Fri, 15 May 2026 22:13:00 +0000 Subject: [PATCH 74/75] Add competition_codes/AROC/Tisimal.py --- competition_codes/AROC/Tisimal.py | 655 ++++++++++++++++++++++++++++++ 1 file changed, 655 insertions(+) create mode 100644 competition_codes/AROC/Tisimal.py diff --git a/competition_codes/AROC/Tisimal.py b/competition_codes/AROC/Tisimal.py new file mode 100644 index 0000000..bb40dd2 --- /dev/null +++ b/competition_codes/AROC/Tisimal.py @@ -0,0 +1,655 @@ +#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge. +import umath +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, [[12,36]],[[12,20,24]] ) # 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) + +""" +Debugging helps +""" +DEBUG = 1 # Enable when you want to show logs +# Example conversion function (adjust min/max values as needed for your hub) +async def get_battery_percentage(voltage_mV:float): + max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us + min_voltage = 5000.0 # min battery level + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100 + return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100 + +async def wait_button_release(): + """Wait for all buttons to be released""" + while hub.buttons.pressed(): + await wait(500) + await wait(1000) # Debounce delay + +WALL_DISTANCE = 300 # mm +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(1000,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) + +# Use this to set default +def set_default_speed(): + drive_base.settings(600, 500, 300, 200) + +# Use this to change drive base movement +def set_speed(straight_speed, st_acc, turn_speed, turn_acc): + drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc) + +# PID Controller Module +class PIDController: + def __init__(self, kp, ki, kd): + self.kp = kp # Proportional gain + self.ki = ki # Integral gain + self.kd = kd # Derivative gain + self.previous_error = 0 + self.integral = 0 + + def calculate(self, desired_value, actual_value, dt): + """ + Calculate PID output + + Args: + desired_value: Target value + actual_value: Current measured value + dt: Time delta in seconds + + Returns: + PID output value + """ + error = desired_value - actual_value + + # Proportional term + proportional = error * self.kp + + # Integral term + self.integral += error * dt + integral = self.integral * self.ki + + # Derivative term + derivative = (error - self.previous_error) / dt * self.kd if dt > 0 else 0 + + # Store current error for next iteration + self.previous_error = error + + # Calculate total output + output = proportional + integral + derivative + + return output + + def reset(self): + """Reset the PID controller""" + self.previous_error = 0 + self.integral = 0 + +# Create PID controllers for Missions 5, 11, and 12 +straight_pid = PIDController(kp=1.2, ki=0.0, kd=0.1) +turn_pid = PIDController(kp=2.0, ki=0.0, kd=0.2) + +""" +Run#1 +- Removed forge and who lived here part +- What's on sale + Silo +- Green Key +""" +async def Run1(): + + # Fast approach to near-stall position + await left_arm.run_angle(2000, -210) # Fast movement downward + + # Gentle stall detection (shorter distance = faster) + await left_arm.run_until_stalled(-1500, duty_limit=15) + left_arm.reset_angle(0) + print(f"Initial left arm angle : {left_arm.angle()}") + + await solve_whats_on_sale_v3() + 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_v3(): + + right_arm.run_angle(500,30) + + #Automated inconsistency + #left_arm.run_angle(500,-119.5) + await left_arm.run_angle(500, 75,Stop.HOLD) + #await left_arm.run_target(500,90,Stop.HOLD) + print(f"Position left arm angle : {left_arm.angle()}") + + await drive_base.straight(190) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_whats_on_sale_v2(): + + right_arm.run_angle(500,30) + + # Bring down left arm to position + await left_arm.run_angle(2000, -120) + #await left_arm.run_until_stalled(-500,duty_limit=15) + print(f"Position left arm angle : {left_arm.angle()}") + left_arm.reset_angle(0) + + await drive_base.straight(180) + + await drive_base.turn(-40) + await drive_base.straight(335) + await left_arm.run_angle(500,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + await left_arm.run_angle(500,50) + + await drive_base.straight(-100) + left_arm.run_angle(500,-50) + await drive_base.turn(-20) + left_arm.run_angle(1000,180) + await drive_base.turn(15) + +async def solve_silo(): + await drive_base.straight(-80) + await drive_base.turn(45) + await drive_base.straight(120) + + SPEED = 10000 # speed in degree per second + SWING_ANGLE = 60 # the angle! + REBOUND_ADJ = 20 + + # Repeat this motion 4 times + for _ in range(4): + await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up + await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down + + + right_arm.run_angle(4000,60, 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() + await solve_flag() + + # return to base + await drive_base.turn(55) + await drive_base.straight(-190) + await drive_base.turn(30) + await drive_base.straight(-720) + drive_base.stop() + +async def solve_forge(): + left_arm.run_angle(100,90) + await right_arm.run_target(50,50) + await wait(800) + # await right_arm.run_angle(50,-30) + await drive_base.straight(50) + await drive_base.turn(-17) + await drive_base.straight(650) + await drive_base.turn(50) + + await drive_base.straight(90) + await drive_base.turn(-70) + await drive_base.straight(-60) + +async def solve_heavy_lifting(): + await right_arm.run_angle(2000,-160) # arm down + await wait(100) + await drive_base.turn(30) # turn right a little bit + await right_arm.run_angle(2000,160) #arm up + await drive_base.turn(-30) #reset position + +async def solve_who_lived_here(): + await drive_base.straight(35) + await drive_base.turn(-20) + await drive_base.straight(50) + await drive_base.turn(-25) + await drive_base.straight(-100) + await drive_base.turn(-5) + await drive_base.straight(300) + await drive_base.turn(60) + +async def solve_flag(): + await drive_base.straight(85) + await left_arm.run_angle(70,-90) + await wait(500) + await left_arm.run_angle(100,120) + await drive_base.straight(-45) + await drive_base.turn(-80) + await drive_base.straight(-20) + await left_arm.run_angle(250,-90) + await left_arm.run_angle(250,90) + + +""" +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_tip_the_scale() + await solve_angler_artifacts() + + #cross over to red side + await multitask( + drive_forward(), + monitor_distance() + ) + +async def solve_tip_the_scale(): + drive_base.straight(20) + await drive_base.arc(-275,None,365) + await drive_base.straight(280) + await drive_base.straight(-80) + await drive_base.turn(-50) + await drive_base.straight(80) + await drive_base.turn(40) + await drive_base.straight(295) + await drive_base.turn(-90) + +async def solve_angler_artifacts(): + await drive_base.straight(55) + await drive_base.turn(-10) + await left_arm.run_angle(10000,-800) + await drive_base.straight(-120) + await drive_base.turn(110) + await drive_base.turn(-25) + + + + + +""" +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(): + # Reset PID controller for this mission + straight_pid.reset() + + # Getting the sand down with PID control + await drive_straight_pid(550) + await right_arm.run_angle(300,100) + await drive_straight_pid(-75) + await right_arm.run_angle(300, -100) + + # Shoving the boat into place + await drive_straight_pid(300) + await drive_straight_pid(-200) + await drive_turn_pid(-15) + + # Solving statue + await drive_straight_pid(350) + await drive_turn_pid(-104) + await drive_straight_pid(-80) + await left_arm.run_angle(500, -300) + await drive_straight_pid(120) + await drive_turn_pid(5) + + # Lift up statue + await left_arm.run_angle(500, 100, Stop.HOLD) + await drive_turn_pid(18) + await drive_straight_pid(-100) + await drive_turn_pid(-90) + await drive_straight_pid(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,200) + await drive_base.turn(30) + +# PID-enhanced drive functions for Mission 5 +async def drive_straight_pid(distance_mm): + """Drive straight using PID control""" + start_distance = drive_base.distance() + target_distance = start_distance + distance_mm + straight_pid.reset() + + stopwatch = StopWatch() + stopwatch.reset() + + # Drive with PID correction + while abs(drive_base.distance() - target_distance) > 5: # 5mm tolerance + elapsed_time = stopwatch.time() / 1000.0 # Convert to seconds + stopwatch.reset() + + # Calculate PID correction + correction = straight_pid.calculate(target_distance, drive_base.distance(), elapsed_time) + + # Apply correction (limit to reasonable values) + correction = max(min(correction, 100), -100) + + # Drive with correction + drive_base.drive(200, correction) # Base speed 200 mm/s + await wait(10) + + drive_base.stop() + +async def drive_turn_pid(angle_degrees): + """Turn using PID control""" + start_angle = drive_base.angle() + target_angle = start_angle + angle_degrees + turn_pid.reset() + + stopwatch = StopWatch() + stopwatch.reset() + + # Turn with PID correction + while abs(drive_base.angle() - target_angle) > 2: # 2 degree tolerance + elapsed_time = stopwatch.time() / 1000.0 # Convert to seconds + stopwatch.reset() + + # Calculate PID correction + correction = turn_pid.calculate(target_angle, drive_base.angle(), elapsed_time) + + # Apply correction (limit to reasonable values) + correction = max(min(correction, 100), -100) + + # Turn with correction + drive_base.drive(0, correction) # No forward movement, just turning + await wait(10) + + drive_base.stop() + +""" +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) + +async def Run10(): # experimental map reveal attachment + + await drive_base.straight(600) + drive_base.settings(150, 750, 50, 500) + await drive_base.turn(-30) + await drive_base.straight(260) + left_arm.run_angle(300,218) + + set_default_speed() + await drive_base.straight(-80) + await drive_base.turn(30) + await drive_base.straight(-300) + await drive_base.straight(400) + #await left_arm.run_angle(50,120) + await drive_base.straight(-200) + await left_arm.run_angle(300,-215) + await drive_base.straight(-600) + drive_base.stop() + +async def Run11(): # experimental surface brushing attachment with PID + # Reset PID controllers + straight_pid.reset() + turn_pid.reset() + + # Drive forward with PID + await drive_straight_pid(600) + + # Turn with PID + await drive_turn_pid(-30) + + # Drive forward with PID + await drive_straight_pid(250) + + # Return with PID control + await drive_straight_pid(-100) + await drive_turn_pid(30) + await drive_straight_pid(-600) + drive_base.stop() + +async def Run12(): + # Reset PID controllers for this mission + straight_pid.reset() + turn_pid.reset() + + # Drive forward with PID + await drive_straight_pid(900) + + # Turn with PID + await drive_turn_pid(83) + + await left_arm.run_angle(3000, -300) + await right_arm.run_angle(1100, -180) + drive_base.settings(150, 50, 50, 50) + await drive_straight_pid(120) + left_arm.reset_angle(0) + await left_arm.run_angle(50, 50) + await right_arm.run_angle(50, 90) + await drive_straight_pid(-100) + drive_base.settings(950, 750, 750, 750) + + # Turn with PID + await drive_turn_pid(110) + + # Drive forward with PID + await drive_straight_pid(1000) + +# 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: + pressed = hub.buttons.pressed() + h, s, v = await color_sensor.hsv() + reflected = await color_sensor.reflection() + color = detect_color(h, s, v, reflected) + if DEBUG : + #print(color_sensor.color()) + #print(h,s,v) + #print(color) + print(f"button pressed: {pressed}") + + + 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 Run11() + elif color == "Light_Blue": + print("Running Mission 2_1") + await Run12() + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + #pass + + # Show battery % for debugging + if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = hub.battery.voltage() + # Use the function with your voltage reading + percentage = await get_battery_percentage(float(battery_voltage_mV)) + if DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await wait(10) +# Run the main function +run_task(main()) \ No newline at end of file From 9771118d3906482277c9cd33d1c9f453eb3155ca Mon Sep 17 00:00:00 2001 From: Johannes <31liwaj@elmbrookstudents.org> Date: Fri, 15 May 2026 23:09:21 +0000 Subject: [PATCH 75/75] Update competition_codes/AROC/Nationals_Main.py --- competition_codes/AROC/Nationals_Main.py | 41 ++++++++++++------------ 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/competition_codes/AROC/Nationals_Main.py b/competition_codes/AROC/Nationals_Main.py index 7e7cad1..000d55b 100644 --- a/competition_codes/AROC/Nationals_Main.py +++ b/competition_codes/AROC/Nationals_Main.py @@ -20,7 +20,7 @@ color_sensor = ColorSensor(Port.F) 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.settings(1000, 600, 500, 300) drive_base.use_gyro(True) """ @@ -110,20 +110,21 @@ async def solve_whats_on_sale_v3(): #await left_arm.run_target(500,90,Stop.HOLD) print(f"Position left arm angle : {left_arm.angle()}") - await drive_base.straight(190) + await drive_base.straight(210) await drive_base.turn(-40) - await drive_base.straight(335) - await left_arm.run_angle(500,-20) + await drive_base.straight(320) + await left_arm.run_angle(490,-30) await drive_base.straight(-100) await drive_base.straight(60) - await left_arm.run_angle(500,50) + await left_arm.run_angle(600,50) await drive_base.straight(-100) left_arm.run_angle(500,-50) - await drive_base.turn(-20) - left_arm.run_angle(1000,180) + await drive_base.turn(5) + await drive_base.turn(-25) + left_arm.run_angle(5000,180) await drive_base.turn(15) async def solve_whats_on_sale_v2(): @@ -136,7 +137,7 @@ async def solve_whats_on_sale_v2(): print(f"Position left arm angle : {left_arm.angle()}") left_arm.reset_angle(0) - await drive_base.straight(180) + await drive_base.straight(175) await drive_base.turn(-40) await drive_base.straight(335) @@ -146,22 +147,22 @@ async def solve_whats_on_sale_v2(): await drive_base.straight(60) await left_arm.run_angle(500,50) - await drive_base.straight(-100) + await drive_base.straight(-90) left_arm.run_angle(500,-50) + left_arm.run_angle(1000,6) await drive_base.turn(-20) - left_arm.run_angle(1000,180) await drive_base.turn(15) async def solve_silo(): - await drive_base.straight(-80) + await drive_base.straight(-75) await drive_base.turn(45) - await drive_base.straight(120) + await drive_base.straight(110) SPEED = 10000 # speed in degree per second SWING_ANGLE = 60 # the angle! REBOUND_ADJ = 20 - # Repeat this motion 4 times + # Repeat this motion 3 times for _ in range(4): await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down @@ -182,8 +183,8 @@ async def Run2(): await solve_flag() # return to base - await drive_base.turn(55) - await drive_base.straight(-190) + await drive_base.turn(50) + await drive_base.straight(-220) await drive_base.turn(30) await drive_base.straight(-720) drive_base.stop() @@ -198,7 +199,7 @@ async def solve_forge(): await drive_base.straight(650) await drive_base.turn(50) - await drive_base.straight(90) + await drive_base.straight(95) await drive_base.turn(-70) await drive_base.straight(-60) @@ -213,9 +214,9 @@ async def solve_who_lived_here(): await drive_base.straight(35) await drive_base.turn(-20) await drive_base.straight(50) - await drive_base.turn(-25) + await drive_base.turn(-23 ) await drive_base.straight(-100) - await drive_base.turn(-5) + await drive_base.turn(-3) await drive_base.straight(300) await drive_base.turn(60) @@ -272,7 +273,7 @@ async def Run3(): async def solve_tip_the_scale(): drive_base.straight(20) - await drive_base.arc(-275,None,365) + await drive_base.arc(-270,None,365) await drive_base.straight(280) await drive_base.straight(-80) await drive_base.turn(-50) @@ -285,7 +286,7 @@ async def solve_angler_artifacts(): await drive_base.straight(55) await drive_base.turn(-10) await left_arm.run_angle(10000,-800) - await drive_base.straight(-120) + await drive_base.straight(-130) await drive_base.turn(110) await drive_base.turn(-25)