Experimentals
This commit is contained in:
228
competition_codes/hazmat/HazmatCodeChanges.py
Normal file
228
competition_codes/hazmat/HazmatCodeChanges.py
Normal file
@@ -0,0 +1,228 @@
|
||||
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)
|
||||
left_arm = Motor(Port.C)
|
||||
right_arm = Motor(Port.D)
|
||||
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 = 300 # mm
|
||||
|
||||
async def drive_forward():
|
||||
"""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
|
||||
await drive_base.stop
|
||||
print(f"Wall detected at {distance}mm!")
|
||||
break
|
||||
|
||||
# Small delay to prevent overwhelming the sensor
|
||||
await wait(50)
|
||||
|
||||
# New Section
|
||||
async def Run1(): # From M8_5.py
|
||||
left_arm.run_angle(1000, -300)
|
||||
right_arm.run_angle(1000, 500)
|
||||
await drive_base.straight(320)
|
||||
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000, 500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000, 500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(277)
|
||||
await drive_base.turn(20)
|
||||
await drive_base.straight(65)
|
||||
await drive_base.turn(-30)
|
||||
right_arm.run_angle(50, 500)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(-145)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(90)
|
||||
await left_arm.run_angle(1000, 450)
|
||||
await drive_base.straight(-145)
|
||||
await left_arm.run_angle(1000, -450)
|
||||
await drive_base.straight(10)
|
||||
await drive_base.turn(35)
|
||||
await drive_base.straight(-600)
|
||||
|
||||
async def Run2(): # From Heavy_lifting_final.py
|
||||
await drive_base.straight(200)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(536)
|
||||
await drive_base.turn(60, Stop.HOLD)
|
||||
await drive_base.straight(30)
|
||||
await right_arm.run_angle(5000, 2900)
|
||||
await drive_base.straight(40)
|
||||
await right_arm.run_angle(5000, -4000)
|
||||
await drive_base.straight(-60)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-670)
|
||||
|
||||
|
||||
async def Run3(): # tip the scale.py
|
||||
left_arm.run_angle(600,-200)
|
||||
right_arm.run_angle(500,200)
|
||||
await drive_base.straight(70)
|
||||
|
||||
await drive_base.turn(-70)
|
||||
await drive_base.straight(900)
|
||||
await drive_base.turn(115)
|
||||
|
||||
await drive_base.straight(75)
|
||||
await drive_base.straight(33)
|
||||
await right_arm.run_angle(500,-250)
|
||||
await right_arm.run_angle(500,250)
|
||||
await drive_base.turn(66)
|
||||
await drive_base.straight(7)
|
||||
|
||||
await left_arm.run_angle(560,390) #going down
|
||||
print('turning now...')
|
||||
await drive_base.turn(40) # turning right
|
||||
await left_arm.run_angle(410,-400) #lift a little bit
|
||||
await drive_base.straight(80)
|
||||
await drive_base.turn(-41) #ma din din din dun 67 41 21 69
|
||||
await drive_base.straight(900)
|
||||
|
||||
|
||||
async def Run4(): # From Send_Over_Final.py
|
||||
#Get to mission
|
||||
await drive_base.straight(920)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(65)
|
||||
#Solve mission
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000, 4000)
|
||||
#Get to Red Start
|
||||
await drive_base.straight(-110)
|
||||
await drive_base.turn(90)
|
||||
# while True:
|
||||
# distance_mm = await lazer_ranger.distance()
|
||||
# print('distancing...',distance_mm)
|
||||
|
||||
# if distance_mm < 300:
|
||||
# drive_base.stop
|
||||
# break
|
||||
# else:
|
||||
# drive_base.straight(300)
|
||||
# print('running...')
|
||||
# await wait(10)
|
||||
await multitask(
|
||||
drive_forward(),
|
||||
monitor_distance()
|
||||
)
|
||||
|
||||
|
||||
|
||||
# Add Rishi's code here
|
||||
async def Run5():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-18)
|
||||
await drive_base.straight(100)
|
||||
await drive_base.straight(-205)
|
||||
await drive_base.turn(63)
|
||||
await drive_base.straight(125)
|
||||
await arm_motor.run_angle(1000, -1200)
|
||||
await drive_base.straight(84)
|
||||
await arm_motor.run_angle(300, 1200)
|
||||
await drive_base.straight(-875)
|
||||
|
||||
|
||||
# Add - Adi's code here
|
||||
async def Run6():
|
||||
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)
|
||||
await drive_base.turn(-94)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, 900)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-10)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(700, -200)
|
||||
await drive_base.turn(30)
|
||||
await drive_base.straight(-60)
|
||||
await drive_base.turn(80)
|
||||
await drive_base.straight(-900)
|
||||
|
||||
|
||||
# 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 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()
|
||||
reflected = await color_sensor.reflection()
|
||||
color = detect_color(h, s, v, reflected)
|
||||
|
||||
if color == "Red":
|
||||
print('Running Mission 3')
|
||||
await Run3() #red
|
||||
elif color == "Orange":
|
||||
print('Running Mission 6')
|
||||
await Run6() #orange
|
||||
elif color == "Yellow":
|
||||
print('Running Mission 4')
|
||||
await Run4() #yellow
|
||||
elif color == "Green":
|
||||
print('Running Mission 1')
|
||||
await Run1() #green - vertically
|
||||
elif color == "Blue":
|
||||
print('Running Mission 5')
|
||||
await Run5() #blue - vertically
|
||||
elif color == "Purple":
|
||||
print('Running Mission 2')
|
||||
await Run2() #purple - vertically
|
||||
else:
|
||||
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||
await wait(10)
|
||||
# Run the main function
|
||||
run_task(main())
|
||||
33
competition_codes/hazmat/Heavy_lifting_final.py
Normal file
33
competition_codes/hazmat/Heavy_lifting_final.py
Normal file
@@ -0,0 +1,33 @@
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.tools import run_task, multitask
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
|
||||
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||
right_arm = Motor(Port.D)
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||
drive_base.settings(600,500,300,200)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
#await drive_base.straight(200)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(536)
|
||||
await drive_base.turn(60, Stop.HOLD)
|
||||
await drive_base.straight(30)
|
||||
|
||||
await right_arm.run_angle(5000,2900)
|
||||
await drive_base.straight(40)
|
||||
await right_arm.run_angle(5000,-4000)
|
||||
await drive_base.straight(-60)
|
||||
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-670)
|
||||
run_task(main())
|
||||
49
competition_codes/hazmat/M8_5.py
Normal file
49
competition_codes/hazmat/M8_5.py
Normal file
@@ -0,0 +1,49 @@
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.tools import run_task, multitask
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
|
||||
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||
right_arm = Motor(Port.D)
|
||||
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||
|
||||
drive_base.settings(600,500,300,200)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
left_arm.run_angle(1000, 300)
|
||||
right_arm.run_angle(1000,500)
|
||||
await drive_base.straight(320)
|
||||
|
||||
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(277)
|
||||
await drive_base.turn(20)
|
||||
await drive_base.straight(65)
|
||||
|
||||
await drive_base.turn(-30)
|
||||
right_arm.run_angle(50,500)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(-145)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(90)
|
||||
await left_arm.run_angle(1000,-450)
|
||||
await drive_base.straight(-145)
|
||||
await left_arm.run_angle(1000,450)
|
||||
await drive_base.straight(10)
|
||||
await drive_base.turn(35)
|
||||
await drive_base.straight(-600)
|
||||
run_task(main())
|
||||
46
competition_codes/hazmat/Send_Over_Final.py
Normal file
46
competition_codes/hazmat/Send_Over_Final.py
Normal file
@@ -0,0 +1,46 @@
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.tools import run_task, multitask
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
|
||||
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||
right_arm = Motor(Port.D)
|
||||
lazer_ranger = UltrasonicSensor(Port.E)
|
||||
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||
|
||||
drive_base.settings(600,500,300,200)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
#Get to mission
|
||||
await drive_base.straight(920)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(65)
|
||||
#Solve mission
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000,-4000)
|
||||
#Get to Red Start
|
||||
await drive_base.straight(-110)
|
||||
await drive_base.turn(90)
|
||||
await drive_base.straight(500)
|
||||
while True:
|
||||
distance_mm = await lazer_ranger.distance()
|
||||
print('distancing...',distance_mm)
|
||||
|
||||
if distance_mm < 300:
|
||||
drive_base.stop
|
||||
break
|
||||
else:
|
||||
drive_base.straight(300)
|
||||
print('running...')
|
||||
await wait(10)
|
||||
|
||||
run_task(main())
|
||||
384
competition_codes/hazmat/hazmat_main.py
Normal file
384
competition_codes/hazmat/hazmat_main.py
Normal file
@@ -0,0 +1,384 @@
|
||||
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)
|
||||
|
||||
left_arm = Motor(Port.C)
|
||||
right_arm = Motor(Port.D)
|
||||
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)
|
||||
|
||||
# Color Settings
|
||||
# https://docs.pybricks.com/en/latest/parameters/color.html
|
||||
print("Default Detected Colors:", color_sensor.detectable_colors())
|
||||
|
||||
# Custom color Hue, Saturation, Brightness value for Lego bricks
|
||||
Color.MAGENTA = Color(300,100,100)
|
||||
Color.VIOLET = Color(277,68,32)
|
||||
Color.BLUE = Color(240,100,100)
|
||||
Color.CYAN = Color(180,100,100)
|
||||
|
||||
LEGO_BRICKS_COLOR = [
|
||||
Color.BLUE,
|
||||
Color.GREEN,
|
||||
Color.WHITE,
|
||||
Color.RED,
|
||||
Color.YELLOW,
|
||||
Color.MAGENTA,
|
||||
Color.VIOLET,
|
||||
Color.NONE
|
||||
]
|
||||
#Update Detectable colors
|
||||
color_sensor.detectable_colors(LEGO_BRICKS_COLOR)
|
||||
|
||||
print("Updated Detected Colors:", color_sensor.detectable_colors())
|
||||
|
||||
hub.speaker.volume(50) # Set the volume of the speaker
|
||||
|
||||
# Celebration sound types
|
||||
class CelebrationSound:
|
||||
VICTORY_FANFARE = 0
|
||||
LEVEL_UP = 1
|
||||
SUCCESS_CHIME = 2
|
||||
TA_DA = 3
|
||||
POWER_UP = 4
|
||||
RICKROLL_INSPIRED = 5
|
||||
|
||||
async def play_victory_fanfare():
|
||||
"""Classic victory fanfare"""
|
||||
notes = [
|
||||
(262, 200), # C4
|
||||
(262, 200), # C4
|
||||
(262, 200), # C4
|
||||
(349, 600), # F4
|
||||
]
|
||||
|
||||
for freq, duration in notes:
|
||||
await hub.speaker.beep(freq, duration)
|
||||
await wait(50)
|
||||
async def play_level_up():
|
||||
"""Upward scale for level completion"""
|
||||
notesold = [
|
||||
(262, 100), # C4
|
||||
(294, 100), # D4
|
||||
(330, 100), # E4
|
||||
(349, 100), # F4
|
||||
(392, 100), # G4
|
||||
(440, 100), # A4
|
||||
(494, 100), # B4
|
||||
(523, 300), # C5
|
||||
]
|
||||
notes = [
|
||||
(277, 100),
|
||||
(330, 100),
|
||||
(277, 100),
|
||||
(554, 100),
|
||||
(277, 100),
|
||||
(413, 100),
|
||||
(330, 100),
|
||||
(277, 100),
|
||||
(413, 100),
|
||||
(277, 100),
|
||||
(554, 100),
|
||||
(413, 100),
|
||||
(277, 100),
|
||||
(413, 100),
|
||||
(554, 100),
|
||||
(413, 100)
|
||||
]
|
||||
|
||||
for freq, duration in notes:
|
||||
await hub.speaker.beep(freq, duration)
|
||||
#await wait(20)
|
||||
async def play_success_chime():
|
||||
"""Simple success notification"""
|
||||
notes = [
|
||||
(523, 150), # C5
|
||||
(659, 150), # E5
|
||||
(784, 300), # G5
|
||||
]
|
||||
|
||||
for freq, duration in notes:
|
||||
await hub.speaker.beep(freq, duration)
|
||||
await wait(50)
|
||||
async def play_ta_da():
|
||||
"""Classic "ta-da!" sound"""
|
||||
notes = [
|
||||
(392, 200), # G4
|
||||
(523, 400), # C5
|
||||
]
|
||||
|
||||
for freq, duration in notes:
|
||||
await hub.speaker.beep(freq, duration)
|
||||
await wait(100)
|
||||
async def play_power_up():
|
||||
"""Rising power-up sound"""
|
||||
for freq in range(200, 800, 50):
|
||||
await hub.speaker.beep(freq, 50)
|
||||
await wait(10)
|
||||
await hub.speaker.beep(1000, 200)
|
||||
async def play_rickroll_inspired():
|
||||
"""Fun 80s-style dance beat inspired sound"""
|
||||
# Upbeat bouncy rhythm
|
||||
pattern = [
|
||||
(392, 200), (440, 200), (494, 200), (523, 200),
|
||||
(440, 200), (392, 200), (349, 200), (392, 300),
|
||||
(440, 200), (392, 200), (349, 200), (330, 400),
|
||||
]
|
||||
|
||||
for freq, duration in pattern:
|
||||
await hub.speaker.beep(freq, duration)
|
||||
await wait(50)
|
||||
async def celebrate_mission_complete(sound_type=CelebrationSound.SUCCESS_CHIME):
|
||||
"""
|
||||
Main celebration function to call after completing a mission.
|
||||
Plays a sound and shows light animation.
|
||||
|
||||
Args:
|
||||
sound_type: CelebrationSound enum value (default: SUCCESS_CHIME)
|
||||
"""
|
||||
# Light show
|
||||
hub.light.on(Color.GREEN)
|
||||
|
||||
# Play the selected celebration sound
|
||||
if sound_type == CelebrationSound.VICTORY_FANFARE:
|
||||
await play_victory_fanfare()
|
||||
elif sound_type == CelebrationSound.LEVEL_UP:
|
||||
await play_level_up()
|
||||
elif sound_type == CelebrationSound.SUCCESS_CHIME:
|
||||
await play_success_chime()
|
||||
elif sound_type == CelebrationSound.TA_DA:
|
||||
await play_ta_da()
|
||||
elif sound_type == CelebrationSound.POWER_UP:
|
||||
await play_power_up()
|
||||
elif sound_type == CelebrationSound.RICKROLL_INSPIRED:
|
||||
await play_rickroll_inspired()
|
||||
else:
|
||||
await play_success_chime() # Default fallback
|
||||
|
||||
# Blink the light
|
||||
for _ in range(3):
|
||||
hub.light.off()
|
||||
await wait(100)
|
||||
hub.light.on(Color.GREEN)
|
||||
await wait(100)
|
||||
|
||||
hub.light.off()
|
||||
|
||||
WALL_DISTANCE = 200 # mm
|
||||
async def drive_forward():
|
||||
"""Drive forward continuously using DriveBase."""
|
||||
#await drive_base.straight(5000)
|
||||
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
|
||||
await drive_base.turn(-180)
|
||||
drive_base.brake
|
||||
print(f"Wall detected at {distance}mm!")
|
||||
break
|
||||
|
||||
# Small delay to prevent overwhelming the sensor
|
||||
await wait(50)
|
||||
|
||||
async def Run1(): #From M8_5.py
|
||||
left_arm.run_angle(1000, 300)
|
||||
right_arm.run_angle(1000,500)
|
||||
await drive_base.straight(320)
|
||||
|
||||
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(277)
|
||||
await drive_base.turn(20)
|
||||
await drive_base.straight(65)
|
||||
|
||||
await drive_base.turn(-30)
|
||||
right_arm.run_angle(50,500)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(-145)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(90)
|
||||
await left_arm.run_angle(1000,-450)
|
||||
await drive_base.straight(-145)
|
||||
await left_arm.run_angle(1000,450)
|
||||
await drive_base.straight(10)
|
||||
await drive_base.turn(35)
|
||||
await drive_base.straight(-600)
|
||||
|
||||
async def Run2(): #From Heavy_lifting_final.py
|
||||
await drive_base.straight(200)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(536)
|
||||
await drive_base.turn(60, Stop.HOLD)
|
||||
await drive_base.straight(30)
|
||||
|
||||
await right_arm.run_angle(5000,2900)
|
||||
await drive_base.straight(40)
|
||||
await right_arm.run_angle(5000,-4000)
|
||||
await drive_base.straight(-60)
|
||||
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-670)
|
||||
|
||||
|
||||
async def Run3(): #tip the scale.py
|
||||
left_arm.run_angle(600,200)
|
||||
right_arm.run_angle(500,200)
|
||||
await drive_base.straight(70)
|
||||
|
||||
await drive_base.turn(-70)
|
||||
await drive_base.straight(900)
|
||||
await drive_base.turn(115)
|
||||
|
||||
await drive_base.straight(75)
|
||||
await drive_base.straight(33)
|
||||
await right_arm.run_angle(500,-250)
|
||||
await right_arm.run_angle(500,250)
|
||||
await drive_base.turn(66)
|
||||
await drive_base.straight(7)
|
||||
|
||||
await left_arm.run_angle(560,-390) #going down
|
||||
await drive_base.turn(40) # turning right
|
||||
await left_arm.run_angle(-410,-400) #lift a little bit
|
||||
|
||||
await drive_base.turn(-46.5) #ma din din din dun
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(900)
|
||||
|
||||
async def Run4(): #From Send_Over_Final.py
|
||||
|
||||
#Get to mission
|
||||
await drive_base.straight(920)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(65)
|
||||
#Solve mission
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000,-4000)
|
||||
#Get to Red Start
|
||||
await drive_base.straight(-110)
|
||||
await drive_base.turn(90)
|
||||
await drive_base.straight(500)
|
||||
|
||||
# while True:
|
||||
# distance_mm = await lazer_ranger.distance()
|
||||
# print('distancing...',distance_mm)
|
||||
|
||||
# if distance_mm < 300:
|
||||
# drive_base.stop
|
||||
# break
|
||||
# else:
|
||||
# drive_base.straight(300)
|
||||
# print('running...')
|
||||
# await wait(10)
|
||||
await multitask(
|
||||
drive_forward(),
|
||||
monitor_distance()
|
||||
)
|
||||
|
||||
#Add Rishi's code here
|
||||
async def Run5():
|
||||
await drive_base.straight(519)
|
||||
await left_arm.run_angle(-10000, 300)
|
||||
await left_arm.run_angle(10000, 600)
|
||||
await drive_base.straight(160)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(3000, 3000)
|
||||
await drive_base.straight(-150)
|
||||
await drive_base.turn(135)
|
||||
await drive_base.straight(25)
|
||||
await right_arm.run_angle(10000, -3000)
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(-55)
|
||||
await right_arm.run_angle(10000, -3000)
|
||||
await drive_base.straight(250)
|
||||
await drive_base.turn(-5)
|
||||
await right_arm.run_angle(10000, 7000)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(100)
|
||||
await drive_base.turn(37)
|
||||
await right_arm.run_angle(10000, -6000)
|
||||
await drive_base.straight(90)
|
||||
await right_arm.run_angle(3000, 3000)
|
||||
await drive_base.turn(-40)
|
||||
|
||||
#Add - Adi's code here
|
||||
async def Run6():
|
||||
await drive_base.straight(420)
|
||||
await right_arm.run_angle(300,-100)
|
||||
await drive_base.straight(-100)
|
||||
await right_arm.run_angle(300, 100)
|
||||
await drive_base.straight(-350)
|
||||
|
||||
async def main():
|
||||
while True:
|
||||
|
||||
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
|
||||
color_reflected_percent = await color_sensor.reflection()
|
||||
print(color_reflected_percent)
|
||||
|
||||
|
||||
if color_reflected_percent > 0: # Make sure we actually have color reflections before checking for color
|
||||
color_detected = await color_sensor.color() # Moved this inside the if clause
|
||||
print(f'Detected color:{color_detected} : {color_detected.h}, {color_detected.s}, {color_detected.v}')
|
||||
|
||||
if color_detected == Color.GREEN:
|
||||
print('Running Mission 1')
|
||||
await Run1()
|
||||
#await celebrate_mission_complete(CelebrationSound.VICTORY_FANFARE)
|
||||
elif color_detected == Color.WHITE:
|
||||
print('Running Mission 2')
|
||||
await Run2()
|
||||
#await celebrate_mission_complete(CelebrationSound.RICKROLL_INSPIRED)
|
||||
elif color_detected == Color.RED:
|
||||
print('Running Mission 3')
|
||||
await Run3()
|
||||
#await celebrate_mission_complete(CelebrationSound.SUCCESS_CHIME )
|
||||
elif color_detected == Color.YELLOW:
|
||||
print('Running Mission 4')
|
||||
await Run4()
|
||||
#await celebrate_mission_complete(CelebrationSound.POWER_UP)
|
||||
elif color_detected == Color.BLUE:
|
||||
print('Running Mission 5')
|
||||
await Run5()
|
||||
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
|
||||
elif color_detected == Color.VIOLET:
|
||||
print('Running Mission 6')
|
||||
await Run6()
|
||||
else:
|
||||
hub.light.off()
|
||||
left_motor.stop()
|
||||
right_motor.stop()
|
||||
color_detected = Color.NONE
|
||||
else:
|
||||
color_detected = Color.NONE
|
||||
|
||||
await wait(1000) #prevent loop from iterating fast
|
||||
# Main execution loop
|
||||
run_task(main())
|
||||
230
competition_codes/hazmat/mainhazmatUPD.py
Normal file
230
competition_codes/hazmat/mainhazmatUPD.py
Normal file
@@ -0,0 +1,230 @@
|
||||
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)
|
||||
left_arm = Motor(Port.C)
|
||||
right_arm = Motor(Port.D)
|
||||
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 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
|
||||
await drive_base.stop
|
||||
print(f"Wall detected at {distance}mm!")
|
||||
break
|
||||
|
||||
# Small delay to prevent overwhelming the sensor
|
||||
await wait(50)
|
||||
|
||||
# New Section
|
||||
async def Run1(): # From M8_5.py
|
||||
|
||||
right_arm.run_angle(1000,450)
|
||||
left_arm.run_angle(500,-90)
|
||||
await drive_base.straight(200)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(325)
|
||||
await left_arm.run_angle(500,90)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(500,-180)
|
||||
|
||||
await drive_base.straight(-90)
|
||||
left_arm.run_angle(500,180)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.turn(15)
|
||||
|
||||
await drive_base.straight(-173)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(120)
|
||||
left_arm.run_angle(1000,-670)
|
||||
|
||||
await right_arm.run_angle(5000,-450, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,450, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-450, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,450, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-450, Stop.HOLD)
|
||||
right_arm.run_angle(5000,450, Stop.HOLD)
|
||||
|
||||
await drive_base.turn(-35)
|
||||
await drive_base.straight(297)
|
||||
await drive_base.turn(63)
|
||||
await drive_base.straight(170)
|
||||
|
||||
await drive_base.turn(-80)
|
||||
await drive_base.straight(87)
|
||||
await drive_base.turn(-15)
|
||||
|
||||
await drive_base.straight(-90)
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.arc(-500,None,600)
|
||||
|
||||
async def Run2(): # From Heavy_lifting_final.py
|
||||
await drive_base.straight(200)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(536)
|
||||
await drive_base.turn(60, Stop.HOLD)
|
||||
await drive_base.straight(30)
|
||||
await right_arm.run_angle(5000, 2900)
|
||||
await drive_base.straight(40)
|
||||
await right_arm.run_angle(5000, -4000)
|
||||
await drive_base.straight(-60)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-670)
|
||||
|
||||
|
||||
async def Run3(): # tip the scale.py
|
||||
left_arm.run_angle(600,-200)
|
||||
right_arm.run_angle(500,200)
|
||||
await drive_base.straight(70)
|
||||
|
||||
await drive_base.turn(-70)
|
||||
await drive_base.straight(900)
|
||||
await drive_base.turn(115)
|
||||
|
||||
await drive_base.straight(75)
|
||||
await drive_base.straight(33)
|
||||
await right_arm.run_angle(500,-250)
|
||||
await right_arm.run_angle(500,250)
|
||||
await drive_base.turn(66)
|
||||
await drive_base.straight(7)
|
||||
|
||||
await left_arm.run_angle(560,390) #going down
|
||||
print('turning now...')
|
||||
await drive_base.turn(40) # turning right
|
||||
await left_arm.run_angle(410,-400) #lift a little bit
|
||||
await drive_base.straight(80)
|
||||
await drive_base.turn(-41) #ma din din din dun 67 41 21
|
||||
await drive_base.straight(900)
|
||||
|
||||
|
||||
async def Run4(): # From Send_Over_Final.py
|
||||
|
||||
await drive_base.straight(920)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(65)
|
||||
#Solve
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000,-4000)
|
||||
await drive_base.straight(-110)
|
||||
await drive_base.turn(90)
|
||||
|
||||
await multitask(
|
||||
drive_forward(),
|
||||
monitor_distance()
|
||||
)
|
||||
|
||||
# Add Rishi's code here
|
||||
async def Run5():
|
||||
await drive_base.straight(600)
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(150)
|
||||
await drive_base.turn(60)
|
||||
await drive_base.straight(100)
|
||||
await drive_base.turn(-86)
|
||||
await drive_base.straight(120)
|
||||
await drive_base.turn(-45)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(75)
|
||||
|
||||
# Add - Adi's code here
|
||||
async def Run6():
|
||||
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)
|
||||
await drive_base.turn(-94)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, 900)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-10)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(700, -200)
|
||||
await drive_base.turn(30)
|
||||
await drive_base.straight(-60)
|
||||
await drive_base.turn(80)
|
||||
await drive_base.straight(-900)
|
||||
# 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 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()
|
||||
reflected = await color_sensor.reflection()
|
||||
color = detect_color(h, s, v, reflected)
|
||||
|
||||
if color == "Red":
|
||||
print('Running Mission 3')
|
||||
await Run3() #red
|
||||
elif color == "Orange":
|
||||
print('Running Mission 6')
|
||||
await Run6() #orange
|
||||
elif color == "Yellow":
|
||||
print('Running Mission 4')
|
||||
await Run4() #yellow
|
||||
elif color == "Green":
|
||||
print('Running Mission 1')
|
||||
await Run1() #green - vertically
|
||||
elif color == "Blue":
|
||||
print('Running Mission 5')
|
||||
await Run5() #blue - vertically
|
||||
elif color == "Purple":
|
||||
print('Running Mission 2')
|
||||
await Run2() #purple - vertically
|
||||
else:
|
||||
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||
await wait(10)
|
||||
# Run the main function
|
||||
run_task(main())
|
||||
42
competition_codes/hazmat/tip the scale.py
Normal file
42
competition_codes/hazmat/tip the scale.py
Normal file
@@ -0,0 +1,42 @@
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.tools import run_task, multitask
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
|
||||
|
||||
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||
right_arm = Motor(Port.D)
|
||||
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||
drive_base.settings(300,1000,300,200)
|
||||
|
||||
#drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
left_arm.run_angle(500,200)
|
||||
right_arm.run_angle(500,200)
|
||||
await drive_base.straight(70)
|
||||
|
||||
await drive_base.turn(-55)
|
||||
await drive_base.straight(900)
|
||||
await drive_base.turn(92.5)
|
||||
|
||||
await drive_base.straight(75)
|
||||
await drive_base.straight(21)
|
||||
await right_arm.run_angle(500,-250)
|
||||
await right_arm.run_angle(500,250)
|
||||
await drive_base.turn(55)
|
||||
|
||||
await left_arm.run_angle(300,-400)
|
||||
|
||||
await drive_base.turn(46.5)
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(900)
|
||||
run_task(main())
|
||||
221
competition_codes/regional-final/Final_combined.py
Normal file
221
competition_codes/regional-final/Final_combined.py
Normal file
@@ -0,0 +1,221 @@
|
||||
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)
|
||||
left_arm = Motor(Port.C)
|
||||
right_arm = Motor(Port.D)
|
||||
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 = 300 # mm
|
||||
|
||||
async def drive_forward():
|
||||
"""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
|
||||
await drive_base.stop
|
||||
print(f"Wall detected at {distance}mm!")
|
||||
break
|
||||
|
||||
# Small delay to prevent overwhelming the sensor
|
||||
await wait(50)
|
||||
|
||||
# New Section
|
||||
async def Run1(): # From M8_5.py
|
||||
right_arm.run_angle(1000,450)
|
||||
left_arm.run_angle(500,90)
|
||||
await drive_base.straight(200)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(325)
|
||||
await left_arm.run_angle(500,-90)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(500,180)
|
||||
|
||||
await drive_base.straight(-90)
|
||||
left_arm.run_angle(500,-180)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.turn(15)
|
||||
|
||||
await drive_base.straight(-173)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(120)
|
||||
left_arm.run_angle(1000,670)
|
||||
|
||||
await right_arm.run_angle(5000,-450, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,450, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-450, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,450, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-450, Stop.HOLD)
|
||||
right_arm.run_angle(5000,450, Stop.HOLD)
|
||||
|
||||
await drive_base.turn(-35)
|
||||
await drive_base.straight(297)
|
||||
await drive_base.turn(63)
|
||||
await drive_base.straight(170)
|
||||
|
||||
await drive_base.turn(-80)
|
||||
await drive_base.straight(87)
|
||||
await drive_base.turn(-15)
|
||||
|
||||
await drive_base.straight(-90)
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.arc(-500,None,600)
|
||||
|
||||
|
||||
async def Run2(): # From Heavy_lifting_final.py
|
||||
await drive_base.straight(200)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(536)
|
||||
await drive_base.turn(60, Stop.HOLD)
|
||||
await drive_base.straight(30)
|
||||
await right_arm.run_angle(5000, 2900)
|
||||
await drive_base.straight(40)
|
||||
await right_arm.run_angle(5000, -4000)
|
||||
await drive_base.straight(-60)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-670)
|
||||
|
||||
|
||||
async def Run3(): # tip the scale.py
|
||||
right_arm.run_angle(500,400)
|
||||
await drive_base.straight(800)
|
||||
await drive_base.turn(90)
|
||||
await drive_base.straight(86)
|
||||
await right_arm.run_angle(800,-600)
|
||||
await right_arm.run_angle(900,800)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(90)
|
||||
await drive_base.straight(800)
|
||||
drive_base.brake()
|
||||
|
||||
|
||||
async def Run4(): # From Send_Over_Final.py
|
||||
|
||||
await drive_base.straight(920)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(65)
|
||||
#Solve
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000,-4000)
|
||||
await drive_base.straight(-110)
|
||||
await drive_base.turn(90)
|
||||
|
||||
await multitask(
|
||||
drive_forward(),
|
||||
monitor_distance()
|
||||
)
|
||||
|
||||
# Add Rishi's code here
|
||||
async def Run5():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(110)
|
||||
await drive_base.straight(-210)
|
||||
await drive_base.turn(63)
|
||||
await drive_base.straight(130)
|
||||
await right_arm.run_angle(1000, -1200)
|
||||
await drive_base.straight(84)
|
||||
await right_arm.run_angle(300, 1200)
|
||||
await drive_base.straight(-875)
|
||||
|
||||
# Add - Adi's code here
|
||||
async def Run6():
|
||||
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)
|
||||
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)
|
||||
await drive_base.straight(-60)
|
||||
await drive_base.turn(80)
|
||||
await drive_base.straight(-900)
|
||||
|
||||
|
||||
|
||||
# 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 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()
|
||||
reflected = await color_sensor.reflection()
|
||||
color = detect_color(h, s, v, reflected)
|
||||
|
||||
if color == "Red":
|
||||
print('Running Mission 3')
|
||||
await Run3() #red
|
||||
elif color == "Orange":
|
||||
print('Running Mission 6')
|
||||
await Run6() #orange
|
||||
elif color == "Yellow":
|
||||
print('Running Mission 4')
|
||||
await Run4() #yellow
|
||||
elif color == "Green":
|
||||
print('Running Mission 1')
|
||||
await Run1() #green - vertically
|
||||
elif color == "Blue":
|
||||
print('Running Mission 5')
|
||||
await Run5() #blue - vertically
|
||||
elif color == "Purple":
|
||||
print('Running Mission 2')
|
||||
await Run2() #purple - vertically
|
||||
else:
|
||||
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||
await wait(10)
|
||||
# Run the main function
|
||||
run_task(main())
|
||||
350
competition_codes/sectionals/sectional_main_experimental.py
Normal file
350
competition_codes/sectionals/sectional_main_experimental.py
Normal file
@@ -0,0 +1,350 @@
|
||||
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) # 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 = 300 # 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 async_drive(speed, direction):
|
||||
drive_base.d
|
||||
|
||||
|
||||
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
|
||||
|
||||
# Small delay to prevent overwhelming the sensor
|
||||
await wait(50)
|
||||
|
||||
# Experimental - Removed forge and who live here part
|
||||
async def Run1(): # From M8_5.py
|
||||
right_arm.run_angle(500,30)
|
||||
left_arm.run_angle(500,90)
|
||||
await drive_base.straight(200)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(325)
|
||||
await left_arm.run_angle(500,-90)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(500,180)
|
||||
|
||||
await drive_base.straight(-90)
|
||||
left_arm.run_angle(500,-180)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.turn(15)
|
||||
|
||||
await drive_base.straight(-173)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(120)
|
||||
left_arm.run_angle(1000,670)
|
||||
|
||||
await right_arm.run_angle(4000,-30, Stop.HOLD)
|
||||
await right_arm.run_angle(4000,30, Stop.HOLD)
|
||||
await right_arm.run_angle(4000,-30, Stop.HOLD)
|
||||
await right_arm.run_angle(4000,30, Stop.HOLD)
|
||||
await right_arm.run_angle(4000,-30, Stop.HOLD)
|
||||
right_arm.run_angle(4000,30, Stop.HOLD)
|
||||
|
||||
# await drive_base.turn(-35)
|
||||
# await drive_base.straight(297)
|
||||
# await drive_base.turn(63)
|
||||
# await drive_base.straight(170)
|
||||
|
||||
# await drive_base.turn(-80)
|
||||
# await drive_base.straight(87)
|
||||
# await drive_base.turn(-15)
|
||||
|
||||
await drive_base.straight(-90)
|
||||
#await drive_base.turn(-100)
|
||||
await drive_base.arc(200,None,-300)
|
||||
drive_base.stop()
|
||||
|
||||
async def Run2(): # experiment with ferris wheel for silo
|
||||
await drive_base.straight(410)
|
||||
#await drive_base.turn(-20)
|
||||
#await drive_base.straight(536)
|
||||
#await drive_base.turn(60, Stop.HOLD)
|
||||
#await drive_base.straight(30)
|
||||
for i in range(3):
|
||||
await right_arm.run_angle(10000, -360)
|
||||
await drive_base.straight(-400)
|
||||
|
||||
# Experimental - This to solve forge, who lived here and heavy lifting combined
|
||||
async def Run2_1():
|
||||
await right_arm.run_target(50,50)
|
||||
#await right_arm.re
|
||||
#await multitask(
|
||||
await right_arm.run_angle(50,-30)
|
||||
await drive_base.arc(350,113, None)
|
||||
#)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-40)
|
||||
|
||||
await right_arm.run_angle(500,-90) # arm down
|
||||
await wait(100)
|
||||
await drive_base.turn(20) # turn right a little bit
|
||||
#await wait(100)
|
||||
await right_arm.run_angle(500,90) #arm up
|
||||
await drive_base.turn(-20) #reset position
|
||||
|
||||
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(-100)
|
||||
await drive_base.arc(-500,None,600)
|
||||
drive_base.brake()
|
||||
|
||||
|
||||
async def Run3(): # tip the scale.py
|
||||
right_arm.run_angle(500,30)
|
||||
await drive_base.straight(800)
|
||||
await drive_base.turn(90)
|
||||
await drive_base.straight(86)
|
||||
await right_arm.run_angle(800,-140)
|
||||
await right_arm.run_angle(900,140)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(90)
|
||||
await drive_base.straight(800)
|
||||
drive_base.brake()
|
||||
|
||||
|
||||
async def Run4(): # From Send_Over_Final.py
|
||||
|
||||
await drive_base.straight(920)
|
||||
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 multitask(
|
||||
drive_forward(),
|
||||
monitor_distance()
|
||||
)
|
||||
|
||||
async def Run_3_4(): #combined angler artifacts and tip the scale - experimental
|
||||
await drive_base.straight(920)
|
||||
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,-95, None)
|
||||
|
||||
await drive_base.straight(100)
|
||||
await right_arm.run_angle(800,-150)
|
||||
await right_arm.run_angle(900,150)
|
||||
|
||||
await drive_base.straight(-80)
|
||||
await drive_base.turn(-67)
|
||||
await drive_base.straight(450,Stop.COAST_SMART)
|
||||
await drive_base.arc(10,-25, None)
|
||||
#await drive_base.turn(-23, Stop.COAST_SMART)
|
||||
|
||||
await multitask(
|
||||
drive_forward(),
|
||||
monitor_distance()
|
||||
)
|
||||
|
||||
|
||||
# Add Rishi's code here
|
||||
async def Run5():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(110)
|
||||
await drive_base.straight(-210)
|
||||
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)
|
||||
await drive_base.straight(-875)
|
||||
|
||||
# Add - Adi's code here
|
||||
async def Run6():
|
||||
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)
|
||||
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)
|
||||
await drive_base.straight(-60)
|
||||
await drive_base.turn(80)
|
||||
await drive_base.straight(-900)
|
||||
|
||||
async def arc_back(radius, angle):
|
||||
#print(radius, angle)
|
||||
drive_base.arc(radius, angle, None)
|
||||
|
||||
async def lift_right_arm():
|
||||
await right_arm.run_angle(100, -90)
|
||||
|
||||
# This is a refactor of Salvage operation mission without wait and realignment
|
||||
async def Run6_1(): #experimental
|
||||
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(100,90)
|
||||
await drive_base.straight(-75)
|
||||
|
||||
#ack back while raising arm
|
||||
await multitask (
|
||||
lift_right_arm(),
|
||||
arc_back(-215, -45)
|
||||
)
|
||||
|
||||
#align to raise the ship
|
||||
await drive_base.straight(-150)
|
||||
await drive_base.turn(-45)
|
||||
|
||||
#await drive_base.straight(-350)
|
||||
#await wait(1000)
|
||||
|
||||
#Solve ship
|
||||
await drive_base.straight(800)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(-15)
|
||||
await drive_base.straight(350)
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, -700)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(700, 250)
|
||||
await drive_base.turn(15)
|
||||
await drive_base.straight(-80)
|
||||
await drive_base.turn(80)
|
||||
await drive_base.straight(-900)
|
||||
drive_base.stop()
|
||||
|
||||
async def Run6_7(): # experiment with ferris wheel for Site Markings
|
||||
await drive_base.straight(500)
|
||||
|
||||
await right_arm.run_angle(100, -10)
|
||||
await wait(50)
|
||||
#await multitask (
|
||||
#lift_right_arm()
|
||||
#await right_arm.run_angle(100, 10)
|
||||
#await arc_back(-215, -140)
|
||||
await drive_base.straight(-300)
|
||||
await drive_base.arc(-150, -140, None)
|
||||
|
||||
await drive_base.straight(-300)
|
||||
await wait(50)
|
||||
await right_arm.run_angle(50, 50)
|
||||
#await drive_base.straight(-300)
|
||||
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:
|
||||
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 == "Red":
|
||||
print('Running Mission 3')
|
||||
await Run3() #red
|
||||
elif color == "Orange":
|
||||
print('Running Mission 6')
|
||||
await Run6_1() #orange
|
||||
elif color == "Yellow":
|
||||
print('Running Mission 4')
|
||||
await Run_3_4() #yellow
|
||||
elif color == "Green":
|
||||
print('Running Mission 1')
|
||||
await Run1() #green - vertically
|
||||
elif color == "Blue":
|
||||
print('Running Mission 5')
|
||||
await Run5() #blue - vertically
|
||||
elif color == "Purple":
|
||||
print('Running Mission 2')
|
||||
await Run6_7() #purple - vertically
|
||||
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