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 01/10] 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 -- 2.49.1 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 02/10] 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 -- 2.49.1 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 03/10] 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 -- 2.49.1 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 04/10] 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 -- 2.49.1 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 05/10] 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) -- 2.49.1 From 16bc6f0747d042285f39c32d73ff307c4ea60dc3 Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Tue, 2 Jun 2026 23:41:08 +0000 Subject: [PATCH 06/10] Add competition_codes/NumberOne.py --- competition_codes/NumberOne.py | 652 +++++++++++++++++++++++++++++++++ 1 file changed, 652 insertions(+) create mode 100644 competition_codes/NumberOne.py diff --git a/competition_codes/NumberOne.py b/competition_codes/NumberOne.py new file mode 100644 index 0000000..9528dad --- /dev/null +++ b/competition_codes/NumberOne.py @@ -0,0 +1,652 @@ +""" +PID Straight Drive — heading-corrected forward / backward movement. + +Improvements over baseline: + • Derivative on measurement (not error) — no derivative kick on setpoint change + • Fixed-interval dt — loop sleeps a constant 20 ms, so /dt is dropped from the + derivative term (dividing by 0.02 was silently multiplying KD by 50) + • prev_error seeded from first real heading — eliminates first-tick spike + • Per-motor speed clamping — prevents one side pivoting at low speeds + • Deceleration ramp — smooth stop in the last RAMP_MM millimetres + • Dual-motor exit + timeout — survives stall on either wheel + • Integral freeze near target — stops windup right where it matters most +""" + +from pybricks.tools import StopWatch, wait +import umath + +WHEEL_DIAMETER = 68.8 # mm — adjust to your wheel (Done) +AXLE_TRACK = 180 # mm — used only if you add turning later (Done) +LOOP_MS = 20 # fixed control loop period +RAMP_MM = 50 # begin decelerating this far from target +MIN_SPEED = 80 # floor speed during ramp (avoid stall) +TIMEOUT_MS = 8_000 # safety abort +# Tuning sequence + +KP = 4.8 # Wide chassis = stable platform, can afford higher P gain + # without oscillation. Pushes harder through corrections. + +KI = 0.012 # Slightly lower than default — large wheels cover distance + # fast so the integrator has less time to build up per run. + # Raise to 0.012 if you see consistent sideways drift at end. + +KD = 7.5 # Higher than default. Big wheels = more rotational momentum, + # needs stronger damping to prevent overshoot on corrections. + +def _mm_to_deg(mm: float) -> float: + return (abs(mm) / (umath.pi * WHEEL_DIAMETER)) * 360.0 + + +def _motor_avg_angle(m1, m2) -> float: + return (abs(m1.angle()) + abs(m2.angle())) / 2.0 + + +async def pid_straight(distance_mm: float, speed: int = 600): + """ + Drive straight for distance_mm at speed (deg/s). + Positive = forward, negative = reverse. + """ + direction = 1 if distance_mm >= 0 else -1 + degrees_need = _mm_to_deg(distance_mm) + target_hdg = hub.imu.heading() + + left_motor.reset_angle(0) + right_motor.reset_angle(0) + + # ── seed integral and prev_error from the real opening heading ── + first_error = target_hdg - hub.imu.heading() # 0.0 on a perfect start, + integral = 0.0 # non-zero if robot is tilted + prev_heading = hub.imu.heading() # for derivative-on-measurement + + watchdog = StopWatch() + + while True: + traveled = _motor_avg_angle(left_motor, right_motor) + + # ── exit: distance reached or timeout ── + if traveled >= degrees_need: + break + if watchdog.time() > TIMEOUT_MS: + break + + # ── heading error with wrap ── + heading = hub.imu.heading() + error = target_hdg - heading + if error > 180: error -= 360 + if error < -180: error += 360 + + # ── derivative on measurement — no kick when target changes ── + d_heading = heading - prev_heading # raw delta (deg / loop) + if d_heading > 180: d_heading -= 360 + if d_heading < -180: d_heading += 360 + prev_heading = heading + + # ── integral with freeze near target ── + near_target = (degrees_need - traveled) < _mm_to_deg(RAMP_MM / 2) + if not near_target: + integral = max(-50.0, min(50.0, integral + error)) # dt=const → no *dt + + # ── PID output ── + # derivative term: -KD * d_heading (measurement, not error difference) + correction = KP * error + KI * integral - KD * d_heading + correction = max(-200.0, min(200.0, correction)) + + # ── ramp speed near end ── + remaining = degrees_need - traveled + ramp_degrees = _mm_to_deg(RAMP_MM) + if remaining < ramp_degrees and ramp_degrees > 0: + ramp_factor = max(float(MIN_SPEED) / speed, + remaining / ramp_degrees) + run_speed = int(speed * ramp_factor) + else: + run_speed = speed + + # ── per-motor clamping — prevents pivot at low speeds ── + l_cmd = max(-speed, min(speed, direction * run_speed + int(correction))) + r_cmd = max(-speed, min(speed, direction * run_speed - int(correction))) + + left_motor.run(l_cmd) + right_motor.run(r_cmd) + await wait(LOOP_MS) + + left_motor.brake() + right_motor.brake() + +#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(stop_mm: int = WALL_DISTANCE): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...', distance) + + if distance is None: + await wait(50) + continue + if distance < stop_mm: + drive_base.stop() + print(f"Wall detected at {distance}mm!") + break + + # 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(-400) + 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(210) + + await drive_base.turn(-40) + await drive_base.straight(320) + await left_arm.run_angle(490,-20) + + await drive_base.straight(-100) + await drive_base.straight(60) + 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(5000,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(-75) + await drive_base.turn(45) + await drive_base.straight(110) + + SPEED = 20000 # 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(50) + await drive_base.straight(-210) + 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(95) + 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(-23) + await drive_base.straight(-100) + await drive_base.turn(-20) + await drive_base.straight(50) + await drive_base.turn(18) + await drive_base.straight(250) + await drive_base.turn(55) + +async def solve_flag(): + await drive_base.straight(100) + await left_arm.run_angle(70,-90) + await wait(500) + await left_arm.run_angle(100,120) + await drive_base.straight(-60) + 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(-270,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(290) + 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(-140) + 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 +- Uses PID straight drive for precision +""" +async def Run5(): + # Getting the sand down + await drive_base.straight(550) + await right_arm.run_angle(300,120) + 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(-17) + # 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 with PID straight + # Using PID straight drive for better precision on long runs + await pid_straight(665, 600) # Forward 600mm with PID + await drive_base.turn(-35) # Regular turn for small angles + await pid_straight(200, 200) # Forward 250mm with PID + await pid_straight(-250, 300) # Backward 100mm with PID + await drive_base.turn(45) # Regular turn + await pid_straight(-705,700) # Fast backward 600mm with PID + drive_base.stop() + +KP = 4 # Wide chassis = stable platform, can afford higher P gain + # without oscillation. Pushes harder through corrections. + +KI = 0.012 # Slightly lower than default — large wheels cover distance + # fast so the integrator has less time to build up per run. + # Raise to 0.012 if you see consistent sideways drift at end. + +KD = 7.5 # Higher than default. Big wheels = more rotational momentum, + # needs stronger damping to prevent overshoot on corrections. + +async def Run12(): + await pid_straight(920) + + + # Using PID straight drive for precision on long run # Forward 900mm with await drive_base.turn(84) # Regular tuawait left_arm.run_angle(3250, -300) + await drive_base.turn(86) + await left_arm.run_angle(1800, -300) + await right_arm.run_angle(1000, -180) + await pid_straight(120, 100) # Short precise forward with PID + left_arm.reset_angle(0) + await drive_base.turn(3) + await left_arm.run_angle(65, 56) + await right_arm.run_angle(100, 120) + await pid_straight(-170, 200) # Short precise backward with PID + await drive_base.turn(100) # Regular turn + await pid_straight(900) # Long return to base with PID + drive_base.stop() + +# 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 11') + await Run11() + elif color == "Light_Blue": + print("Running Mission 12") + 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 -- 2.49.1 From 74fd7135b0720f5de1bdcfd679bdb4a8c993c4e2 Mon Sep 17 00:00:00 2001 From: Arcmyx Official Date: Tue, 2 Jun 2026 23:50:02 +0000 Subject: [PATCH 07/10] Update competition_codes/AROC/Nationals_old.py --- competition_codes/AROC/{Nationals_Main.py => Nationals_old.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename competition_codes/AROC/{Nationals_Main.py => Nationals_old.py} (100%) diff --git a/competition_codes/AROC/Nationals_Main.py b/competition_codes/AROC/Nationals_old.py similarity index 100% rename from competition_codes/AROC/Nationals_Main.py rename to competition_codes/AROC/Nationals_old.py -- 2.49.1 From f8f6d100b4002f41667d4e6265dc93d881d25298 Mon Sep 17 00:00:00 2001 From: Arcmyx Official Date: Tue, 2 Jun 2026 23:50:11 +0000 Subject: [PATCH 08/10] Update competition_codes/AROC/Nationals_old2.py --- .../AROC/{The_South-Awakens.py => Nationals_old2.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename competition_codes/AROC/{The_South-Awakens.py => Nationals_old2.py} (100%) diff --git a/competition_codes/AROC/The_South-Awakens.py b/competition_codes/AROC/Nationals_old2.py similarity index 100% rename from competition_codes/AROC/The_South-Awakens.py rename to competition_codes/AROC/Nationals_old2.py -- 2.49.1 From 3daf7a6a37555c3d0ac85025d2eb6ffc85622567 Mon Sep 17 00:00:00 2001 From: Arcmyx Official Date: Tue, 2 Jun 2026 23:50:19 +0000 Subject: [PATCH 09/10] Update competition_codes/AROC/Nationals_old3.py --- competition_codes/AROC/{Tisimal.py => Nationals_old3.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename competition_codes/AROC/{Tisimal.py => Nationals_old3.py} (100%) diff --git a/competition_codes/AROC/Tisimal.py b/competition_codes/AROC/Nationals_old3.py similarity index 100% rename from competition_codes/AROC/Tisimal.py rename to competition_codes/AROC/Nationals_old3.py -- 2.49.1 From ea2ae1795952b478e05c1638d957a1eb926250d2 Mon Sep 17 00:00:00 2001 From: Arcmyx Official Date: Tue, 2 Jun 2026 23:50:27 +0000 Subject: [PATCH 10/10] Update competition_codes/AROC/NumberOne.py --- competition_codes/{ => AROC}/NumberOne.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename competition_codes/{ => AROC}/NumberOne.py (100%) diff --git a/competition_codes/NumberOne.py b/competition_codes/AROC/NumberOne.py similarity index 100% rename from competition_codes/NumberOne.py rename to competition_codes/AROC/NumberOne.py -- 2.49.1