""" 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 = 62.4 # mm (adjust for your wheels) AXLE_TRACK = 150 # 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) async def multilift_up(rotation_angle): await multitask( right_arm.run_angle(400,rotation_angle), left_arm.run_angle(400,-1*rotation_angle) ) async def multilift_down(rotation_angle): await multitask( right_arm.run_angle(1000,-1*rotation_angle), left_arm.run_angle(1000,rotation_angle) ) async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Johannes await set_speed(600,200,300,200) await drive_base.straight(500) await multitask( drive_base.straight(-200), right_arm.run_angle(400,-150) ) await set_speed(600,500,100,200) await drive_base.turn(45) await drive_base.straight(346) await drive_base.turn(-68) await multitask( left_arm.run_angle(600,240), set_speed(300,200,300,200), drive_base.straight(280) ) await left_arm.run_angle(600,-100) await drive_base.turn(22) await drive_base.straight(-100) await right_arm.run_angle(5000,45) await set_speed(600,500,100,200) await drive_base.straight(200) await drive_base.arc(-1500,None,-1000) async def Run2(): # Exploding Seeds - Johannes await multitask( right_arm.run_angle(400,-100), left_arm.run_angle(300,100), drive_base.arc(150,None,115) ) await drive_base.straight(270) await left_arm.run_angle(300,-100) await drive_base.straight(-100) await left_arm.run_angle(300,-100) await drive_base.arc(-1000,None,-800) async def Run2_1(): # Drone Survey, Experimental - Johannes await multitask( right_arm.run_angle(400,100), set_speed(600,200,300,200), drive_base.straight(300) ) set_speed(600,500,300,200) await drive_base.straight(470) await drive_base.turn(-15) await drive_base.straight(55) await drive_base.straight(-280) await drive_base.turn(15) await right_arm.run_angle(400,-100) await drive_base.straight(40) await drive_base.turn(-15) await drive_base.straight(50) await drive_base.turn(-25) async def Run3(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimental - Johannes await drive_base.straight(110) await drive_base.turn(-45) await drive_base.straight(240) await right_arm.run_angle(300,120) await drive_base.straight(290) await drive_base.turn(50) await drive_base.straight(300) await drive_base.turn(40) set_speed(100,200,300,200) await drive_base.straight(130) await drive_base.straight(-115) await drive_base.turn(87) await drive_base.straight(230) set_speed(600, 500, 300, 200) await drive_base.straight(-135) await multitask( drive_base.turn(-40), right_arm.run_angle(100,-110) ) set_speed(100,200,300,200) await drive_base.straight(220) await right_arm.run_angle(300,85) await wait(500) await drive_base.straight(142) await drive_base.arc(110,None,-166) await drive_base.straight(140) await drive_base.straight(-150) set_speed(600,500,300,200) await drive_base.straight(150) await drive_base.arc(800,None,-900) async def Run4(): #Flip the Rock, Lucky Leaves, Reaching Roots - Rishabh #Run await drive_base.straight(550)#Works (600(Previous)) await drive_base.straight(-200) await drive_base.turn(45) await drive_base.straight(575) await drive_base.turn(-90)#-90 for Coach Cisco table await left_arm.run_angle(200,270) await drive_base.straight(300) await left_arm.run_angle(200,-265) await right_arm.run_angle(200, -200) await drive_base.straight(-105)#Use -105 for Coach Cisco table(-105 for school) await drive_base.turn(32)#was 24-Use 24 for Coach Cisco's table(32 for school) await right_arm.run_angle(1500, 180) await drive_base.turn(30) await drive_base.straight(-1000) async def Run5(): async def Run6_7(): async def Run10(): async def Run11(): # Interchangable Mission - Parthiv await left_arm.run_angle(200,-80) await right_arm.run_angle(200,-70) await drive_base.arc(-400,angle=90) await drive_base.turn(55) await drive_base.straight(240) await drive_base.turn(-40) await drive_base.straight(85) await drive_base.straight(-32) await run_both_arms() await drive_base.straight(-53) await right_arm.run_angle(200,100) await drive_base.turn(30) await drive_base.straight(-775) await drive_base.stop async def Run12(): # Experimental Research Platform Lifter await multilift_up(1200) await multilift_down(300) await drive_base.straight(50) # Function to classify color based on HSV def detect_color(h, s, v, reflected): if reflected > 4: if h < 4 or h > 350: # red return "Red" elif 3 < h < 40 and s > 70: # orange return "Orange" elif 47 < h < 56: # yellow return "Yellow" elif 70 < h < 160: # green - do it vertically not horizontally for accuracy return "Green" elif 195 < h < 198: # light blue return "Light_Blue" elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy return "Blue" elif 260 < h < 350: # purple return "Purple" else: return "Unknown" return "Unknown" async def main(): while True: 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 Run3() 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())