FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed
Template
10
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 8 Wiki Activity
Files
79158d0f15af2766ddf1e0dcf6877c2ca129c93e
solutions_season_unearthed/competition_codes/state/Updated state.py

513 lines
15 KiB
Python
Raw Blame History

#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 = 200 # 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)
"""
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, 180) # Fast movement upward
# 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_v2()
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():
right_arm.run_angle(500,30)
#Automated inconsistency
#left_arm.run_angle(500,-119.5)
left_arm.run_target(500,-121.5, Stop.HOLD)
print(f"Position left arm angle : {left_arm.angle()}")
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_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(42)
await drive_base.straight(95)
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()
# return to base
await drive_base.arc(-500,None,600)
drive_base.stop()
async def solve_forge():
await right_arm.run_target(50,50)
# await right_arm.run_angle(50,-30)
await drive_base.arc(350,113, None)
await drive_base.straight(20)
await drive_base.turn(-60)
await drive_base.straight(-47)
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(50)
await drive_base.turn(-15)
await drive_base.straight(50)
await drive_base.turn(-25)
await drive_base.straight(-50)
await drive_base.turn(-40)
await drive_base.straight(50)
right_arm.run_angle(1000,-160)
await drive_base.turn(-60)
await right_arm.run_angle(2000,160)
"""
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_angler_artifacts()
await solve_tip_the_scale()
#cross over to red side
await multitask(
drive_forward(),
monitor_distance()
)
async def solve_angler_artifacts():
await drive_base.straight(870)
await drive_base.turn(-90,Stop.HOLD)
await drive_base.straight(45)
#Solve
drive_base.turn(-10)
await left_arm.run_angle(10000,-750)
await drive_base.straight(-130)
await drive_base.turn(67)
async def solve_tip_the_scale():
await drive_base.straight(-200)
await drive_base.straight(60)
await drive_base.turn(22)
"""
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 drive_base.straight(550)
await right_arm.run_angle(300,100)
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(-15)
# 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
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(-190)
#await drive_base.straight(400)
#await left_arm.run_angle(50,120)
#await drive_base.straight(-200)
await left_arm.run_angle(300,-300)
await drive_base.straight(-600)
drive_base.stop()
async def Run12():
await drive_base.straight(900)
await drive_base.turn(83)
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)
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)
# 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())
Reference in New Issue View Git Blame Copy Permalink