Compare commits
1 Commits
dev
...
d1eb92cf25
| Author | SHA1 | Date | |
|---|---|---|---|
| d1eb92cf25 |
12
LINEUPS.md
12
LINEUPS.md
@@ -2,14 +2,14 @@
|
||||
|
||||
## These are the line-up positions for the robot game for various missions.
|
||||
|
||||
- Mission Run #1 (Run #1) [Right/Blue Home] - The left yellow part of the right arm attachment is positioned with its right edge on the 5th thin line from the left. Note that this is NOT positioned from the back of the robot. Also, when counting these lines, make sure you count from the inside curve, not the outside.
|
||||
- Mission Run #1 (Mission #1) [Right/Blue Home] - The left yellow part of the right arm attachment is positioned with its right edge on the 5th thin line from the left. Note that this is NOT positioned from the back of the robot.
|
||||
|
||||
<img src="https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/do-not-merge/r1l.png" alt="Alt text" width="300"/>
|
||||
- Mission Run #2 (Send Over) [Right/Blue Home] - The robot should be lined up on the vertical edge of the left home. The robot's right side should be positioned on the 1st thin line from the top. Note that the 0th line is the one that is not part of the curve.
|
||||
|
||||
- Mission Run #2 (Tip the scales) [Right/Blue Home] - The middle of the left edge of the robot should be positioned on the 2nd thick line from the left.
|
||||
- Mission Run #3 (Sand mission) [Right/Blue Home] - The robot should be lined up on the vertical edge of the left home. The robot's left side should be positioned on the 1/2th thin line from the bottom.
|
||||
|
||||
- Mission Run #3 (Send Over) [Right/Blue Home] - The robot should be lined up on the vertical edge of the right home. The robot's right edge should be positioned on the 1st thin line from the top. Note that the 0th line is the one that is in the inner curve.
|
||||
- Mission Run #4 (Boat mission) [Left/Red Home] - The robot should be lined up on the vertical edge of the left home. The robot's right side should be positioned on the 2nd thick line from the bottom.
|
||||
|
||||
- Mission Run #4 (Run #4) [Left/Red Home] - The robot's left edge should be positioned on the 2nd thick line from the left.
|
||||
- Mission Run #5 (Bautism) [Left/Red Home] - The robot's left edge should be positioned at the 1st thick, 2nd thin line from the left.
|
||||
|
||||
- Mission Run #5 (Boat mission) [Left/Red Home] - There are two alignments for this. When sending off the robot for part 1, the robot should be facing the right home. It's right edge should be positioned at the very bottom edge of the board. Once it completes the pulling part, once it comes back begin part 2. For part 2, the middle of the robot's right side should be positioned in the middle of the 3rd thick and the 3rd thick, 1st thin lines from the top. For both runs the robot should be facing the blue home.
|
||||
- Mission Run #6 (Not-so-heavy Lifting) [Right/Blue Home] - The robot's right edge should be positioned at the 1st thick from the right.
|
||||
25
README.md
25
README.md
@@ -63,14 +63,17 @@ Repository
|
||||
|
||||
### Installation & Deployment - from the server - everyday
|
||||
|
||||
1. Download the file competition_codes/sectionals/sectional_main_dec_6.py
|
||||
1. Download the file codes_for_scrimmage/hazmat/mainhazmatUPD.py
|
||||
- You can do this through the repo, by using cURL, or by using git.
|
||||
- Repo - Go to the [latest release](https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/releases) and click the "Download as ZIP" button to get the full repository.
|
||||
- Single file - Go to the latest release and click the file link to get the raw master file.
|
||||
- Repo - Go to [codes_for_scrimmage/hazmat/mainhazmatUPD.py](codes_for_scrimmage/hazmat/mainhazmatUPD.py) and click the "Download" button.
|
||||
- cURL or another HTTP data transferrer -
|
||||
|
||||
```curl -o mainhazmatUPD.py https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed/raw/branch/main/codes_for_scrimmage/hazmat/mainhazmatUPD.py```
|
||||
- git CLI -
|
||||
```git clone https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git```
|
||||
|
||||
```git clone -b dev https://codes.fll-65266.org/FLL_65266_LEGO_DYNAMICS/solutions_season_unearthed.git && cd solutions_season_unearthed/codes_for_scrimmage/hazmat```
|
||||
|
||||
Then use the master file.
|
||||
Then use mainhazmatUPD.py.
|
||||
|
||||
2. Open https://code.pybricks.com/ and select the "Import a file" button on the top of the left bar. Import the .py file. Pair your robot via Bluetooth in Pybricks by selecting the Bluetooth button.
|
||||
|
||||
@@ -91,12 +94,12 @@ Repository
|
||||
### Color Start System
|
||||
| Color | Mission |
|
||||
|-------|-----------|
|
||||
| ```Green 🟩 ```| Run 1 |
|
||||
| ```Purple 🟪 ```| Run 2 |
|
||||
| ```Red 🟥 ```| Run 3 |
|
||||
| ```Yellow 🟨 ```| Run 4 |
|
||||
| ```Blue 🟦 ```| Run 5 |
|
||||
| ```Orange 🟧 ```| Run 6 |
|
||||
| Green 🟩 | Run 1 |
|
||||
| Purple 🟪 | Run 2 |
|
||||
| Red 🟥| Run 3 |
|
||||
| Yellow 🟨| Run 4 |
|
||||
| Blue 🟦| Run 5 |
|
||||
| Orange 🟧| Run 6 |
|
||||
|
||||
---
|
||||
|
||||
|
||||
@@ -140,41 +140,40 @@ async def Run4(): # From Send_Over_Final.py
|
||||
|
||||
# Add Rishi's code here
|
||||
async def Run5():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-18)
|
||||
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(120)
|
||||
await drive_base.straight(25)
|
||||
await right_arm.run_angle(10000, -3000)
|
||||
await drive_base.straight(-110)
|
||||
await drive_base.turn(-43)
|
||||
await right_arm.run_angle(10000, -3000)
|
||||
await drive_base.straight(295)
|
||||
await right_arm.run_angle(10000, 9000)
|
||||
await drive_base.straight(-65)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(175)
|
||||
await drive_base.turn(24.5)
|
||||
await drive_base.straight(-100)
|
||||
await right_arm.run_angle(10000, -8500)
|
||||
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)
|
||||
await right_arm.run_angle(10000, 3500)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(-300)
|
||||
await drive_base.turn(-80)
|
||||
await drive_base.straight(-700)
|
||||
|
||||
|
||||
# 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)
|
||||
await drive_base.straight(750)
|
||||
await drive_base.straight(-650)
|
||||
|
||||
|
||||
# Function to classify color based on HSV
|
||||
@@ -21,7 +21,7 @@ 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
|
||||
WALL_DISTANCE = 300 # mm
|
||||
|
||||
async def drive_forward():
|
||||
"""Drive forward continuously using DriveBase."""
|
||||
@@ -45,49 +45,31 @@ async def monitor_distance():
|
||||
|
||||
# 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)
|
||||
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.turn(15)
|
||||
|
||||
await drive_base.straight(-173)
|
||||
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(120)
|
||||
left_arm.run_angle(1000,-670)
|
||||
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)
|
||||
|
||||
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)
|
||||
@@ -123,62 +105,77 @@ async def Run3(): # tip the scale.py
|
||||
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.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
|
||||
#Solve mission
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000,-4000)
|
||||
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(600)
|
||||
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(120)
|
||||
await drive_base.straight(25)
|
||||
await right_arm.run_angle(10000, -3000)
|
||||
await drive_base.straight(-110)
|
||||
await drive_base.turn(-43)
|
||||
await right_arm.run_angle(10000, -3000)
|
||||
await drive_base.straight(295)
|
||||
await right_arm.run_angle(10000, 9000)
|
||||
await drive_base.straight(-65)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(175)
|
||||
await drive_base.turn(24.5)
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(150)
|
||||
await drive_base.turn(60)
|
||||
await right_arm.run_angle(10000, -8500)
|
||||
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)
|
||||
await right_arm.run_angle(10000, 3500)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(-300)
|
||||
await drive_base.turn(-80)
|
||||
await drive_base.straight(-700)
|
||||
|
||||
|
||||
# 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)
|
||||
await drive_base.straight(750)
|
||||
await drive_base.straight(-650)
|
||||
|
||||
|
||||
# Function to classify color based on HSV
|
||||
def detect_color(h, s, v, reflected):
|
||||
if reflected > 4:
|
||||
@@ -1,221 +0,0 @@
|
||||
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())
|
||||
@@ -1,220 +0,0 @@
|
||||
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(-900)
|
||||
|
||||
|
||||
# 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(-102.5)
|
||||
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, 270)
|
||||
await drive_base.turn(30)
|
||||
await drive_base.straight(-60)
|
||||
|
||||
|
||||
|
||||
# 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())
|
||||
@@ -1,383 +0,0 @@
|
||||
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 = 200 # mm
|
||||
|
||||
async def drive_forward():
|
||||
"""Drive forward continuously using DriveBase."""
|
||||
drive_base.drive(400, 0)
|
||||
|
||||
async def drive_backward():
|
||||
"""Drive forward continuously using DriveBase."""
|
||||
drive_base.drive(400, 0)
|
||||
|
||||
|
||||
async def monitor_distance():
|
||||
"""Monitor ultrasonic sensor and stop when wall is detected."""
|
||||
while True:
|
||||
distance = await lazer_ranger.distance()
|
||||
print('Distancing...',distance)
|
||||
|
||||
if distance < WALL_DISTANCE:
|
||||
# Stop the drivebase
|
||||
drive_base.stop()
|
||||
print(f"Wall detected at {distance}mm!")
|
||||
break
|
||||
if distance is None:
|
||||
continue
|
||||
|
||||
|
||||
# Small delay to prevent overwhelming the sensor
|
||||
await wait(50)
|
||||
|
||||
"""
|
||||
Run#1
|
||||
- Removed forge and who lived here part
|
||||
- What's on sale + Silo
|
||||
- Green Key
|
||||
"""
|
||||
async def Run1():
|
||||
await solve_whats_on_sale()
|
||||
await solve_silo()
|
||||
|
||||
# return to base
|
||||
await drive_base.straight(-90)
|
||||
#await drive_base.turn(-100)
|
||||
await drive_base.arc(200,None,-300)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_whats_on_sale():
|
||||
|
||||
right_arm.run_angle(500,30)
|
||||
left_arm.run_angle(500,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)
|
||||
|
||||
async def solve_silo():
|
||||
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)
|
||||
|
||||
|
||||
"""
|
||||
Run#2
|
||||
- This to solve forge, who lived here and heavy lifting combined
|
||||
- Red Key
|
||||
"""
|
||||
async def Run2():
|
||||
await solve_forge()
|
||||
await solve_heavy_lifting()
|
||||
await solve_who_lived_here()
|
||||
|
||||
# return to base
|
||||
await drive_base.arc(-500,None,600)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_forge():
|
||||
await right_arm.run_target(50,50)
|
||||
# await right_arm.run_angle(50,-30)
|
||||
await drive_base.arc(350,113, None)
|
||||
|
||||
await drive_base.straight(20)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-47)
|
||||
|
||||
async def solve_heavy_lifting():
|
||||
await right_arm.run_angle(500,-160) # arm down
|
||||
await wait(100)
|
||||
await drive_base.turn(20) # turn right a little bit
|
||||
await right_arm.run_angle(500,160) #arm up
|
||||
await drive_base.turn(-20) #reset position
|
||||
|
||||
async def solve_who_lived_here():
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-15)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-25)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(-100)
|
||||
|
||||
"""
|
||||
Run#2.1
|
||||
- Alternate solution for Forge, Who lived here and Heavy Lifting combined
|
||||
- Light Blue Key
|
||||
- Different alignment
|
||||
"""
|
||||
async def Run2_1():
|
||||
await solve_forge_straight()
|
||||
await solve_heavy_lifting()
|
||||
await solve_who_lived_here()
|
||||
|
||||
# return to base
|
||||
await drive_base.arc(-500,None,600)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_forge_straight():
|
||||
await right_arm.run_target(50,50)
|
||||
await right_arm.run_angle(50,-30)
|
||||
await drive_base.straight(700)
|
||||
# await drive_base.turn(-30)
|
||||
# await drive_base.straight(20)
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(-30)
|
||||
|
||||
"""
|
||||
Run#3
|
||||
- Combined angler artifacts and tip the scale
|
||||
- Yellow key
|
||||
"""
|
||||
async def Run3():
|
||||
await solve_angler_artifacts()
|
||||
await solve_tip_the_scale()
|
||||
|
||||
#cross over to red side
|
||||
await multitask(
|
||||
drive_forward(),
|
||||
monitor_distance()
|
||||
)
|
||||
|
||||
async def solve_angler_artifacts():
|
||||
await drive_base.straight(940)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(65)
|
||||
#Solve
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000,-3000)
|
||||
await drive_base.straight(-110)
|
||||
|
||||
await drive_base.turn(90)
|
||||
await drive_base.arc(-150,-103, None)
|
||||
|
||||
async def solve_tip_the_scale():
|
||||
await drive_base.turn(3)
|
||||
await drive_base.straight(142.5)
|
||||
await right_arm.run_angle(800,-150)
|
||||
await right_arm.run_angle(900,150)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(-65)
|
||||
await drive_base.straight(300,Stop.COAST_SMART)
|
||||
await drive_base.arc(10,-47, None)
|
||||
#await drive_base.turn(-23, Stop.COAST_SMART)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
"""
|
||||
Run #4
|
||||
- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal
|
||||
- Blue Key
|
||||
"""
|
||||
async def Run4():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-18)
|
||||
await drive_base.straight(120)
|
||||
await drive_base.straight(-210)
|
||||
await drive_base.turn(61)
|
||||
await drive_base.straight(133)
|
||||
await right_arm.run_angle(400, -200)
|
||||
await drive_base.straight(90)
|
||||
await right_arm.run_angle(100, 95)
|
||||
#await drive_base.straight(-875)
|
||||
#return with arc
|
||||
await drive_base.straight(-600)
|
||||
await drive_base.arc(10,-47, None)
|
||||
await drive_base.straight(-400,Stop.COAST_SMART)
|
||||
|
||||
async def solve_brush_reveal():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(110)
|
||||
await drive_base.straight(-210)
|
||||
|
||||
async def solve_mineshaft_explorer():
|
||||
await drive_base.turn(63)
|
||||
await drive_base.straight(130)
|
||||
await right_arm.run_angle(1000, -90)
|
||||
await drive_base.straight(84)
|
||||
await right_arm.run_angle(300, 90)
|
||||
|
||||
"""
|
||||
Run#5
|
||||
- Solves Salvage Operation + Statue Rebuild
|
||||
- Orange Key
|
||||
"""
|
||||
async def Run5():
|
||||
await drive_base.straight(550)
|
||||
await right_arm.run_angle(300,100)
|
||||
await drive_base.straight(-75)
|
||||
await right_arm.run_angle(300, -100)
|
||||
await drive_base.straight(300)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(-15)
|
||||
#solving statue
|
||||
await drive_base.straight(350)
|
||||
await drive_base.turn(-104)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, -900)
|
||||
await drive_base.straight(120)
|
||||
await drive_base.turn(5)
|
||||
await left_arm.run_angle(500, 290)
|
||||
await drive_base.turn(18)
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(-90)
|
||||
await drive_base.straight(900)
|
||||
drive_base.stop()
|
||||
|
||||
|
||||
async def solve_salvage_operation():
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(300,500)
|
||||
await drive_base.straight(-75)
|
||||
await right_arm.run_angle(300, -900)
|
||||
await drive_base.straight(-350)
|
||||
await wait(1000)
|
||||
await drive_base.straight(800)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(-15)
|
||||
await drive_base.straight(350)
|
||||
|
||||
async def solve_statue_rebuild():
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, -900)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(700, 250)
|
||||
await drive_base.turn(30)
|
||||
|
||||
"""
|
||||
Run#6
|
||||
- Solve 2/3 Site Markings
|
||||
- Run only if have time
|
||||
- Purple Key
|
||||
"""
|
||||
async def Run6_7(): # experiment with ferris wheel for Site Markings
|
||||
#solve_site_mark_1()
|
||||
#solve_site_mark_2()
|
||||
await drive_base.straight(640)
|
||||
await drive_base.straight(-150)
|
||||
await drive_base.turn(-8)
|
||||
await left_arm.run_angle(500, -520)
|
||||
await drive_base.straight(300)
|
||||
await wait(50)
|
||||
await left_arm.run_angle(500, 700)
|
||||
#return to base
|
||||
await drive_base.straight(-300)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_site_mark_1():
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(100, -10)
|
||||
await wait(50)
|
||||
await drive_base.straight(-300)
|
||||
await drive_base.arc(-150, -140, None)
|
||||
|
||||
async def solve_site_mark_2():
|
||||
await drive_base.straight(-300)
|
||||
await wait(50)
|
||||
await right_arm.run_angle(50, 50)
|
||||
|
||||
|
||||
|
||||
# Function to classify color based on HSV
|
||||
def detect_color(h, s, v, reflected):
|
||||
if reflected > 4:
|
||||
if h < 4 or h > 350: # red
|
||||
return "Red"
|
||||
elif 3 < h < 40 and s > 70: # orange
|
||||
return "Orange"
|
||||
elif 47 < h < 56: # yellow
|
||||
return "Yellow"
|
||||
elif 70 < h < 160: # green - do it vertically not horizontally for accuracy
|
||||
return "Green"
|
||||
elif 195 < h < 198: # light blue
|
||||
return "Light_Blue"
|
||||
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
|
||||
return "Blue"
|
||||
elif 260 < h < 350: # purple
|
||||
return "Purple"
|
||||
|
||||
else:
|
||||
return "Unknown"
|
||||
return "Unknown"
|
||||
|
||||
|
||||
async def main():
|
||||
while True:
|
||||
h, s, v = await color_sensor.hsv()
|
||||
print(color_sensor.color())
|
||||
print(h,s,v)
|
||||
reflected = await color_sensor.reflection()
|
||||
color = detect_color(h, s, v, reflected)
|
||||
print(color)
|
||||
|
||||
|
||||
if color == "Green":
|
||||
print('Running Mission 1')
|
||||
await Run1()
|
||||
elif color == "Red":
|
||||
print('Running Mission 2')
|
||||
await Run2()
|
||||
elif color == "Yellow":
|
||||
print('Running Mission 3')
|
||||
await Run3()
|
||||
elif color == "Blue":
|
||||
print('Running Mission 4')
|
||||
await Run4()
|
||||
elif color == "Orange":
|
||||
print('Running Mission 5')
|
||||
await Run5()
|
||||
elif color == "Purple":
|
||||
print('Running Mission 6_7')
|
||||
await Run6_7()
|
||||
elif color == "Light_Blue":
|
||||
print("Running Mission 2_1")
|
||||
await Run2_1()
|
||||
else:
|
||||
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||
await wait(10)
|
||||
# Run the main function
|
||||
run_task(main())
|
||||
@@ -1,513 +0,0 @@
|
||||
#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge.
|
||||
import umath
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.tools import run_task, multitask
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.hubs import PrimeHub
|
||||
|
||||
# Initialize hub and devices
|
||||
hub = PrimeHub()
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
|
||||
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
|
||||
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
|
||||
lazer_ranger = UltrasonicSensor(Port.E)
|
||||
color_sensor = ColorSensor(Port.F)
|
||||
|
||||
# DriveBase configuration
|
||||
WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels)
|
||||
AXLE_TRACK = 180 # mm (distance between wheels)
|
||||
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||
drive_base.settings(600, 500, 300, 200)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
"""
|
||||
Debugging helps
|
||||
"""
|
||||
DEBUG = 1 # Enable when you want to show logs
|
||||
# Example conversion function (adjust min/max values as needed for your hub)
|
||||
async def get_battery_percentage(voltage_mV:float):
|
||||
max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us
|
||||
min_voltage = 5000.0 # min battery level
|
||||
percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100
|
||||
return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100
|
||||
|
||||
async def wait_button_release():
|
||||
"""Wait for all buttons to be released"""
|
||||
while hub.buttons.pressed():
|
||||
await wait(500)
|
||||
await wait(1000) # Debounce delay
|
||||
|
||||
WALL_DISTANCE = 200 # mm
|
||||
async def drive_forward():
|
||||
"""Drive forward continuously using DriveBase."""
|
||||
drive_base.drive(1000,0)
|
||||
|
||||
async def drive_backward():
|
||||
"""Drive forward continuously using DriveBase."""
|
||||
drive_base.drive(400, 0)
|
||||
|
||||
|
||||
async def monitor_distance():
|
||||
"""Monitor ultrasonic sensor and stop when wall is detected."""
|
||||
while True:
|
||||
distance = await lazer_ranger.distance()
|
||||
print('Distancing...',distance)
|
||||
|
||||
if distance < WALL_DISTANCE:
|
||||
# Stop the drivebase
|
||||
drive_base.stop()
|
||||
print(f"Wall detected at {distance}mm!")
|
||||
break
|
||||
if distance is None:
|
||||
continue
|
||||
|
||||
# Small delay to prevent overwhelming the sensor
|
||||
await wait(50)
|
||||
|
||||
# Use this to set default
|
||||
def set_default_speed():
|
||||
drive_base.settings(600, 500, 300, 200)
|
||||
|
||||
# Use this to change drive base movement
|
||||
def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
|
||||
drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
|
||||
|
||||
"""
|
||||
Run#1
|
||||
- Removed forge and who lived here part
|
||||
- What's on sale + Silo
|
||||
- Green Key
|
||||
"""
|
||||
async def Run1():
|
||||
|
||||
# Fast approach to near-stall position
|
||||
await left_arm.run_angle(2000, 180) # Fast movement upward
|
||||
|
||||
# Gentle stall detection (shorter distance = faster)
|
||||
await left_arm.run_until_stalled(1500, duty_limit=15)
|
||||
left_arm.reset_angle(0)
|
||||
print(f"Initial left arm angle : {left_arm.angle()}")
|
||||
|
||||
await solve_whats_on_sale_v2()
|
||||
await solve_silo()
|
||||
|
||||
# return to base
|
||||
await drive_base.straight(-90)
|
||||
#await drive_base.turn(-100)
|
||||
await drive_base.arc(200,None,-300)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_whats_on_sale():
|
||||
|
||||
right_arm.run_angle(500,30)
|
||||
|
||||
#Automated inconsistency
|
||||
#left_arm.run_angle(500,-119.5)
|
||||
left_arm.run_target(500,-121.5, Stop.HOLD)
|
||||
print(f"Position left arm angle : {left_arm.angle()}")
|
||||
|
||||
await drive_base.straight(180)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(335)
|
||||
await left_arm.run_angle(500,-20)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(60)
|
||||
await left_arm.run_angle(500,50)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
left_arm.run_angle(500,-50)
|
||||
await drive_base.turn(-20)
|
||||
left_arm.run_angle(1000,180)
|
||||
await drive_base.turn(15)
|
||||
|
||||
async def solve_whats_on_sale_v2():
|
||||
|
||||
right_arm.run_angle(500,30)
|
||||
|
||||
# Bring down left arm to position
|
||||
await left_arm.run_angle(2000, -120)
|
||||
#await left_arm.run_until_stalled(-500,duty_limit=15)
|
||||
print(f"Position left arm angle : {left_arm.angle()}")
|
||||
left_arm.reset_angle(0)
|
||||
|
||||
await drive_base.straight(180)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(335)
|
||||
await left_arm.run_angle(500,-20)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(60)
|
||||
await left_arm.run_angle(500,50)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
left_arm.run_angle(500,-50)
|
||||
await drive_base.turn(-20)
|
||||
left_arm.run_angle(1000,180)
|
||||
await drive_base.turn(15)
|
||||
|
||||
async def solve_silo():
|
||||
await drive_base.straight(-80)
|
||||
await drive_base.turn(42)
|
||||
await drive_base.straight(95)
|
||||
|
||||
SPEED = 10000 # speed in degree per second
|
||||
SWING_ANGLE = 60 # the angle!
|
||||
REBOUND_ADJ = 20
|
||||
|
||||
# Repeat this motion 4 times
|
||||
for _ in range(4):
|
||||
await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up
|
||||
await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down
|
||||
|
||||
|
||||
right_arm.run_angle(4000,60, Stop.HOLD)
|
||||
|
||||
|
||||
"""
|
||||
Run#2
|
||||
- This to solve forge, who lived here and heavy lifting combined
|
||||
- Red Key
|
||||
"""
|
||||
async def Run2():
|
||||
await solve_forge()
|
||||
await solve_heavy_lifting()
|
||||
await solve_who_lived_here()
|
||||
|
||||
# return to base
|
||||
await drive_base.arc(-500,None,600)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_forge():
|
||||
await right_arm.run_target(50,50)
|
||||
# await right_arm.run_angle(50,-30)
|
||||
await drive_base.arc(350,113, None)
|
||||
|
||||
await drive_base.straight(20)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-47)
|
||||
|
||||
async def solve_heavy_lifting():
|
||||
await right_arm.run_angle(2000,-160) # arm down
|
||||
await wait(100)
|
||||
await drive_base.turn(30) # turn right a little bit
|
||||
await right_arm.run_angle(2000,160) #arm up
|
||||
await drive_base.turn(-30) #reset position
|
||||
|
||||
async def solve_who_lived_here():
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-15)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-25)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(50)
|
||||
right_arm.run_angle(1000,-160)
|
||||
await drive_base.turn(-60)
|
||||
await right_arm.run_angle(2000,160)
|
||||
|
||||
"""
|
||||
Run#2.1
|
||||
- Alternate solution for Forge, Who lived here and Heavy Lifting combined
|
||||
- Light Blue Key
|
||||
- Different alignment
|
||||
"""
|
||||
async def Run2_1():
|
||||
await solve_forge_straight()
|
||||
await solve_heavy_lifting()
|
||||
await solve_who_lived_here()
|
||||
|
||||
# return to base
|
||||
await drive_base.arc(-500,None,600)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_forge_straight():
|
||||
await right_arm.run_target(50,50)
|
||||
await right_arm.run_angle(50,-30)
|
||||
await drive_base.straight(700)
|
||||
# await drive_base.turn(-30)
|
||||
# await drive_base.straight(20)
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(-30)
|
||||
|
||||
"""
|
||||
Run#3
|
||||
- Combined angler artifacts and tip the scale
|
||||
- Yellow key
|
||||
"""
|
||||
async def Run3():
|
||||
await solve_angler_artifacts()
|
||||
await solve_tip_the_scale()
|
||||
|
||||
#cross over to red side
|
||||
await multitask(
|
||||
drive_forward(),
|
||||
monitor_distance()
|
||||
)
|
||||
|
||||
async def solve_angler_artifacts():
|
||||
await drive_base.straight(870)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(45)
|
||||
#Solve
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000,-750)
|
||||
await drive_base.straight(-130)
|
||||
|
||||
await drive_base.turn(67)
|
||||
|
||||
async def solve_tip_the_scale():
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.straight(60)
|
||||
await drive_base.turn(22)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
"""
|
||||
Run #4
|
||||
- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal
|
||||
- Blue Key
|
||||
"""
|
||||
async def Run4():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-18)
|
||||
await drive_base.straight(120)
|
||||
await drive_base.straight(-210)
|
||||
await drive_base.turn(61)
|
||||
await drive_base.straight(133)
|
||||
await right_arm.run_angle(400, -200)
|
||||
await drive_base.straight(90)
|
||||
await right_arm.run_angle(100, 95)
|
||||
await drive_base.straight(-875)
|
||||
|
||||
async def solve_brush_reveal():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(110)
|
||||
await drive_base.straight(-210)
|
||||
|
||||
async def solve_mineshaft_explorer():
|
||||
await drive_base.turn(63)
|
||||
await drive_base.straight(130)
|
||||
await right_arm.run_angle(1000, -90)
|
||||
await drive_base.straight(84)
|
||||
await right_arm.run_angle(300, 90)
|
||||
|
||||
"""
|
||||
Run#5
|
||||
- Solves Salvage Operation + Statue Rebuild
|
||||
- Orange Key
|
||||
"""
|
||||
async def Run5():
|
||||
# Getting the sand down
|
||||
await drive_base.straight(550)
|
||||
await right_arm.run_angle(300,100)
|
||||
await drive_base.straight(-75)
|
||||
await right_arm.run_angle(300, -100)
|
||||
# Shoving the boat into place
|
||||
await drive_base.straight(300)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(-15)
|
||||
# Solving statue
|
||||
await drive_base.straight(350)
|
||||
await drive_base.turn(-104)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, -300)
|
||||
await drive_base.straight(120)
|
||||
await drive_base.turn(5)
|
||||
# Lift up statue
|
||||
await left_arm.run_angle(500, 100, Stop.HOLD)
|
||||
await drive_base.turn(18)
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(-90)
|
||||
await drive_base.straight(900)
|
||||
drive_base.stop()
|
||||
|
||||
|
||||
async def solve_salvage_operation():
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(300,500)
|
||||
await drive_base.straight(-75)
|
||||
await right_arm.run_angle(300, -900)
|
||||
await drive_base.straight(-350)
|
||||
await wait(1000)
|
||||
await drive_base.straight(800)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(-15)
|
||||
await drive_base.straight(350)
|
||||
|
||||
async def solve_statue_rebuild():
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, -900)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(700,200)
|
||||
await drive_base.turn(30)
|
||||
|
||||
"""
|
||||
Run#6
|
||||
- Solve 2/3 Site Markings
|
||||
- Run only if have time
|
||||
- Purple Key
|
||||
"""
|
||||
async def Run6_7(): # experiment with ferris wheel for Site Markings
|
||||
solve_site_mark_1()
|
||||
solve_site_mark_2()
|
||||
#return to base
|
||||
await drive_base.straight(-300)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_site_mark_1():
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(100, -10)
|
||||
await wait(50)
|
||||
await drive_base.straight(-300)
|
||||
await drive_base.arc(-150, -140, None)
|
||||
|
||||
async def solve_site_mark_2():
|
||||
await drive_base.straight(-300)
|
||||
await wait(50)
|
||||
await right_arm.run_angle(50, 50)
|
||||
|
||||
async def Run10(): # experimental map reveal attachment
|
||||
|
||||
await drive_base.straight(600)
|
||||
drive_base.settings(150, 750, 50, 500)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(260)
|
||||
left_arm.run_angle(300,218)
|
||||
|
||||
set_default_speed()
|
||||
await drive_base.straight(-80)
|
||||
await drive_base.turn(30)
|
||||
await drive_base.straight(-300)
|
||||
await drive_base.straight(400)
|
||||
#await left_arm.run_angle(50,120)
|
||||
await drive_base.straight(-200)
|
||||
await left_arm.run_angle(300,-215)
|
||||
await drive_base.straight(-600)
|
||||
drive_base.stop()
|
||||
|
||||
async def Run11(): # experimental surface brushing attachment
|
||||
|
||||
await drive_base.straight(600)
|
||||
drive_base.settings(150, 750, 50, 500)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(250)
|
||||
#left_arm.run_angle(300,218)
|
||||
|
||||
set_default_speed()
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(30)
|
||||
await drive_base.straight(-190)
|
||||
#await drive_base.straight(400)
|
||||
#await left_arm.run_angle(50,120)
|
||||
#await drive_base.straight(-200)
|
||||
await left_arm.run_angle(300,-300)
|
||||
await drive_base.straight(-600)
|
||||
drive_base.stop()
|
||||
|
||||
async def Run12():
|
||||
await drive_base.straight(900)
|
||||
await drive_base.turn(83)
|
||||
await left_arm.run_angle(3000, -300)
|
||||
await right_arm.run_angle(1100, -180)
|
||||
drive_base.settings(150, 50, 50, 50)
|
||||
await drive_base.straight(120)
|
||||
left_arm.reset_angle(0)
|
||||
await left_arm.run_angle(50, 50)
|
||||
await right_arm.run_angle(50, 90)
|
||||
await drive_base.straight(-100)
|
||||
drive_base.settings(950, 750, 750, 750)
|
||||
await drive_base.turn(110)
|
||||
await drive_base.straight(1000)
|
||||
|
||||
|
||||
# Function to classify color based on HSV
|
||||
def detect_color(h, s, v, reflected):
|
||||
if reflected > 4:
|
||||
if h < 4 or h > 350: # red
|
||||
return "Red"
|
||||
elif 3 < h < 40 and s > 70: # orange
|
||||
return "Orange"
|
||||
elif 47 < h < 56: # yellow
|
||||
return "Yellow"
|
||||
elif 70 < h < 160: # green - do it vertically not horizontally for accuracy
|
||||
return "Green"
|
||||
elif 195 < h < 198: # light blue
|
||||
return "Light_Blue"
|
||||
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
|
||||
return "Blue"
|
||||
elif 260 < h < 350: # purple
|
||||
return "Purple"
|
||||
|
||||
else:
|
||||
return "Unknown"
|
||||
return "Unknown"
|
||||
|
||||
|
||||
async def main():
|
||||
while True:
|
||||
pressed = hub.buttons.pressed()
|
||||
h, s, v = await color_sensor.hsv()
|
||||
reflected = await color_sensor.reflection()
|
||||
color = detect_color(h, s, v, reflected)
|
||||
if DEBUG :
|
||||
#print(color_sensor.color())
|
||||
#print(h,s,v)
|
||||
#print(color)
|
||||
print(f"button pressed: {pressed}")
|
||||
|
||||
|
||||
if color == "Green":
|
||||
print('Running Mission 1')
|
||||
await Run1()
|
||||
elif color == "Red":
|
||||
print('Running Mission 2')
|
||||
await Run2()
|
||||
elif color == "Yellow":
|
||||
print('Running Mission 3')
|
||||
await Run3()
|
||||
elif color == "Blue":
|
||||
print('Running Mission 4')
|
||||
await Run4()
|
||||
elif color == "Orange":
|
||||
print('Running Mission 5')
|
||||
await Run5()
|
||||
elif color == "Purple":
|
||||
print('Running Mission 6')
|
||||
await Run11()
|
||||
elif color == "Light_Blue":
|
||||
print("Running Mission 2_1")
|
||||
await Run12()
|
||||
else:
|
||||
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||
#pass
|
||||
|
||||
# Show battery % for debugging
|
||||
if Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor
|
||||
# Get the battery voltage in millivolts (mV)
|
||||
battery_voltage_mV = hub.battery.voltage()
|
||||
# Use the function with your voltage reading
|
||||
percentage = await get_battery_percentage(float(battery_voltage_mV))
|
||||
if DEBUG:
|
||||
print(f"Battery voltage: {battery_voltage_mV} mV")
|
||||
print(f"Battery level: {percentage:.3f}%")
|
||||
print("FLL Robot System Ready!")
|
||||
await hub.display.text(f"{percentage:.0f}")
|
||||
break
|
||||
elif pressed == None:
|
||||
continue
|
||||
|
||||
await wait(10)
|
||||
# Run the main function
|
||||
run_task(main())
|
||||
@@ -1,16 +0,0 @@
|
||||
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()
|
||||
@@ -1,16 +0,0 @@
|
||||
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(-80)
|
||||
await drive_base.turn(30)
|
||||
await drive_base.straight(-180)
|
||||
#await drive_base.straight(400)
|
||||
#await left_arm.run_angle(50,120)
|
||||
#await drive_base.straight(-200)
|
||||
await left_arm.run_angle(300,-300)
|
||||
await drive_base.straight(-600)
|
||||
drive_base.stop()
|
||||
@@ -1,513 +0,0 @@
|
||||
#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 = 0 # Enable when you want to show logs
|
||||
# Example conversion function (adjust min/max values as needed for your hub)
|
||||
async def get_battery_percentage(voltage_mV:float):
|
||||
max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us
|
||||
min_voltage = 5000.0 # min battery level
|
||||
percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100
|
||||
return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100
|
||||
|
||||
async def wait_button_release():
|
||||
"""Wait for all buttons to be released"""
|
||||
while hub.buttons.pressed():
|
||||
await wait(500)
|
||||
await wait(1000) # Debounce delay
|
||||
|
||||
WALL_DISTANCE = 200 # mm
|
||||
async def drive_forward():
|
||||
"""Drive forward continuously using DriveBase."""
|
||||
drive_base.drive(1000,0)
|
||||
|
||||
async def drive_backward():
|
||||
"""Drive forward continuously using DriveBase."""
|
||||
drive_base.drive(400, 0)
|
||||
|
||||
|
||||
async def monitor_distance():
|
||||
"""Monitor ultrasonic sensor and stop when wall is detected."""
|
||||
while True:
|
||||
distance = await lazer_ranger.distance()
|
||||
print('Distancing...',distance)
|
||||
|
||||
if distance < WALL_DISTANCE:
|
||||
# Stop the drivebase
|
||||
drive_base.stop()
|
||||
print(f"Wall detected at {distance}mm!")
|
||||
break
|
||||
if distance is None:
|
||||
continue
|
||||
|
||||
# Small delay to prevent overwhelming the sensor
|
||||
await wait(50)
|
||||
|
||||
# Use this to set default
|
||||
def set_default_speed():
|
||||
drive_base.settings(600, 500, 300, 200)
|
||||
|
||||
# Use this to change drive base movement
|
||||
def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
|
||||
drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
|
||||
|
||||
"""
|
||||
Run#1
|
||||
- Removed forge and who lived here part
|
||||
- What's on sale + Silo
|
||||
- Green Key
|
||||
"""
|
||||
async def Run1():
|
||||
|
||||
# Fast approach to near-stall position
|
||||
await left_arm.run_angle(2000, -190) # 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)
|
||||
left_arm.run_target(500,70, Stop.HOLD)
|
||||
print(f"Position left arm angle : {left_arm.angle()}")
|
||||
|
||||
await drive_base.straight(180)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(335)
|
||||
await left_arm.run_angle(500,-20)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(60)
|
||||
await left_arm.run_angle(500,50)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
left_arm.run_angle(500,-50)
|
||||
await drive_base.turn(-20)
|
||||
left_arm.run_angle(1000,180)
|
||||
await drive_base.turn(15)
|
||||
|
||||
async def solve_whats_on_sale_v2():
|
||||
|
||||
right_arm.run_angle(500,30)
|
||||
|
||||
# Bring down left arm to position
|
||||
await left_arm.run_angle(2000, -120)
|
||||
#await left_arm.run_until_stalled(-500,duty_limit=15)
|
||||
print(f"Position left arm angle : {left_arm.angle()}")
|
||||
left_arm.reset_angle(0)
|
||||
|
||||
await drive_base.straight(180)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(335)
|
||||
await left_arm.run_angle(500,-20)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(60)
|
||||
await left_arm.run_angle(500,50)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
left_arm.run_angle(500,-50)
|
||||
await drive_base.turn(-20)
|
||||
left_arm.run_angle(1000,180)
|
||||
await drive_base.turn(15)
|
||||
|
||||
async def solve_silo():
|
||||
await drive_base.straight(-80)
|
||||
await drive_base.turn(42)
|
||||
await drive_base.straight(95)
|
||||
|
||||
SPEED = 10000 # speed in degree per second
|
||||
SWING_ANGLE = 60 # the angle!
|
||||
REBOUND_ADJ = 20
|
||||
|
||||
# Repeat this motion 4 times
|
||||
for _ in range(4):
|
||||
await right_arm.run_angle(SPEED,SWING_ANGLE, Stop.HOLD) # Swing up
|
||||
await right_arm.run_angle(SPEED,(-1 * SWING_ANGLE),Stop.HOLD) # Swing down
|
||||
|
||||
|
||||
right_arm.run_angle(4000,60, Stop.HOLD)
|
||||
|
||||
|
||||
"""
|
||||
Run#2
|
||||
- This to solve forge, who lived here and heavy lifting combined
|
||||
- Red Key
|
||||
"""
|
||||
async def Run2():
|
||||
await solve_forge()
|
||||
await solve_heavy_lifting()
|
||||
await solve_who_lived_here()
|
||||
|
||||
# return to base
|
||||
await drive_base.arc(-500,None,600)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_forge():
|
||||
await right_arm.run_target(50,50)
|
||||
# await right_arm.run_angle(50,-30)
|
||||
await drive_base.arc(350,113, None)
|
||||
|
||||
await drive_base.straight(20)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-47)
|
||||
|
||||
async def solve_heavy_lifting():
|
||||
await right_arm.run_angle(2000,-160) # arm down
|
||||
await wait(100)
|
||||
await drive_base.turn(30) # turn right a little bit
|
||||
await right_arm.run_angle(2000,160) #arm up
|
||||
await drive_base.turn(-30) #reset position
|
||||
|
||||
async def solve_who_lived_here():
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-15)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-25)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(50)
|
||||
right_arm.run_angle(1000,-160)
|
||||
await drive_base.turn(-60)
|
||||
await right_arm.run_angle(2000,160)
|
||||
|
||||
"""
|
||||
Run#2.1
|
||||
- Alternate solution for Forge, Who lived here and Heavy Lifting combined
|
||||
- Light Blue Key
|
||||
- Different alignment
|
||||
"""
|
||||
async def Run2_1():
|
||||
await solve_forge_straight()
|
||||
await solve_heavy_lifting()
|
||||
await solve_who_lived_here()
|
||||
|
||||
# return to base
|
||||
await drive_base.arc(-500,None,600)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_forge_straight():
|
||||
await right_arm.run_target(50,50)
|
||||
await right_arm.run_angle(50,-30)
|
||||
await drive_base.straight(700)
|
||||
# await drive_base.turn(-30)
|
||||
# await drive_base.straight(20)
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(-30)
|
||||
|
||||
"""
|
||||
Run#3
|
||||
- Combined angler artifacts and tip the scale
|
||||
- Yellow key
|
||||
"""
|
||||
async def Run3():
|
||||
await solve_angler_artifacts()
|
||||
await solve_tip_the_scale()
|
||||
|
||||
#cross over to red side
|
||||
await multitask(
|
||||
drive_forward(),
|
||||
monitor_distance()
|
||||
)
|
||||
|
||||
async def solve_angler_artifacts():
|
||||
await drive_base.straight(870)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(45)
|
||||
#Solve
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000,-750)
|
||||
await drive_base.straight(-130)
|
||||
|
||||
await drive_base.turn(67)
|
||||
|
||||
async def solve_tip_the_scale():
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.straight(60)
|
||||
await drive_base.turn(22)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
"""
|
||||
Run #4
|
||||
- Solves the Mineshaft explorer + 2/3 Surface Brush + 1/3 Map Reveal
|
||||
- Blue Key
|
||||
"""
|
||||
async def Run4():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-18)
|
||||
await drive_base.straight(120)
|
||||
await drive_base.straight(-210)
|
||||
await drive_base.turn(61)
|
||||
await drive_base.straight(133)
|
||||
await right_arm.run_angle(400, -200)
|
||||
await drive_base.straight(90)
|
||||
await right_arm.run_angle(100, 95)
|
||||
await drive_base.straight(-875)
|
||||
|
||||
async def solve_brush_reveal():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(110)
|
||||
await drive_base.straight(-210)
|
||||
|
||||
async def solve_mineshaft_explorer():
|
||||
await drive_base.turn(63)
|
||||
await drive_base.straight(130)
|
||||
await right_arm.run_angle(1000, -90)
|
||||
await drive_base.straight(84)
|
||||
await right_arm.run_angle(300, 90)
|
||||
|
||||
"""
|
||||
Run#5
|
||||
- Solves Salvage Operation + Statue Rebuild
|
||||
- Orange Key
|
||||
"""
|
||||
async def Run5():
|
||||
# Getting the sand down
|
||||
await drive_base.straight(550)
|
||||
await right_arm.run_angle(300,100)
|
||||
await drive_base.straight(-75)
|
||||
await right_arm.run_angle(300, -100)
|
||||
# Shoving the boat into place
|
||||
await drive_base.straight(300)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(-15)
|
||||
# Solving statue
|
||||
await drive_base.straight(350)
|
||||
await drive_base.turn(-104)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, -300)
|
||||
await drive_base.straight(120)
|
||||
await drive_base.turn(5)
|
||||
# Lift up statue
|
||||
await left_arm.run_angle(500, 100, Stop.HOLD)
|
||||
await drive_base.turn(18)
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(-90)
|
||||
await drive_base.straight(900)
|
||||
drive_base.stop()
|
||||
|
||||
|
||||
async def solve_salvage_operation():
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(300,500)
|
||||
await drive_base.straight(-75)
|
||||
await right_arm.run_angle(300, -900)
|
||||
await drive_base.straight(-350)
|
||||
await wait(1000)
|
||||
await drive_base.straight(800)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(-15)
|
||||
await drive_base.straight(350)
|
||||
|
||||
async def solve_statue_rebuild():
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, -900)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(700,200)
|
||||
await drive_base.turn(30)
|
||||
|
||||
"""
|
||||
Run#6
|
||||
- Solve 2/3 Site Markings
|
||||
- Run only if have time
|
||||
- Purple Key
|
||||
"""
|
||||
async def Run6_7(): # experiment with ferris wheel for Site Markings
|
||||
solve_site_mark_1()
|
||||
solve_site_mark_2()
|
||||
#return to base
|
||||
await drive_base.straight(-300)
|
||||
drive_base.stop()
|
||||
|
||||
async def solve_site_mark_1():
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(100, -10)
|
||||
await wait(50)
|
||||
await drive_base.straight(-300)
|
||||
await drive_base.arc(-150, -140, None)
|
||||
|
||||
async def solve_site_mark_2():
|
||||
await drive_base.straight(-300)
|
||||
await wait(50)
|
||||
await right_arm.run_angle(50, 50)
|
||||
|
||||
async def Run10(): # experimental map reveal attachment
|
||||
|
||||
await drive_base.straight(600)
|
||||
drive_base.settings(150, 750, 50, 500)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(260)
|
||||
left_arm.run_angle(300,218)
|
||||
|
||||
set_default_speed()
|
||||
await drive_base.straight(-80)
|
||||
await drive_base.turn(30)
|
||||
await drive_base.straight(-300)
|
||||
await drive_base.straight(400)
|
||||
#await left_arm.run_angle(50,120)
|
||||
await drive_base.straight(-200)
|
||||
await left_arm.run_angle(300,-215)
|
||||
await drive_base.straight(-600)
|
||||
drive_base.stop()
|
||||
|
||||
async def Run11(): # experimental surface brushing attachment
|
||||
|
||||
await drive_base.straight(600)
|
||||
drive_base.settings(150, 750, 50, 500)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(250)
|
||||
#left_arm.run_angle(300,218)
|
||||
|
||||
set_default_speed()
|
||||
await drive_base.straight(-230)
|
||||
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(-80)
|
||||
#await left_arm.run_angle(300, -75)
|
||||
await drive_base.arc(-650,None,-800)
|
||||
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())
|
||||
40
config.py
Normal file
40
config.py
Normal file
@@ -0,0 +1,40 @@
|
||||
# config.py - Robot configuration shared across all modules
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor
|
||||
from pybricks.parameters import Port
|
||||
|
||||
# Initialize hub
|
||||
hub = PrimeHub()
|
||||
|
||||
# Robot hardware configuration
|
||||
ROBOT_CONFIG = {
|
||||
# Drive motors
|
||||
'left_motor': Motor(Port.A),
|
||||
'right_motor': Motor(Port.B),
|
||||
|
||||
# Attachment motors
|
||||
'attachment_motor': Motor(Port.C),
|
||||
'lift_motor': Motor(Port.D),
|
||||
|
||||
# Sensors
|
||||
#'color_left': ColorSensor(Port.E),
|
||||
'color_back': ColorSensor(Port.F),
|
||||
'ultrasonic': UltrasonicSensor(Port.E),
|
||||
|
||||
# Hub reference
|
||||
'hub': hub
|
||||
}
|
||||
|
||||
# Speed and distance constants
|
||||
SPEEDS = {
|
||||
'FAST': 400,
|
||||
'NORMAL': 250,
|
||||
'SLOW': 100,
|
||||
'PRECISE': 50
|
||||
}
|
||||
|
||||
DISTANCES = {
|
||||
'TILE_SIZE': 300, # mm per field tile
|
||||
'ROBOT_LENGTH': 180, # mm
|
||||
'ROBOT_WIDTH': 140 # mm
|
||||
}
|
||||
@@ -1,3 +1,4 @@
|
||||
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor
|
||||
from pybricks.parameters import Port, Stop, Color, Direction
|
||||
@@ -1,3 +1,4 @@
|
||||
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor
|
||||
from pybricks.parameters import Port, Stop, Color, Direction
|
||||
@@ -2,7 +2,7 @@ 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, run_task, multitask
|
||||
from pybricks.tools import wait, StopWatch, run_task
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
@@ -23,15 +23,23 @@ drive_base.settings(300,1000,300,750)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-17)
|
||||
await drive_base.straight(100)
|
||||
await drive_base.straight(-205)
|
||||
await drive_base.turn(62)
|
||||
await drive_base.straight(125)
|
||||
await arm_motor.run_angle(1000, -1200)
|
||||
await drive_base.straight(87)
|
||||
await arm_motor.run_angle(300, 1200)
|
||||
await drive_base.straight(-875)
|
||||
await drive_base.straight(519)
|
||||
await arm_motor_left.run_angle(300, -100)
|
||||
await arm_motor_left.run_angle(300, 500)
|
||||
await drive_base.straight(180)
|
||||
await drive_base.turn(-37)
|
||||
await drive_base.straight(50)
|
||||
await arm_motor.run_angle(300, -400)
|
||||
await drive_base.straight(-150)
|
||||
await drive_base.turn(135)
|
||||
await drive_base.straight(50)
|
||||
await arm_motor.run_angle(300, 400)
|
||||
await drive_base.straight(-75)
|
||||
await arm_motor.run_angle(300, 300)
|
||||
await drive_base.turn(-50)
|
||||
await drive_base.straight(162)
|
||||
await arm_motor.run_angle(100, -200)
|
||||
await drive_base.straight(30)
|
||||
await arm_motor.run_angle(50,-500)
|
||||
|
||||
run_task(main())
|
||||
@@ -2,36 +2,26 @@ 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, run_task, multitask
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.tools import run_task,multitask
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
# Initialize both motors. In this example, the motor on the
|
||||
# left must turn counterclockwise to make the robot go forward.
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||
right_arm = Motor(Port.D)
|
||||
|
||||
arm_motor = Motor(Port.D, Direction.CLOCKWISE)
|
||||
arm_motor_left= Motor(Port.C, Direction.CLOCKWISE)
|
||||
# Initialize the drive base. In this example, the wheel diameter is 56mm.
|
||||
# The distance between the two wheel-ground contact points is 112mm.
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||
|
||||
print('The default settings are: ' + str(drive_base.settings()))
|
||||
drive_base.settings(300,1000,300,750)
|
||||
# Optionally, uncomment the line below to use the gyro for improved accuracy.
|
||||
drive_base.settings(550,700,100,100)
|
||||
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-17)
|
||||
await drive_base.straight(100)
|
||||
await drive_base.straight(-205)
|
||||
await drive_base.turn(62)
|
||||
await drive_base.straight(125)
|
||||
await arm_motor.run_angle(1000, -1200)
|
||||
await drive_base.straight(87)
|
||||
await arm_motor.run_angle(300, 1200)
|
||||
await drive_base.straight(-875)
|
||||
first_run = False
|
||||
|
||||
async def main():
|
||||
await drive_base.straight(750)
|
||||
await drive_base.straight(-650)
|
||||
|
||||
run_task(main())
|
||||
27
missions/Boat_mission.py
Normal file
27
missions/Boat_mission.py
Normal file
@@ -0,0 +1,27 @@
|
||||
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(880,850,700,700)
|
||||
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
first_run = True
|
||||
|
||||
async def main():
|
||||
await drive_base.straight(750)
|
||||
await drive_base.straight(-650)
|
||||
|
||||
run_task(main())
|
||||
37
missions/Heavy lifting.py
Normal file
37
missions/Heavy lifting.py
Normal file
@@ -0,0 +1,37 @@
|
||||
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 right_arm.run_angle(2000,1000)
|
||||
|
||||
await drive_base.straight(200)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(525)
|
||||
await drive_base.turn(60)
|
||||
await drive_base.straight(30)
|
||||
|
||||
await right_arm.run_angle(2000,-1000)
|
||||
await drive_base.straight(30)
|
||||
await right_arm.run_angle(3000,1000)
|
||||
await drive_base.straight(-60)
|
||||
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-525)
|
||||
await drive_base.turn(20)
|
||||
await drive_base.straight(-200)
|
||||
|
||||
36
missions/Heavy_lifting_final.py
Normal file
36
missions/Heavy_lifting_final.py
Normal file
@@ -0,0 +1,36 @@
|
||||
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():
|
||||
#Get to mission
|
||||
await drive_base.straight(200)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(525)
|
||||
await drive_base.turn(60)
|
||||
#Solve mission
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(2000,1000)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(350,-1000)
|
||||
#Return home
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(-600)
|
||||
run_task(main())
|
||||
34
missions/Lift.py
Normal file
34
missions/Lift.py
Normal file
@@ -0,0 +1,34 @@
|
||||
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(525)
|
||||
await drive_base.turn(60)
|
||||
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(2000,1000)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(350,-1000)
|
||||
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(-600)
|
||||
run_task(main())
|
||||
34
missions/Lift2.py
Normal file
34
missions/Lift2.py
Normal file
@@ -0,0 +1,34 @@
|
||||
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(525)
|
||||
await drive_base.turn(60)
|
||||
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(2000,1000)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(350,-1000)
|
||||
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(-600)
|
||||
run_task(main())
|
||||
@@ -19,47 +19,31 @@ drive_base.settings(600,500,300,200)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
|
||||
right_arm.run_angle(1000,450)
|
||||
left_arm.run_angle(500,-90)
|
||||
await drive_base.straight(200)
|
||||
left_arm.run_angle(1000, 300)
|
||||
right_arm.run_angle(1000,500)
|
||||
await drive_base.straight(320)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(325)
|
||||
await left_arm.run_angle(500,90)
|
||||
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.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(277)
|
||||
await drive_base.turn(20)
|
||||
await drive_base.straight(65)
|
||||
|
||||
await drive_base.straight(-173)
|
||||
await drive_base.turn(-30)
|
||||
right_arm.run_angle(50,500)
|
||||
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)
|
||||
|
||||
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())
|
||||
@@ -1,38 +0,0 @@
|
||||
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 main():
|
||||
await drive_base.straight(700)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(110)
|
||||
await drive_base.straight(-220)
|
||||
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)
|
||||
|
||||
run_task(main())
|
||||
29
missions/Sand Mission.py
Normal file
29
missions/Sand Mission.py
Normal file
@@ -0,0 +1,29 @@
|
||||
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(400,500,100,100)
|
||||
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
|
||||
async def main():
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(300,100)
|
||||
await drive_base.straight(-100)
|
||||
await right_arm.run_angle(300,-100)
|
||||
await drive_base.straight(-350)
|
||||
|
||||
run_task(main())
|
||||
@@ -1,29 +1,31 @@
|
||||
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.tools import run_task, multitask
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.tools import run_task, multitask
|
||||
|
||||
# Initialize hub and devices
|
||||
hub = PrimeHub()
|
||||
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
left_arm = Motor(Port.C)
|
||||
|
||||
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||
right_arm = Motor(Port.D)
|
||||
lazer_ranger = UltrasonicSensor(Port.E)
|
||||
color_sensor = ColorSensor(Port.F)
|
||||
# DriveBase configuration
|
||||
WHEEL_DIAMETER = 68.8 # mm
|
||||
AXLE_TRACK = 180 # mm
|
||||
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||
drive_base.settings(600, 500, 300, 200)
|
||||
|
||||
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)
|
||||
|
||||
WALL_DISTANCE = 200 # mm
|
||||
|
||||
async def main():
|
||||
# Your code goes here
|
||||
|
||||
run_task(main())
|
||||
await drive_base.straight(915)
|
||||
await drive_base.turn(-90)
|
||||
await drive_base.straight(60)
|
||||
await left_arm.run_angle(10000,-4000)
|
||||
await drive_base.straight(-90)
|
||||
await drive_base.turn(80)
|
||||
await drive_base.straight(2000)
|
||||
|
||||
run_task(main())
|
||||
@@ -19,41 +19,28 @@ drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, 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)
|
||||
|
||||
async def main():
|
||||
await drive_base.straight(-920)
|
||||
#Get to mission
|
||||
await drive_base.straight(920)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(65)
|
||||
#Solve
|
||||
#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()
|
||||
)
|
||||
|
||||
run_task(main())
|
||||
50
missions/Set2.py
Normal file
50
missions/Set2.py
Normal file
@@ -0,0 +1,50 @@
|
||||
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, run_task, multitask
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
# Initialize both motors. In this example, the motor on the
|
||||
# left must turn counterclockwise to make the robot go forward.
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
|
||||
arm_motor = Motor(Port.D, Direction.CLOCKWISE)
|
||||
arm_motor_left= Motor(Port.C, Direction.CLOCKWISE)
|
||||
# Initialize the drive base. In this example, the wheel diameter is 56mm.
|
||||
# The distance between the two wheel-ground contact points is 112mm.
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||
|
||||
print('The default settings are: ' + str(drive_base.settings()))
|
||||
drive_base.settings(300,1000,300,750)
|
||||
# Optionally, uncomment the line below to use the gyro for improved accuracy.
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
await drive_base.straight(519)
|
||||
await arm_motor_left.run_angle(-10000, 300)
|
||||
await arm_motor_left.run_angle(10000, 600)
|
||||
await drive_base.straight(160)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(50)
|
||||
await arm_motor.run_angle(3000, 3000)
|
||||
await drive_base.straight(-150)
|
||||
await drive_base.turn(135)
|
||||
await drive_base.straight(50)
|
||||
await arm_motor.run_angle(10000, -3000)
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(-54)
|
||||
await arm_motor.run_angle(10000, -3000)
|
||||
await drive_base.straight(250)
|
||||
await drive_base.turn(-5)
|
||||
await arm_motor.run_angle(10000, 7000)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(68)
|
||||
await arm_motor.run_angle(10000, -6000)
|
||||
await drive_base.straight(200)
|
||||
await arm_motor.run_angle(10000, 4000)
|
||||
await drive_base.turn(-40)
|
||||
await arm_motor.run_angle(10000, 7000)
|
||||
run_task(main())
|
||||
44
missions/mission_09_old.py
Normal file
44
missions/mission_09_old.py
Normal file
@@ -0,0 +1,44 @@
|
||||
# ---JOHANNES---
|
||||
# THIS CODE IS NOT USED ANYMORE AND SHOULD NOT BE USED!!!!!!
|
||||
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
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
# Initialize both motors. In this example, the motor on the
|
||||
# left must turn counterclockwise to make the robot go forward.
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
|
||||
arm_motor = Motor(Port.E, Direction.CLOCKWISE)
|
||||
arm_motor.run_angle(299,90, Stop.HOLD)
|
||||
# Initialize the drive base. In this example, the wheel diameter is 56mm.
|
||||
# The distance between the two wheel-ground contact points is 112mm.
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=140)
|
||||
|
||||
print('The default settings are: ' + str(drive_base.settings()))
|
||||
drive_base.settings(100,1000,166,750)
|
||||
# Optionally, uncomment the line below to use the gyro for improved accuracy.
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def solveM9():
|
||||
print("Solving Mission 9")
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(260)
|
||||
await arm_motor.run_angle(500,-500, Stop.HOLD)
|
||||
await drive_base.straight(-40)
|
||||
await drive_base.turn(92)
|
||||
await drive_base.straight(-120)
|
||||
await drive_base.straight(220)
|
||||
await arm_motor.run_angle(500,100, Stop.HOLD)
|
||||
await drive_base.turn(-50)
|
||||
await drive_base.straight(-600)
|
||||
async def main():
|
||||
await drive_base.straight(50)
|
||||
print("Hello, Robot is starting to run.")
|
||||
await solveM9()
|
||||
|
||||
run_task(main())
|
||||
@@ -20,18 +20,29 @@ drive_base.settings(600,500,300,200)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
|
||||
right_arm.run_angle(500,400)
|
||||
await drive_base.straight(800)
|
||||
await drive_base.turn(90)
|
||||
await drive_base.straight(88)
|
||||
await right_arm.run_angle(100,-300)
|
||||
await right_arm.run_angle(400,400)
|
||||
left_arm.run_angle(600,200)
|
||||
right_arm.run_angle(500,200)
|
||||
await drive_base.straight(70)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(90)
|
||||
await drive_base.straight(800)
|
||||
drive_base.brake()
|
||||
|
||||
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.turn(-46.5) #ma din din din dun
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(900)
|
||||
run_task(main())
|
||||
|
||||
|
||||
|
||||
303
test_10_17_2025.py
Normal file
303
test_10_17_2025.py
Normal file
@@ -0,0 +1,303 @@
|
||||
# Stuff from 10/15/2025
|
||||
# Atharv trying with no avail to add more colors to the color sensor logic 😭😭😭😭😭😭😭
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor
|
||||
from pybricks.parameters import Port, Stop, Color, Direction
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.tools import wait, StopWatch, multitask, run_task
|
||||
|
||||
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(550,700,100,100)
|
||||
drive_base.use_gyro(True)
|
||||
color_sensor = ColorSensor(Port.F)
|
||||
Color.ORANGE = Color(28, 61, 61)
|
||||
Color.BLUE = Color(230,100,100)
|
||||
Color.YELLOW = Color(37, 85, 95)
|
||||
Color.PURPLE = Color(326, 60, 87)
|
||||
color_sensor.detectable_colors([Color.ORANGE, Color.BLUE, Color.GREEN, Color.WHITE, Color.RED, Color.YELLOW, Color.NONE, Color.PURPLE])
|
||||
hub.speaker.volume(50) # Set the volume of the speaker
|
||||
color_sensor.detectable_colors()
|
||||
# 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()
|
||||
|
||||
async def Run1():
|
||||
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(-700)
|
||||
|
||||
async def Run2():
|
||||
await drive_base.straight(200)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(525)
|
||||
await drive_base.turn(60)
|
||||
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(2000,1000)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(350,-1000)
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(-750)
|
||||
|
||||
|
||||
async def Run3():
|
||||
await drive_base.straight(920)
|
||||
await drive_base.turn(-90)
|
||||
await drive_base.straight(60)
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000,-4000)
|
||||
await drive_base.straight(-110)
|
||||
await drive_base.turn(90)
|
||||
await drive_base.straight(2000)
|
||||
|
||||
async def Run4():
|
||||
await drive_base.straight(519)
|
||||
await drive_base.straight(519)
|
||||
await arm_motor_left.run_angle(-10000, 300)
|
||||
await arm_motor_left.run_angle(10000, 600)
|
||||
await drive_base.straight(160)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(50)
|
||||
await arm_motor.run_angle(3000, 3000)
|
||||
await drive_base.straight(-150)
|
||||
await drive_base.turn(135)
|
||||
await drive_base.straight(50)
|
||||
await arm_motor.run_angle(10000, -3000)
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.turn(-54)
|
||||
await arm_motor.run_angle(10000, -3000)
|
||||
await drive_base.straight(250)
|
||||
await drive_base.turn(-5)
|
||||
await arm_motor.run_angle(10000, 7000)
|
||||
await drive_base.straight(-50)
|
||||
await drive_base.turn(68)
|
||||
await arm_motor.run_angle(10000, -6000)
|
||||
await drive_base.straight(200)
|
||||
await arm_motor.run_angle(10000, 4000)
|
||||
await drive_base.turn(-40)
|
||||
await arm_motor.run_angle(10000, 7000)
|
||||
|
||||
async def Run5():
|
||||
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 Run6():
|
||||
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)
|
||||
async def main():
|
||||
while True:
|
||||
|
||||
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
|
||||
color_reflected_percent = await color_sensor.reflection()
|
||||
print(color_reflected_percent)
|
||||
|
||||
color_detected = await color_sensor.color()
|
||||
print(f'Detected color: {color_detected.h}, {color_detected.s}, {color_detected.v}')
|
||||
if color_reflected_percent > 1:
|
||||
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.YELLOW:
|
||||
print('Running Mission 3')
|
||||
await Run3()
|
||||
#await celebrate_mission_complete(CelebrationSound.SUCCESS_CHIME )
|
||||
elif color_detected == Color.BLUE:
|
||||
print('Running Mission 4')
|
||||
await Run4()
|
||||
#await celebrate_mission_complete(CelebrationSound.POWER_UP)
|
||||
elif color_detected == Color.ORANGE:
|
||||
print('Running Mission 5')
|
||||
await Run5()
|
||||
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
|
||||
elif color_detected == Color.PURPLE:
|
||||
print('Running Mission 6 (this is ayaan\'s code)')
|
||||
await Run6()
|
||||
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
|
||||
else:
|
||||
hub.light.off()
|
||||
left_motor.stop()
|
||||
right_motor.stop()
|
||||
await wait(1000) #prevent loop from iterating fast
|
||||
# Main execution loop
|
||||
run_task(main())
|
||||
41
utils/color_sensor_navi.py
Normal file
41
utils/color_sensor_navi.py
Normal file
@@ -0,0 +1,41 @@
|
||||
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
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
# Robot hardware configuration
ROBOT_CONFIG = {
# Drive motors
'left_motor': Motor(Port.A),
'right_motor': Motor(Port.B),
# Attachment motors
'attachment_motor': Motor(Port.C),
'lift_motor': Motor(Port.D),
# Sensors
'color_left': ColorSensor(Port.E),
'color_right': ColorSensor(Port.F),
'ultrasonic': UltrasonicSensor(Port.S1),
# Hub reference
'hub': hub
}
# Speed and distance constants
SPEEDS = {
'FAST': 400,
'NORMAL': 250,
'SLOW': 100,
'PRECISE': 50
}
DISTANCES = {
'TILE_SIZE': 300, # mm per field tile
'ROBOT_LENGTH': 180, # mm
'ROBOT_WIDTH': 140 # mm
}
|
||||
|
||||
|
||||
def mission_run_1():
|
||||
print('Running missions in Run 1')
|
||||
|
||||
def mission_run_2():
|
||||
print('Running missions in Run 2')
|
||||
|
||||
def mission_run_3():
|
||||
print('Running missions in Run 3')
|
||||
|
||||
# In main.py - sensor-based navigation
|
||||
def main(self):
|
||||
"""Use color sensor to select runs"""
|
||||
print("Place colored object in front of sensor:")
|
||||
print("RED=Run1, GREEN=Run2, BLUE=Run3, YELLOW=Test")
|
||||
while True:
|
||||
color = ROBOT_CONFIG['color_left'].color()
|
||||
if color == Color.RED:
|
||||
mission_run_1()
|
||||
break
|
||||
elif color == Color.GREEN:
|
||||
mission_run_2()
|
||||
break
|
||||
elif color == Color.BLUE:
|
||||
mission_run_3()
|
||||
break
|
||||
elif color == Color.YELLOW:
|
||||
self.test_mode()
|
||||
break
|
||||
wait(1000)
|
||||
main()
|
||||
13
utils/constants.py
Normal file
13
utils/constants.py
Normal file
@@ -0,0 +1,13 @@
|
||||
#Speed and distance constants
|
||||
SPEEDS = {
|
||||
'FAST': 400,
|
||||
'NORMAL': 250,
|
||||
'SLOW': 100,
|
||||
'PRECISE': 50
|
||||
}
|
||||
|
||||
DISTANCES = {
|
||||
'TILE_SIZE': 300, # mm per field tile
|
||||
'ROBOT_LENGTH': 180, # mm
|
||||
'ROBOT_WIDTH': 140 # mm
|
||||
}
|
||||
36
utils/starter_drivebase_code.py
Normal file
36
utils/starter_drivebase_code.py
Normal file
@@ -0,0 +1,36 @@
|
||||
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
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
# Initialize both motors. In this example, the motor on the
|
||||
# left must turn counterclockwise to make the robot go forward.
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
|
||||
arm_motor = Motor(Port.E, Direction.CLOCKWISE)
|
||||
arm_motor.run_angle(299,90, Stop.HOLD)
|
||||
# Initialize the drive base. In this example, the wheel diameter is 56mm.
|
||||
# The distance between the two wheel-ground contact points is 112mm.
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=112)
|
||||
|
||||
print('The default settings are: ' + str(drive_base.settings()))
|
||||
drive_base.settings(100,1000,166,750)
|
||||
# Optionally, uncomment the line below to use the gyro for improved accuracy.
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
# Drive forward by 500mm (half a meter).
|
||||
drive_base.straight(500)
|
||||
|
||||
# Turn around clockwise by 180 degrees.
|
||||
drive_base.turn(180)
|
||||
|
||||
# Drive forward again to get back to the start.
|
||||
drive_base.straight(500)
|
||||
|
||||
# Turn around counterclockwise.
|
||||
drive_base.turn(-180)
|
||||
arm_motor.run_angle(299,-90, Stop.HOLD)
|
||||
Reference in New Issue
Block a user