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] 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