FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed
Template
10
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 8 Wiki Activity

The-South_Awakens

Browse Source 
Has an advanced PID module, already implemented in RED Side.
This commit is contained in:
Rishikesh Lolapu
2026-05-09 01:08:23 +00:00
parent 0abada1c4b
commit 63bae21b88
1 changed files with 745 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

745
competition_codes/AROC/The_South-Awakens.py Normal file
View File

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