Add competition_codes/state/sunprarie_state_main.py
This commit is contained in:
382
competition_codes/state/sunprarie_state_main.py
Normal file
382
competition_codes/state/sunprarie_state_main.py
Normal file
@@ -0,0 +1,382 @@
|
|||||||
|
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)
|
||||||
|
|
||||||
|
WALL_DISTANCE = 200 # mm
|
||||||
|
|
||||||
|
async def drive_forward():
|
||||||
|
"""Drive forward continuously using DriveBase."""
|
||||||
|
drive_base.drive(400, 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)
|
||||||
|
|
||||||
|
"""
|
||||||
|
Run#1
|
||||||
|
- Removed forge and who lived here part
|
||||||
|
- What's on sale + Silo
|
||||||
|
- Green Key
|
||||||
|
"""
|
||||||
|
async def Run1():
|
||||||
|
|
||||||
|
await left_arm.run_until_stalled(1500 ,duty_limit=15)
|
||||||
|
left_arm.reset_angle(0)
|
||||||
|
await solve_whats_on_sale()
|
||||||
|
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)
|
||||||
|
left_arm.run_angle(500,-119.5)
|
||||||
|
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(90)
|
||||||
|
|
||||||
|
|
||||||
|
await right_arm.run_angle(10000,-80, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(10000,80, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(10000,-80, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(10000,80, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(10000,-80, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(10000,80, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(10000,-80, Stop.HOLD)
|
||||||
|
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(940)
|
||||||
|
await drive_base.turn(-90,Stop.HOLD)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
#Solve
|
||||||
|
drive_base.turn(-10)
|
||||||
|
await left_arm.run_angle(10000,-3000)
|
||||||
|
await drive_base.straight(-110)
|
||||||
|
|
||||||
|
await drive_base.turn(90)
|
||||||
|
await drive_base.arc(-150,-103, None)
|
||||||
|
|
||||||
|
async def solve_tip_the_scale():
|
||||||
|
await drive_base.turn(3)
|
||||||
|
await drive_base.straight(142.5)
|
||||||
|
await right_arm.run_angle(800,-150)
|
||||||
|
await right_arm.run_angle(900,150)
|
||||||
|
|
||||||
|
await drive_base.straight(-100)
|
||||||
|
await drive_base.turn(-65)
|
||||||
|
await drive_base.straight(300,Stop.COAST_SMART)
|
||||||
|
await drive_base.arc(10,-47, None)
|
||||||
|
#await drive_base.turn(-23, Stop.COAST_SMART)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
"""
|
||||||
|
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():
|
||||||
|
await drive_base.straight(550)
|
||||||
|
await right_arm.run_angle(300,100)
|
||||||
|
await drive_base.straight(-75)
|
||||||
|
await right_arm.run_angle(300, -100)
|
||||||
|
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, -900)
|
||||||
|
await drive_base.straight(120)
|
||||||
|
await drive_base.turn(5)
|
||||||
|
await left_arm.run_angle(500, 290)
|
||||||
|
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, 250)
|
||||||
|
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)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 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:
|
||||||
|
h, s, v = await color_sensor.hsv()
|
||||||
|
print(color_sensor.color())
|
||||||
|
print(h,s,v)
|
||||||
|
reflected = await color_sensor.reflection()
|
||||||
|
color = detect_color(h, s, v, reflected)
|
||||||
|
print(color)
|
||||||
|
|
||||||
|
|
||||||
|
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 Run6_7()
|
||||||
|
elif color == "Light_Blue":
|
||||||
|
print("Running Mission 2_1")
|
||||||
|
await Run2_1()
|
||||||
|
else:
|
||||||
|
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||||
|
await wait(10)
|
||||||
|
# Run the main function
|
||||||
|
run_task(main())
|
||||||
Reference in New Issue
Block a user