534
competition_codes/AROC/Nationals_old.py
Normal file
534
competition_codes/AROC/Nationals_old.py
Normal file
@@ -0,0 +1,534 @@
|
||||
#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(1000, 600, 500, 300)
|
||||
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)
|
||||
|
||||
"""
|
||||
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(210)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(320)
|
||||
await left_arm.run_angle(490,-30)
|
||||
|
||||
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(5)
|
||||
await drive_base.turn(-25)
|
||||
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(175)
|
||||
|
||||
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(-90)
|
||||
left_arm.run_angle(500,-50)
|
||||
left_arm.run_angle(1000,6)
|
||||
await drive_base.turn(-20)
|
||||
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 = 10000 # speed in degree per second
|
||||
SWING_ANGLE = 60 # the angle!
|
||||
REBOUND_ADJ = 20
|
||||
|
||||
# Repeat this motion 3 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(-220)
|
||||
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(-3)
|
||||
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(-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(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(-130)
|
||||
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 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(400)
|
||||
#await left_arm.run_angle(50,120)
|
||||
#await drive_base.straight(-200)
|
||||
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())
|
||||
1017
competition_codes/AROC/Nationals_old2.py
Normal file
1017
competition_codes/AROC/Nationals_old2.py
Normal file
File diff suppressed because it is too large
Load Diff
655
competition_codes/AROC/Nationals_old3.py
Normal file
655
competition_codes/AROC/Nationals_old3.py
Normal file
@@ -0,0 +1,655 @@
|
||||
#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)
|
||||
|
||||
# PID Controller Module
|
||||
class PIDController:
|
||||
def __init__(self, kp, ki, kd):
|
||||
self.kp = kp # Proportional gain
|
||||
self.ki = ki # Integral gain
|
||||
self.kd = kd # Derivative gain
|
||||
self.previous_error = 0
|
||||
self.integral = 0
|
||||
|
||||
def calculate(self, desired_value, actual_value, dt):
|
||||
"""
|
||||
Calculate PID output
|
||||
|
||||
Args:
|
||||
desired_value: Target value
|
||||
actual_value: Current measured value
|
||||
dt: Time delta in seconds
|
||||
|
||||
Returns:
|
||||
PID output value
|
||||
"""
|
||||
error = desired_value - actual_value
|
||||
|
||||
# Proportional term
|
||||
proportional = error * self.kp
|
||||
|
||||
# Integral term
|
||||
self.integral += error * dt
|
||||
integral = self.integral * self.ki
|
||||
|
||||
# Derivative term
|
||||
derivative = (error - self.previous_error) / dt * self.kd if dt > 0 else 0
|
||||
|
||||
# Store current error for next iteration
|
||||
self.previous_error = error
|
||||
|
||||
# Calculate total output
|
||||
output = proportional + integral + derivative
|
||||
|
||||
return output
|
||||
|
||||
def reset(self):
|
||||
"""Reset the PID controller"""
|
||||
self.previous_error = 0
|
||||
self.integral = 0
|
||||
|
||||
# Create PID controllers for Missions 5, 11, and 12
|
||||
straight_pid = PIDController(kp=1.2, ki=0.0, kd=0.1)
|
||||
turn_pid = PIDController(kp=2.0, ki=0.0, kd=0.2)
|
||||
|
||||
"""
|
||||
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():
|
||||
# Reset PID controller for this mission
|
||||
straight_pid.reset()
|
||||
|
||||
# Getting the sand down with PID control
|
||||
await drive_straight_pid(550)
|
||||
await right_arm.run_angle(300,100)
|
||||
await drive_straight_pid(-75)
|
||||
await right_arm.run_angle(300, -100)
|
||||
|
||||
# Shoving the boat into place
|
||||
await drive_straight_pid(300)
|
||||
await drive_straight_pid(-200)
|
||||
await drive_turn_pid(-15)
|
||||
|
||||
# Solving statue
|
||||
await drive_straight_pid(350)
|
||||
await drive_turn_pid(-104)
|
||||
await drive_straight_pid(-80)
|
||||
await left_arm.run_angle(500, -300)
|
||||
await drive_straight_pid(120)
|
||||
await drive_turn_pid(5)
|
||||
|
||||
# Lift up statue
|
||||
await left_arm.run_angle(500, 100, Stop.HOLD)
|
||||
await drive_turn_pid(18)
|
||||
await drive_straight_pid(-100)
|
||||
await drive_turn_pid(-90)
|
||||
await drive_straight_pid(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)
|
||||
|
||||
# PID-enhanced drive functions for Mission 5
|
||||
async def drive_straight_pid(distance_mm):
|
||||
"""Drive straight using PID control"""
|
||||
start_distance = drive_base.distance()
|
||||
target_distance = start_distance + distance_mm
|
||||
straight_pid.reset()
|
||||
|
||||
stopwatch = StopWatch()
|
||||
stopwatch.reset()
|
||||
|
||||
# Drive with PID correction
|
||||
while abs(drive_base.distance() - target_distance) > 5: # 5mm tolerance
|
||||
elapsed_time = stopwatch.time() / 1000.0 # Convert to seconds
|
||||
stopwatch.reset()
|
||||
|
||||
# Calculate PID correction
|
||||
correction = straight_pid.calculate(target_distance, drive_base.distance(), elapsed_time)
|
||||
|
||||
# Apply correction (limit to reasonable values)
|
||||
correction = max(min(correction, 100), -100)
|
||||
|
||||
# Drive with correction
|
||||
drive_base.drive(200, correction) # Base speed 200 mm/s
|
||||
await wait(10)
|
||||
|
||||
drive_base.stop()
|
||||
|
||||
async def drive_turn_pid(angle_degrees):
|
||||
"""Turn using PID control"""
|
||||
start_angle = drive_base.angle()
|
||||
target_angle = start_angle + angle_degrees
|
||||
turn_pid.reset()
|
||||
|
||||
stopwatch = StopWatch()
|
||||
stopwatch.reset()
|
||||
|
||||
# Turn with PID correction
|
||||
while abs(drive_base.angle() - target_angle) > 2: # 2 degree tolerance
|
||||
elapsed_time = stopwatch.time() / 1000.0 # Convert to seconds
|
||||
stopwatch.reset()
|
||||
|
||||
# Calculate PID correction
|
||||
correction = turn_pid.calculate(target_angle, drive_base.angle(), elapsed_time)
|
||||
|
||||
# Apply correction (limit to reasonable values)
|
||||
correction = max(min(correction, 100), -100)
|
||||
|
||||
# Turn with correction
|
||||
drive_base.drive(0, correction) # No forward movement, just turning
|
||||
await wait(10)
|
||||
|
||||
drive_base.stop()
|
||||
|
||||
"""
|
||||
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
|
||||
# Reset PID controllers
|
||||
straight_pid.reset()
|
||||
turn_pid.reset()
|
||||
|
||||
# Drive forward with PID
|
||||
await drive_straight_pid(600)
|
||||
|
||||
# Turn with PID
|
||||
await drive_turn_pid(-30)
|
||||
|
||||
# Drive forward with PID
|
||||
await drive_straight_pid(250)
|
||||
|
||||
# Return with PID control
|
||||
await drive_straight_pid(-100)
|
||||
await drive_turn_pid(30)
|
||||
await drive_straight_pid(-600)
|
||||
drive_base.stop()
|
||||
|
||||
async def Run12():
|
||||
# Reset PID controllers for this mission
|
||||
straight_pid.reset()
|
||||
turn_pid.reset()
|
||||
|
||||
# Drive forward with PID
|
||||
await drive_straight_pid(900)
|
||||
|
||||
# Turn with PID
|
||||
await drive_turn_pid(83)
|
||||
|
||||
await left_arm.run_angle(3000, -300)
|
||||
await right_arm.run_angle(1100, -180)
|
||||
drive_base.settings(150, 50, 50, 50)
|
||||
await drive_straight_pid(120)
|
||||
left_arm.reset_angle(0)
|
||||
await left_arm.run_angle(50, 50)
|
||||
await right_arm.run_angle(50, 90)
|
||||
await drive_straight_pid(-100)
|
||||
drive_base.settings(950, 750, 750, 750)
|
||||
|
||||
# Turn with PID
|
||||
await drive_turn_pid(110)
|
||||
|
||||
# Drive forward with PID
|
||||
await drive_straight_pid(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())
|
||||
652
competition_codes/AROC/NumberOne.py
Normal file
652
competition_codes/AROC/NumberOne.py
Normal file
@@ -0,0 +1,652 @@
|
||||
"""
|
||||
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())
|
||||
Reference in New Issue
Block a user