Compare commits
54 Commits
HazmatScri
...
dev
| Author | SHA1 | Date | |
|---|---|---|---|
| 2d614c0e38 | |||
| c7801d17c0 | |||
| 974f87c4c4 | |||
| a50b0cd1d7 | |||
| 16e96693cc | |||
| acc44dac22 | |||
| 411f46d55e | |||
| 69745c053f | |||
| 214ea1c22e | |||
| 074703af1a | |||
| e924043945 | |||
| 94043eb522 | |||
| a690350d1d | |||
| 8d4b296331 | |||
| 73553b0964 | |||
| b1f378eae7 | |||
| 439aca673f | |||
| 4b7491637d | |||
| 9ca07a36a8 | |||
| 6c6b7f1f02 | |||
| b97dcf6837 | |||
| eea26150f9 | |||
| d056255994 | |||
| 5f0ffea7bc | |||
| 9a573c4eb2 | |||
| a0a4fa4792 | |||
| 14bc8d291a | |||
| b91e701f4c | |||
| debbc4e4d0 | |||
| 0c7633d92d | |||
| 3b81a3ff2b | |||
| 401033b185 | |||
| 9cdd9886e3 | |||
| 3a45721812 | |||
| a65ad1470c | |||
| b36a9f3355 | |||
| e785a8ad05 | |||
| a6bbba9419 | |||
| 8ccf8b9a5a | |||
| bb95852bf1 | |||
| 9ca160a8c7 | |||
| 882ba3e13a | |||
| 33b926cb1f | |||
| 4ff5de480d | |||
| aa062122de | |||
| 0fbb38baeb | |||
| 3e1928fcad | |||
| 1dec912abb | |||
| d3294bcb0b | |||
| 5ba2811af3 | |||
| 387e00b5c3 | |||
| 3fdd6ae9d1 | |||
| ee0b8eb6a0 | |||
| a745ed8c79 |
@@ -1,33 +0,0 @@
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.tools import run_task, multitask
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B)
|
||||
|
||||
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||
right_arm = Motor(Port.D)
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||
drive_base.settings(600,500,300,200)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
await drive_base.straight(200)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(536)
|
||||
await drive_base.turn(60, Stop.HOLD)
|
||||
await drive_base.straight(30)
|
||||
|
||||
await right_arm.run_angle(5000,3000)
|
||||
await drive_base.straight(40)
|
||||
await right_arm.run_angle(5000,-4000)
|
||||
await drive_base.straight(-60)
|
||||
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(-670)
|
||||
run_task(main())
|
||||
12
LINEUPS.md
12
LINEUPS.md
@@ -2,14 +2,12 @@
|
||||
|
||||
## These are the line-up positions for the robot game for various missions.
|
||||
|
||||
- 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.
|
||||
- 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.
|
||||
|
||||
- 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 thin 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 #6 (Not-so-heavy Lifting) [Right/Blue Home] - The robot's right edge should be positioned at the 1st thick from the right.
|
||||
- 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.
|
||||
73
README.md
73
README.md
@@ -14,7 +14,7 @@
|
||||
|
||||
## Project Overview
|
||||
|
||||
Welcome to the official code repository for **Team 65266 Lego Dynamics**! This repository contains all the Pybricks code powering our robot through the UNEARTHED season missions. Our modular approach allows for flexible mission execution and quick color-sensor-based run selection.
|
||||
Welcome to the official code repository for **Team 65266 - Lego Dynamics**! This repository contains all the Pybricks code powering our robot through the UNEARTHED season missions. Our modular approach allows for flexible mission execution and quick color-sensor-based run selection.
|
||||
|
||||
---
|
||||
|
||||
@@ -26,8 +26,8 @@ Welcome to the official code repository for **Team 65266 Lego Dynamics**! This r
|
||||
|-----------|--------------|
|
||||
| **Robot Name** | Optimus Prime III |
|
||||
| **Firmware** | Pybricks |
|
||||
| **Attachment Motors** | 2× Large Motors (Ports C & D) |
|
||||
| **Drive Motors** | 2× Small Motors (Ports A & B) |
|
||||
| **Attachment Motors** | 2x Large Motors (Ports C & D) |
|
||||
| **Drive Motors** | 2x Small Motors (Ports A & B) |
|
||||
| **Sensors** | Up-facing Color Sensor (Quick Start) |
|
||||
| **Attachments** | Multiple mission-specific tools |
|
||||
|
||||
@@ -46,10 +46,10 @@ Our codebase is organized for maximum efficiency and modularity:
|
||||
```
|
||||
Repository
|
||||
┣ run_1.py # Individual mission runs
|
||||
┣ run_2.py # Each file = 1+ mission completions
|
||||
┣ run_2.py
|
||||
┣ run_3.py
|
||||
┣ ...
|
||||
┗ master.py # 🎯 Combined master file with color-start logic
|
||||
┗ masterFile.py # 🎯 Combined master file with color-start logic - this changes periodically as we release new versions. Check this README if you are unsure what code should be loaded on the robot.
|
||||
```
|
||||
|
||||
### Workflow
|
||||
@@ -61,34 +61,45 @@ Repository
|
||||
|
||||
## How to Use
|
||||
|
||||
### Installation & Deployment
|
||||
### Installation & Deployment - from the server - everyday
|
||||
|
||||
1. **Load the Code**
|
||||
```bash
|
||||
# Open the master.py file in Pybricks IDE
|
||||
```
|
||||
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 [codes_for_scrimmage/hazmat/mainhazmatUPD.py](codes_for_scrimmage/hazmat/mainhazmatUPD.py) and click the "Download" button.
|
||||
- cURL or another HTTP data transferrer -
|
||||
|
||||
2. **Connect to Robot**
|
||||
- Pair your robot via Bluetooth in Pybricks
|
||||
```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 -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 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.
|
||||
|
||||
- Import button looks like this:
|
||||

|
||||
- Bluetooth button looks like this:
|
||||

|
||||
|
||||
3. **Upload to Robot** - Click "Download and Run" or send the program to the robot
|
||||
- Run button looks like this: 
|
||||
|
||||
3. **Upload to Robot**
|
||||
- Click "Download and Run" or send the program to the robot
|
||||
|
||||
4. **Start Your Run**
|
||||
- Hold a colored LEGO brick up to the color sensor
|
||||
- Different colors trigger different mission runs!
|
||||
- If starting without Pybricks, press the center circular button on the SPIKE Prime Hub to start the program.
|
||||
- Hold a colored LEGO brick up to the color sensor.
|
||||
- Different colors trigger different mission runs - color mapping is below.
|
||||
|
||||
### Color Start System
|
||||
| Color | Mission | Celebration Sound |
|
||||
|-------|-----------|------------------|
|
||||
| Green 🟩 | Run 1 | Victory Fanfare |
|
||||
| White ⚪ | Run 2 | Rickroll Inspired |
|
||||
| Yellow 🟨 | Run 3 | Success Chime |
|
||||
| Orange 🟧 | Run 4 | Power Up |
|
||||
| Blue 🟦 | Run 5 | Power Up |
|
||||
| Red 🟥 | Run 6 | Ta-Da! |
|
||||
|
||||
> **Tip** Organize your colored bricks before the match for quick run selection!
|
||||
| Color | Mission |
|
||||
|-------|-----------|
|
||||
| ```Green 🟩 ```| Run 1 |
|
||||
| ```Purple 🟪 ```| Run 2 |
|
||||
| ```Red 🟥 ```| Run 3 |
|
||||
| ```Yellow 🟨 ```| Run 4 |
|
||||
| ```Blue 🟦 ```| Run 5 |
|
||||
| ```Orange 🟧 ```| Run 6 |
|
||||
|
||||
---
|
||||
|
||||
@@ -114,9 +125,7 @@ Team members can contribute by:
|
||||
|
||||
**GNU General Public License v3.0**
|
||||
|
||||
```
|
||||
You can take inspiration from our code, but you can't take our exact code.
|
||||
```
|
||||
|
||||
This project is licensed under GPL-3.0 - see the [LICENSE](LICENSE) file for complete details.
|
||||
|
||||
@@ -130,14 +139,8 @@ This project is licensed under GPL-3.0 - see the [LICENSE](LICENSE) file for com
|
||||
|
||||
## Contact & Support
|
||||
|
||||
**Team 65266 Lego Dynamics**
|
||||
**Team 65266 - Lego Dynamics**
|
||||
|
||||
Questions about our approach? Interested in collaboration? Reach out!
|
||||
|
||||
---
|
||||
|
||||
<div align="center">
|
||||
|
||||
Star this repo if you found it helpful!
|
||||
|
||||
</div>
|
||||
@@ -140,40 +140,41 @@ async def Run4(): # From Send_Over_Final.py
|
||||
|
||||
# Add Rishi's code here
|
||||
async def Run5():
|
||||
await drive_base.straight(519)
|
||||
await left_arm.run_angle(-10000, 300)
|
||||
await left_arm.run_angle(10000, 600)
|
||||
await drive_base.straight(160)
|
||||
await drive_base.turn(-30)
|
||||
await drive_base.straight(50)
|
||||
await right_arm.run_angle(3000, 3000)
|
||||
await drive_base.straight(-150)
|
||||
await drive_base.turn(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(700)
|
||||
await drive_base.turn(-18)
|
||||
await drive_base.straight(100)
|
||||
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)
|
||||
await drive_base.straight(-205)
|
||||
await drive_base.turn(63)
|
||||
await drive_base.straight(125)
|
||||
await arm_motor.run_angle(1000, -1200)
|
||||
await drive_base.straight(84)
|
||||
await arm_motor.run_angle(300, 1200)
|
||||
await drive_base.straight(-875)
|
||||
|
||||
|
||||
# Add - Adi's code here
|
||||
async def Run6():
|
||||
await drive_base.straight(750)
|
||||
await drive_base.straight(-650)
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(300,500)
|
||||
await drive_base.straight(-75)
|
||||
await right_arm.run_angle(300, -900)
|
||||
await drive_base.straight(-350)
|
||||
await wait(1000)
|
||||
await drive_base.straight(800)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(-15)
|
||||
await drive_base.straight(350)
|
||||
await drive_base.turn(-94)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, 900)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-10)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(700, -200)
|
||||
await drive_base.turn(30)
|
||||
await drive_base.straight(-60)
|
||||
await drive_base.turn(80)
|
||||
await drive_base.straight(-900)
|
||||
|
||||
|
||||
# Function to classify color based on HSV
|
||||
|
||||
@@ -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 = 300 # mm
|
||||
WALL_DISTANCE = 200 # mm
|
||||
|
||||
async def drive_forward():
|
||||
"""Drive forward continuously using DriveBase."""
|
||||
@@ -45,30 +45,45 @@ async def monitor_distance():
|
||||
|
||||
# New Section
|
||||
async def Run1(): # From M8_5.py
|
||||
left_arm.run_angle(1000, -300)
|
||||
right_arm.run_angle(1000, 500)
|
||||
await drive_base.straight(320)
|
||||
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000, 500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000, 500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(277)
|
||||
await drive_base.turn(20)
|
||||
await drive_base.straight(65)
|
||||
await drive_base.turn(-30)
|
||||
right_arm.run_angle(50, 500)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(-145)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(90)
|
||||
await left_arm.run_angle(1000, 450)
|
||||
await drive_base.straight(-145)
|
||||
await left_arm.run_angle(1000, -450)
|
||||
await drive_base.straight(10)
|
||||
await drive_base.turn(35)
|
||||
await drive_base.straight(-600)
|
||||
right_arm.run_angle(1000,450)
|
||||
left_arm.run_angle(500,80)
|
||||
await drive_base.straight(200)
|
||||
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(325)
|
||||
await left_arm.run_angle(500,-80)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(500,170)
|
||||
|
||||
await drive_base.straight(-270)
|
||||
await drive_base.turn(40)
|
||||
await drive_base.straight(135)
|
||||
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(300)
|
||||
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(-120)
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(300)
|
||||
await drive_base.turn(-45)
|
||||
await drive_base.straight(500)
|
||||
|
||||
|
||||
async def Run2(): # From Heavy_lifting_final.py
|
||||
await drive_base.straight(200)
|
||||
@@ -105,77 +120,62 @@ 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 69
|
||||
await drive_base.turn(-41) #ma din din din dun 67 41 21
|
||||
await drive_base.straight(900)
|
||||
|
||||
|
||||
async def Run4(): # From Send_Over_Final.py
|
||||
#Get to mission
|
||||
|
||||
await drive_base.straight(920)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(65)
|
||||
#Solve mission
|
||||
#Solve
|
||||
drive_base.turn(-10)
|
||||
await left_arm.run_angle(10000, 4000)
|
||||
#Get to Red Start
|
||||
await left_arm.run_angle(10000,-4000)
|
||||
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(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 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)
|
||||
|
||||
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)
|
||||
|
||||
# Add - Adi's code here
|
||||
async def Run6():
|
||||
await drive_base.straight(750)
|
||||
await drive_base.straight(-650)
|
||||
|
||||
|
||||
await drive_base.straight(500)
|
||||
await right_arm.run_angle(300,500)
|
||||
await drive_base.straight(-75)
|
||||
await right_arm.run_angle(300, -900)
|
||||
await drive_base.straight(-350)
|
||||
await wait(1000)
|
||||
await drive_base.straight(800)
|
||||
await drive_base.straight(-200)
|
||||
await drive_base.turn(-15)
|
||||
await drive_base.straight(350)
|
||||
await drive_base.turn(-94)
|
||||
await drive_base.straight(-80)
|
||||
await left_arm.run_angle(500, 900)
|
||||
await drive_base.straight(50)
|
||||
await drive_base.turn(-10)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(700, -200)
|
||||
await drive_base.turn(30)
|
||||
await drive_base.straight(-60)
|
||||
await drive_base.turn(80)
|
||||
await drive_base.straight(-900)
|
||||
# Function to classify color based on HSV
|
||||
def detect_color(h, s, v, reflected):
|
||||
if reflected > 4:
|
||||
|
||||
@@ -2,26 +2,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
|
||||
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)
|
||||
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)
|
||||
|
||||
drive_base.settings(550,700,100,100)
|
||||
|
||||
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)
|
||||
|
||||
first_run = False
|
||||
|
||||
async def main():
|
||||
await drive_base.straight(750)
|
||||
await drive_base.straight(-650)
|
||||
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)
|
||||
|
||||
run_task(main())
|
||||
@@ -1,27 +0,0 @@
|
||||
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())
|
||||
@@ -1,37 +0,0 @@
|
||||
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)
|
||||
|
||||
@@ -1,36 +0,0 @@
|
||||
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())
|
||||
@@ -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
|
||||
from pybricks.tools import wait, StopWatch, run_task, multitask
|
||||
|
||||
hub = PrimeHub()
|
||||
|
||||
@@ -23,23 +23,15 @@ drive_base.settings(300,1000,300,750)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
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)
|
||||
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)
|
||||
|
||||
run_task(main())
|
||||
@@ -1,34 +0,0 @@
|
||||
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())
|
||||
@@ -1,34 +0,0 @@
|
||||
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,31 +19,44 @@ drive_base.settings(600,500,300,200)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
left_arm.run_angle(1000, 300)
|
||||
right_arm.run_angle(1000,500)
|
||||
await drive_base.straight(320)
|
||||
|
||||
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||
right_arm.run_angle(1000,450)
|
||||
left_arm.run_angle(500,-80)
|
||||
await drive_base.straight(200)
|
||||
|
||||
await drive_base.turn(-20)
|
||||
await drive_base.straight(277)
|
||||
await drive_base.turn(20)
|
||||
await drive_base.straight(65)
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(325)
|
||||
await left_arm.run_angle(500,80)
|
||||
|
||||
await drive_base.straight(-100)
|
||||
await drive_base.straight(50)
|
||||
await left_arm.run_angle(500,-170)
|
||||
|
||||
await drive_base.straight(-270)
|
||||
await drive_base.turn(40)
|
||||
await drive_base.straight(135)
|
||||
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(300)
|
||||
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(-120)
|
||||
await drive_base.turn(-100)
|
||||
await drive_base.straight(300)
|
||||
await drive_base.turn(-45)
|
||||
await drive_base.straight(500)
|
||||
|
||||
await drive_base.turn(-30)
|
||||
right_arm.run_angle(50,500)
|
||||
await drive_base.turn(45)
|
||||
await drive_base.straight(-145)
|
||||
await drive_base.turn(-60)
|
||||
await drive_base.straight(90)
|
||||
await left_arm.run_angle(1000,-450)
|
||||
await drive_base.straight(-145)
|
||||
await left_arm.run_angle(1000,450)
|
||||
await drive_base.straight(10)
|
||||
await drive_base.turn(35)
|
||||
await drive_base.straight(-600)
|
||||
run_task(main())
|
||||
@@ -37,14 +37,6 @@ async def main():
|
||||
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)
|
||||
await arm_motor.run_angle(10000, 10000)
|
||||
run_task(main())
|
||||
@@ -1,29 +0,0 @@
|
||||
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,31 +0,0 @@
|
||||
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(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,28 +19,41 @@ 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():
|
||||
#Get to mission
|
||||
await drive_base.straight(920)
|
||||
await drive_base.straight(-920)
|
||||
await drive_base.turn(-90,Stop.HOLD)
|
||||
await drive_base.straight(65)
|
||||
#Solve mission
|
||||
#Solve
|
||||
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())
|
||||
@@ -1,44 +0,0 @@
|
||||
# ---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())
|
||||
@@ -15,31 +15,34 @@ left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||
right_arm = Motor(Port.D)
|
||||
|
||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||
drive_base.settings(300,1000,300,200)
|
||||
drive_base.settings(600,500,300,200)
|
||||
|
||||
#drive_base.use_gyro(True)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
async def main():
|
||||
left_arm.run_angle(500,200)
|
||||
left_arm.run_angle(600,200)
|
||||
right_arm.run_angle(500,200)
|
||||
await drive_base.straight(70)
|
||||
|
||||
await drive_base.turn(-55)
|
||||
await drive_base.turn(-70)
|
||||
await drive_base.straight(900)
|
||||
await drive_base.turn(92.5)
|
||||
await drive_base.turn(115)
|
||||
|
||||
await drive_base.straight(75)
|
||||
await drive_base.straight(21)
|
||||
await drive_base.straight(33)
|
||||
await right_arm.run_angle(500,-250)
|
||||
await right_arm.run_angle(500,250)
|
||||
await drive_base.turn(55)
|
||||
await drive_base.turn(66)
|
||||
await drive_base.straight(7)
|
||||
|
||||
await left_arm.run_angle(300,-400)
|
||||
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)
|
||||
await drive_base.turn(-46.5) #ma din din din dun
|
||||
await drive_base.turn(-40)
|
||||
await drive_base.straight(900)
|
||||
run_task(main())
|
||||
|
||||
|
||||
|
||||
|
||||
69
utils/tests/colorsensortest.py
Normal file
69
utils/tests/colorsensortest.py
Normal file
@@ -0,0 +1,69 @@
|
||||
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()
|
||||
|
||||
color_sensor = ColorSensor(Port.F)
|
||||
|
||||
# Color Settings
|
||||
# https://docs.pybricks.com/en/latest/parameters/color.html
|
||||
print("Default Detected Colors:", color_sensor.detectable_colors())
|
||||
|
||||
# Custom color Hue, Saturation, Brightness value for Lego bricks
|
||||
Color.MAGENTA = Color(315,100,60)
|
||||
Color.BLUE = Color(240,100,100)
|
||||
Color.CYAN = Color(180,100,100)
|
||||
Color.RED = Color(350, 100, 100)
|
||||
LEGO_BRICKS_COLOR = [
|
||||
Color.BLUE,
|
||||
Color.GREEN,
|
||||
Color.WHITE,
|
||||
Color.RED,
|
||||
Color.YELLOW,
|
||||
Color.MAGENTA,
|
||||
Color.NONE
|
||||
]
|
||||
magenta_counter = 0
|
||||
stable_color = None
|
||||
real_color = None
|
||||
#Update Detectable colors
|
||||
color_sensor.detectable_colors(LEGO_BRICKS_COLOR)
|
||||
print(f'Yellow:{Color.YELLOW} : {Color.YELLOW.h}, {Color.YELLOW.s}, {Color.YELLOW.v}')
|
||||
print("Updated Detected Colors:", color_sensor.detectable_colors())
|
||||
async def main():
|
||||
while True:
|
||||
global magenta_counter, stable_color, real_color
|
||||
color_reflected_percent = await color_sensor.reflection()
|
||||
print("Reflection: ", color_reflected_percent)
|
||||
if color_reflected_percent > 15:
|
||||
color_detected = await color_sensor.color()
|
||||
|
||||
if color_detected == Color.MAGENTA:
|
||||
magenta_counter += 1
|
||||
else:
|
||||
magenta_counter = 0
|
||||
stable_color = color_detected
|
||||
|
||||
# Only accept magenta if it's been stable for a while - usually triggers before other colors so we gotta do this :|
|
||||
if magenta_counter > 10:
|
||||
stable_color = Color.MAGENTA
|
||||
if stable_color != Color.MAGENTA:
|
||||
stable_color = await color_sensor.color()
|
||||
|
||||
real_color = stable_color
|
||||
#if(color_detected != Color.NONE):
|
||||
# return
|
||||
|
||||
print("Magenta counter: ", magenta_counter)
|
||||
if real_color is not None:
|
||||
print(f'Detected color: {real_color} : {real_color.h}, {real_color.s}, {real_color.v}')
|
||||
else:
|
||||
print("No valid color detected yet.")
|
||||
await wait(50)
|
||||
|
||||
run_task(main())
|
||||
Reference in New Issue
Block a user