""" 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())