From 28b3f2b0376bb0d9ea613862da177fcd32978b1e Mon Sep 17 00:00:00 2001 From: Rishikesh Lolapu <31lolapr@elmbrookstudents.org> Date: Wed, 20 May 2026 23:04:14 +0000 Subject: [PATCH] Update competition_codes/AROC/Nationals_Main.py --- competition_codes/AROC/Nationals_Main.py | 198 +++++++++++++++++------ 1 file changed, 147 insertions(+), 51 deletions(-) diff --git a/competition_codes/AROC/Nationals_Main.py b/competition_codes/AROC/Nationals_Main.py index 000d55b..63508e5 100644 --- a/competition_codes/AROC/Nationals_Main.py +++ b/competition_codes/AROC/Nationals_Main.py @@ -1,3 +1,117 @@ +""" +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 @@ -20,7 +134,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(1000, 600, 500, 300) +drive_base.settings(600, 500, 300, 200) drive_base.use_gyro(True) """ @@ -324,36 +438,26 @@ 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 -""" +# Solves Salvage Operation + Statue Rebuild andOrange Key async def Run5(): - # Getting the sand down - await drive_base.straight(550) + # Getting the sand down with PID control for precision + await pid_straight(550, 400) # Forward 550mm await right_arm.run_angle(300,100) - await drive_base.straight(-75) + await pid_straight(-75, 300) # Backward 75mm 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) + await pid_straight(300, 400) # Forward 300mm + await pid_straight(-200, 300) # Backward 200mm + await drive_base.turn(-15) # Keep regular turn for small angles + # Solving statue - await drive_base.straight(350) - await drive_base.turn(-104) - await drive_base.straight(-80) + await pid_straight(350, 400) # Forward 350mm + await drive_base.turn(-104) # Regular turn + await pid_straight(-80, 300) # Backward 80mm 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() - + await pid_straight(120, 300) # Forward 120mm + await drive_base.turn(5) async def solve_salvage_operation(): await drive_base.straight(500) @@ -420,38 +524,30 @@ async def Run10(): # experimental map reveal attachment 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) +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(-200, 300) # Backward 100mm with PID + await drive_base.turn(35) # Regular turn + await pid_straight(-665, 500) # Fast backward 600mm with PID drive_base.stop() async def Run12(): - await drive_base.straight(900) - await drive_base.turn(83) + # Using PID straight drive for precision on long runs + await pid_straight(900) # Forward 900mm with PID + await drive_base.turn(89.9) # Regular turn 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) + await pid_straight(120, 200) # Short precise forward with PID 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) - + await pid_straight(-120, 200) # Short precise backward with PID + await drive_base.turn(90) # 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): @@ -505,10 +601,10 @@ async def main(): print('Running Mission 5') await Run5() elif color == "Purple": - print('Running Mission 6') + print('Running Mission 11') await Run11() elif color == "Light_Blue": - print("Running Mission 2_1") + print("Running Mission 12") await Run12() else: print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")