Merge branch 'Vickram_dev_' into dev
Merging Vickram_dev_ into dev
This commit is contained in:
alkadienePhoton
73
README.md
73
README.md
@@ -14,7 +14,7 @@
|
|||||||
|
|
||||||
## Project Overview
|
## 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 |
|
| **Robot Name** | Optimus Prime III |
|
||||||
| **Firmware** | Pybricks |
|
| **Firmware** | Pybricks |
|
||||||
| **Attachment Motors** | 2× Large Motors (Ports C & D) |
|
| **Attachment Motors** | 2x Large Motors (Ports C & D) |
|
||||||
| **Drive Motors** | 2× Small Motors (Ports A & B) |
|
| **Drive Motors** | 2x Small Motors (Ports A & B) |
|
||||||
| **Sensors** | Up-facing Color Sensor (Quick Start) |
|
| **Sensors** | Up-facing Color Sensor (Quick Start) |
|
||||||
| **Attachments** | Multiple mission-specific tools |
|
| **Attachments** | Multiple mission-specific tools |
|
||||||
|
|
||||||
@@ -46,10 +46,10 @@ Our codebase is organized for maximum efficiency and modularity:
|
|||||||
```
|
```
|
||||||
Repository
|
Repository
|
||||||
┣ run_1.py # Individual mission runs
|
┣ run_1.py # Individual mission runs
|
||||||
┣ run_2.py # Each file = 1+ mission completions
|
┣ run_2.py
|
||||||
┣ run_3.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
|
### Workflow
|
||||||
@@ -61,34 +61,45 @@ Repository
|
|||||||
|
|
||||||
## How to Use
|
## How to Use
|
||||||
|
|
||||||
### Installation & Deployment
|
### Installation & Deployment - from the server - everyday
|
||||||
|
|
||||||
1. **Load the Code**
|
1. Download the file codes_for_scrimmage/hazmat/mainhazmatUPD.py
|
||||||
```bash
|
- You can do this through the repo, by using cURL, or by using git.
|
||||||
# Open the master.py file in Pybricks IDE
|
- 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**
|
```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```
|
||||||
- Pair your robot via Bluetooth in Pybricks
|
- 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**
|
4. **Start Your Run**
|
||||||
- Hold a colored LEGO brick up to the color sensor
|
- If starting without Pybricks, press the center circular button on the SPIKE Prime Hub to start the program.
|
||||||
- Different colors trigger different mission runs!
|
- Hold a colored LEGO brick up to the color sensor.
|
||||||
|
- Different colors trigger different mission runs - color mapping is below.
|
||||||
|
|
||||||
### Color Start System
|
### Color Start System
|
||||||
| Color | Mission | Celebration Sound |
|
| Color | Mission |
|
||||||
|-------|-----------|------------------|
|
|-------|-----------|
|
||||||
| Green 🟩 | Run 1 | Victory Fanfare |
|
| Green 🟩 | Run 1 |
|
||||||
| White ⚪ | Run 2 | Rickroll Inspired |
|
| Purple 🟪 | Run 2 |
|
||||||
| Yellow 🟨 | Run 3 | Success Chime |
|
| Red 🟥| Run 3 |
|
||||||
| Orange 🟧 | Run 4 | Power Up |
|
| Yellow 🟨| Run 4 |
|
||||||
| Blue 🟦 | Run 5 | Power Up |
|
| Blue 🟦| Run 5 |
|
||||||
| Red 🟥 | Run 6 | Ta-Da! |
|
| Orange 🟧| Run 6 |
|
||||||
|
|
||||||
> **Tip** Organize your colored bricks before the match for quick run selection!
|
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
@@ -114,9 +125,7 @@ Team members can contribute by:
|
|||||||
|
|
||||||
**GNU General Public License v3.0**
|
**GNU General Public License v3.0**
|
||||||
|
|
||||||
```
|
|
||||||
You can take inspiration from our code, but you can't take our exact code.
|
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.
|
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
|
## Contact & Support
|
||||||
|
|
||||||
**Team 65266 Lego Dynamics**
|
**Team 65266 - Lego Dynamics**
|
||||||
|
|
||||||
Questions about our approach? Interested in collaboration? Reach out!
|
Questions about our approach? Interested in collaboration? Reach out!
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
<div align="center">
|
|
||||||
|
|
||||||
Star this repo if you found it helpful!
|
|
||||||
|
|
||||||
</div>
|
|
||||||
42
RoshoDaGoat.py
Normal file
42
RoshoDaGoat.py
Normal file
@@ -0,0 +1,42 @@
|
|||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch, 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(200)
|
||||||
|
await arm_motor.run_angle(10000, 10000)
|
||||||
|
run_task(main())
|
||||||
227
codes_for_scrimmage/hazmat/HazmatCodeChanges.py
Normal file
227
codes_for_scrimmage/hazmat/HazmatCodeChanges.py
Normal file
@@ -0,0 +1,227 @@
|
|||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.tools import run_task, multitask
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
|
||||||
|
# Initialize hub and devices
|
||||||
|
hub = PrimeHub()
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B)
|
||||||
|
left_arm = Motor(Port.C)
|
||||||
|
right_arm = Motor(Port.D)
|
||||||
|
lazer_ranger = UltrasonicSensor(Port.E)
|
||||||
|
color_sensor = ColorSensor(Port.F)
|
||||||
|
|
||||||
|
# DriveBase configuration
|
||||||
|
WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels)
|
||||||
|
AXLE_TRACK = 180 # mm (distance between wheels)
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||||
|
drive_base.settings(600, 500, 300, 200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
WALL_DISTANCE = 300 # mm
|
||||||
|
|
||||||
|
async def drive_forward():
|
||||||
|
"""Drive forward continuously using DriveBase."""
|
||||||
|
drive_base.drive(400, 0)
|
||||||
|
|
||||||
|
|
||||||
|
async def monitor_distance():
|
||||||
|
"""Monitor ultrasonic sensor and stop when wall is detected."""
|
||||||
|
while True:
|
||||||
|
distance = await lazer_ranger.distance()
|
||||||
|
print('Distancing...',distance)
|
||||||
|
|
||||||
|
if distance < WALL_DISTANCE:
|
||||||
|
# Stop the drivebase
|
||||||
|
await drive_base.stop
|
||||||
|
print(f"Wall detected at {distance}mm!")
|
||||||
|
break
|
||||||
|
|
||||||
|
# Small delay to prevent overwhelming the sensor
|
||||||
|
await wait(50)
|
||||||
|
|
||||||
|
# New Section
|
||||||
|
async def Run1(): # From M8_5.py
|
||||||
|
left_arm.run_angle(1000, -300)
|
||||||
|
right_arm.run_angle(1000, 500)
|
||||||
|
await drive_base.straight(320)
|
||||||
|
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000, 500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000, 500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await drive_base.straight(277)
|
||||||
|
await drive_base.turn(20)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
await drive_base.turn(-30)
|
||||||
|
right_arm.run_angle(50, 500)
|
||||||
|
await drive_base.turn(45)
|
||||||
|
await drive_base.straight(-145)
|
||||||
|
await drive_base.turn(-60)
|
||||||
|
await drive_base.straight(90)
|
||||||
|
await left_arm.run_angle(1000, 450)
|
||||||
|
await drive_base.straight(-145)
|
||||||
|
await left_arm.run_angle(1000, -450)
|
||||||
|
await drive_base.straight(10)
|
||||||
|
await drive_base.turn(35)
|
||||||
|
await drive_base.straight(-600)
|
||||||
|
|
||||||
|
async def Run2(): # From Heavy_lifting_final.py
|
||||||
|
await drive_base.straight(200)
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await drive_base.straight(536)
|
||||||
|
await drive_base.turn(60, Stop.HOLD)
|
||||||
|
await drive_base.straight(30)
|
||||||
|
await right_arm.run_angle(5000, 2900)
|
||||||
|
await drive_base.straight(40)
|
||||||
|
await right_arm.run_angle(5000, -4000)
|
||||||
|
await drive_base.straight(-60)
|
||||||
|
await drive_base.turn(-60)
|
||||||
|
await drive_base.straight(-670)
|
||||||
|
|
||||||
|
|
||||||
|
async def Run3(): # tip the scale.py
|
||||||
|
left_arm.run_angle(600,-200)
|
||||||
|
right_arm.run_angle(500,200)
|
||||||
|
await drive_base.straight(70)
|
||||||
|
|
||||||
|
await drive_base.turn(-70)
|
||||||
|
await drive_base.straight(900)
|
||||||
|
await drive_base.turn(115)
|
||||||
|
|
||||||
|
await drive_base.straight(75)
|
||||||
|
await drive_base.straight(33)
|
||||||
|
await right_arm.run_angle(500,-250)
|
||||||
|
await right_arm.run_angle(500,250)
|
||||||
|
await drive_base.turn(66)
|
||||||
|
await drive_base.straight(7)
|
||||||
|
|
||||||
|
await left_arm.run_angle(560,390) #going down
|
||||||
|
print('turning now...')
|
||||||
|
await drive_base.turn(40) # turning right
|
||||||
|
await left_arm.run_angle(410,-400) #lift a little bit
|
||||||
|
await drive_base.straight(80)
|
||||||
|
await drive_base.turn(-41) #ma din din din dun 67 41 21 69
|
||||||
|
await drive_base.straight(900)
|
||||||
|
|
||||||
|
|
||||||
|
async def Run4(): # From Send_Over_Final.py
|
||||||
|
#Get to mission
|
||||||
|
await drive_base.straight(920)
|
||||||
|
await drive_base.turn(-90,Stop.HOLD)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
#Solve mission
|
||||||
|
drive_base.turn(-10)
|
||||||
|
await left_arm.run_angle(10000, 4000)
|
||||||
|
#Get to Red Start
|
||||||
|
await drive_base.straight(-110)
|
||||||
|
await drive_base.turn(90)
|
||||||
|
# while True:
|
||||||
|
# distance_mm = await lazer_ranger.distance()
|
||||||
|
# print('distancing...',distance_mm)
|
||||||
|
|
||||||
|
# if distance_mm < 300:
|
||||||
|
# drive_base.stop
|
||||||
|
# break
|
||||||
|
# else:
|
||||||
|
# drive_base.straight(300)
|
||||||
|
# print('running...')
|
||||||
|
# await wait(10)
|
||||||
|
await multitask(
|
||||||
|
drive_forward(),
|
||||||
|
monitor_distance()
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# Add Rishi's code here
|
||||||
|
async def Run5():
|
||||||
|
await drive_base.straight(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)
|
||||||
|
|
||||||
|
|
||||||
|
# Add - Adi's code here
|
||||||
|
async def Run6():
|
||||||
|
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:
|
||||||
|
if h < 4 or h > 350: # red
|
||||||
|
return "Red"
|
||||||
|
elif 3 < h < 40 and s > 70: # orange
|
||||||
|
return "Orange"
|
||||||
|
elif 47 < h < 56: # yellow
|
||||||
|
return "Yellow"
|
||||||
|
elif 70 < h < 160: # green - do it vertically not horizontally for accuracy
|
||||||
|
return "Green"
|
||||||
|
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
|
||||||
|
return "Blue"
|
||||||
|
elif 260 < h < 350: # purple
|
||||||
|
return "Purple"
|
||||||
|
else:
|
||||||
|
return "Unknown"
|
||||||
|
return "Unknown"
|
||||||
|
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
while True:
|
||||||
|
h, s, v = await color_sensor.hsv()
|
||||||
|
reflected = await color_sensor.reflection()
|
||||||
|
color = detect_color(h, s, v, reflected)
|
||||||
|
|
||||||
|
if color == "Red":
|
||||||
|
print('Running Mission 3')
|
||||||
|
await Run3() #red
|
||||||
|
elif color == "Orange":
|
||||||
|
print('Running Mission 6')
|
||||||
|
await Run6() #orange
|
||||||
|
elif color == "Yellow":
|
||||||
|
print('Running Mission 4')
|
||||||
|
await Run4() #yellow
|
||||||
|
elif color == "Green":
|
||||||
|
print('Running Mission 1')
|
||||||
|
await Run1() #green - vertically
|
||||||
|
elif color == "Blue":
|
||||||
|
print('Running Mission 5')
|
||||||
|
await Run5() #blue - vertically
|
||||||
|
elif color == "Purple":
|
||||||
|
print('Running Mission 2')
|
||||||
|
await Run2() #purple - vertically
|
||||||
|
else:
|
||||||
|
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||||
|
await wait(10)
|
||||||
|
# Run the main function
|
||||||
|
run_task(main())
|
||||||
33
codes_for_scrimmage/hazmat/Heavy_lifting_final.py
Normal file
33
codes_for_scrimmage/hazmat/Heavy_lifting_final.py
Normal file
@@ -0,0 +1,33 @@
|
|||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.tools import run_task, multitask
|
||||||
|
|
||||||
|
hub = PrimeHub()
|
||||||
|
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B)
|
||||||
|
|
||||||
|
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_arm = Motor(Port.D)
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||||
|
drive_base.settings(600,500,300,200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
#await drive_base.straight(200)
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await drive_base.straight(536)
|
||||||
|
await drive_base.turn(60, Stop.HOLD)
|
||||||
|
await drive_base.straight(30)
|
||||||
|
|
||||||
|
await right_arm.run_angle(5000,2900)
|
||||||
|
await drive_base.straight(40)
|
||||||
|
await right_arm.run_angle(5000,-4000)
|
||||||
|
await drive_base.straight(-60)
|
||||||
|
|
||||||
|
await drive_base.turn(-60)
|
||||||
|
await drive_base.straight(-670)
|
||||||
|
run_task(main())
|
||||||
49
codes_for_scrimmage/hazmat/M8_5.py
Normal file
49
codes_for_scrimmage/hazmat/M8_5.py
Normal file
@@ -0,0 +1,49 @@
|
|||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.tools import run_task, multitask
|
||||||
|
|
||||||
|
hub = PrimeHub()
|
||||||
|
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B)
|
||||||
|
|
||||||
|
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_arm = Motor(Port.D)
|
||||||
|
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||||
|
|
||||||
|
drive_base.settings(600,500,300,200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
left_arm.run_angle(1000, 300)
|
||||||
|
right_arm.run_angle(1000,500)
|
||||||
|
await drive_base.straight(320)
|
||||||
|
|
||||||
|
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||||
|
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await drive_base.straight(277)
|
||||||
|
await drive_base.turn(20)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
|
||||||
|
await drive_base.turn(-30)
|
||||||
|
right_arm.run_angle(50,500)
|
||||||
|
await drive_base.turn(45)
|
||||||
|
await drive_base.straight(-145)
|
||||||
|
await drive_base.turn(-60)
|
||||||
|
await drive_base.straight(90)
|
||||||
|
await left_arm.run_angle(1000,-450)
|
||||||
|
await drive_base.straight(-145)
|
||||||
|
await left_arm.run_angle(1000,450)
|
||||||
|
await drive_base.straight(10)
|
||||||
|
await drive_base.turn(35)
|
||||||
|
await drive_base.straight(-600)
|
||||||
|
run_task(main())
|
||||||
46
codes_for_scrimmage/hazmat/Send_Over_Final.py
Normal file
46
codes_for_scrimmage/hazmat/Send_Over_Final.py
Normal file
@@ -0,0 +1,46 @@
|
|||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.tools import run_task, multitask
|
||||||
|
|
||||||
|
hub = PrimeHub()
|
||||||
|
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B)
|
||||||
|
|
||||||
|
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_arm = Motor(Port.D)
|
||||||
|
lazer_ranger = UltrasonicSensor(Port.E)
|
||||||
|
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||||
|
|
||||||
|
drive_base.settings(600,500,300,200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
#Get to mission
|
||||||
|
await drive_base.straight(920)
|
||||||
|
await drive_base.turn(-90,Stop.HOLD)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
#Solve mission
|
||||||
|
drive_base.turn(-10)
|
||||||
|
await left_arm.run_angle(10000,-4000)
|
||||||
|
#Get to Red Start
|
||||||
|
await drive_base.straight(-110)
|
||||||
|
await drive_base.turn(90)
|
||||||
|
await drive_base.straight(500)
|
||||||
|
while True:
|
||||||
|
distance_mm = await lazer_ranger.distance()
|
||||||
|
print('distancing...',distance_mm)
|
||||||
|
|
||||||
|
if distance_mm < 300:
|
||||||
|
drive_base.stop
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
drive_base.straight(300)
|
||||||
|
print('running...')
|
||||||
|
await wait(10)
|
||||||
|
|
||||||
|
run_task(main())
|
||||||
384
codes_for_scrimmage/hazmat/hazmat_main.py
Normal file
384
codes_for_scrimmage/hazmat/hazmat_main.py
Normal file
@@ -0,0 +1,384 @@
|
|||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.tools import run_task, multitask
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
|
||||||
|
# Initialize hub and devices
|
||||||
|
hub = PrimeHub()
|
||||||
|
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B)
|
||||||
|
|
||||||
|
left_arm = Motor(Port.C)
|
||||||
|
right_arm = Motor(Port.D)
|
||||||
|
lazer_ranger = UltrasonicSensor(Port.E)
|
||||||
|
color_sensor = ColorSensor(Port.F)
|
||||||
|
|
||||||
|
# DriveBase configuration
|
||||||
|
WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels)
|
||||||
|
AXLE_TRACK = 180 # mm (distance between wheels)
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||||
|
drive_base.settings(600,500,300,200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
# Color Settings
|
||||||
|
# https://docs.pybricks.com/en/latest/parameters/color.html
|
||||||
|
print("Default Detected Colors:", color_sensor.detectable_colors())
|
||||||
|
|
||||||
|
# Custom color Hue, Saturation, Brightness value for Lego bricks
|
||||||
|
Color.MAGENTA = Color(300,100,100)
|
||||||
|
Color.VIOLET = Color(277,68,32)
|
||||||
|
Color.BLUE = Color(240,100,100)
|
||||||
|
Color.CYAN = Color(180,100,100)
|
||||||
|
|
||||||
|
LEGO_BRICKS_COLOR = [
|
||||||
|
Color.BLUE,
|
||||||
|
Color.GREEN,
|
||||||
|
Color.WHITE,
|
||||||
|
Color.RED,
|
||||||
|
Color.YELLOW,
|
||||||
|
Color.MAGENTA,
|
||||||
|
Color.VIOLET,
|
||||||
|
Color.NONE
|
||||||
|
]
|
||||||
|
#Update Detectable colors
|
||||||
|
color_sensor.detectable_colors(LEGO_BRICKS_COLOR)
|
||||||
|
|
||||||
|
print("Updated Detected Colors:", color_sensor.detectable_colors())
|
||||||
|
|
||||||
|
hub.speaker.volume(50) # Set the volume of the speaker
|
||||||
|
|
||||||
|
# Celebration sound types
|
||||||
|
class CelebrationSound:
|
||||||
|
VICTORY_FANFARE = 0
|
||||||
|
LEVEL_UP = 1
|
||||||
|
SUCCESS_CHIME = 2
|
||||||
|
TA_DA = 3
|
||||||
|
POWER_UP = 4
|
||||||
|
RICKROLL_INSPIRED = 5
|
||||||
|
|
||||||
|
async def play_victory_fanfare():
|
||||||
|
"""Classic victory fanfare"""
|
||||||
|
notes = [
|
||||||
|
(262, 200), # C4
|
||||||
|
(262, 200), # C4
|
||||||
|
(262, 200), # C4
|
||||||
|
(349, 600), # F4
|
||||||
|
]
|
||||||
|
|
||||||
|
for freq, duration in notes:
|
||||||
|
await hub.speaker.beep(freq, duration)
|
||||||
|
await wait(50)
|
||||||
|
async def play_level_up():
|
||||||
|
"""Upward scale for level completion"""
|
||||||
|
notesold = [
|
||||||
|
(262, 100), # C4
|
||||||
|
(294, 100), # D4
|
||||||
|
(330, 100), # E4
|
||||||
|
(349, 100), # F4
|
||||||
|
(392, 100), # G4
|
||||||
|
(440, 100), # A4
|
||||||
|
(494, 100), # B4
|
||||||
|
(523, 300), # C5
|
||||||
|
]
|
||||||
|
notes = [
|
||||||
|
(277, 100),
|
||||||
|
(330, 100),
|
||||||
|
(277, 100),
|
||||||
|
(554, 100),
|
||||||
|
(277, 100),
|
||||||
|
(413, 100),
|
||||||
|
(330, 100),
|
||||||
|
(277, 100),
|
||||||
|
(413, 100),
|
||||||
|
(277, 100),
|
||||||
|
(554, 100),
|
||||||
|
(413, 100),
|
||||||
|
(277, 100),
|
||||||
|
(413, 100),
|
||||||
|
(554, 100),
|
||||||
|
(413, 100)
|
||||||
|
]
|
||||||
|
|
||||||
|
for freq, duration in notes:
|
||||||
|
await hub.speaker.beep(freq, duration)
|
||||||
|
#await wait(20)
|
||||||
|
async def play_success_chime():
|
||||||
|
"""Simple success notification"""
|
||||||
|
notes = [
|
||||||
|
(523, 150), # C5
|
||||||
|
(659, 150), # E5
|
||||||
|
(784, 300), # G5
|
||||||
|
]
|
||||||
|
|
||||||
|
for freq, duration in notes:
|
||||||
|
await hub.speaker.beep(freq, duration)
|
||||||
|
await wait(50)
|
||||||
|
async def play_ta_da():
|
||||||
|
"""Classic "ta-da!" sound"""
|
||||||
|
notes = [
|
||||||
|
(392, 200), # G4
|
||||||
|
(523, 400), # C5
|
||||||
|
]
|
||||||
|
|
||||||
|
for freq, duration in notes:
|
||||||
|
await hub.speaker.beep(freq, duration)
|
||||||
|
await wait(100)
|
||||||
|
async def play_power_up():
|
||||||
|
"""Rising power-up sound"""
|
||||||
|
for freq in range(200, 800, 50):
|
||||||
|
await hub.speaker.beep(freq, 50)
|
||||||
|
await wait(10)
|
||||||
|
await hub.speaker.beep(1000, 200)
|
||||||
|
async def play_rickroll_inspired():
|
||||||
|
"""Fun 80s-style dance beat inspired sound"""
|
||||||
|
# Upbeat bouncy rhythm
|
||||||
|
pattern = [
|
||||||
|
(392, 200), (440, 200), (494, 200), (523, 200),
|
||||||
|
(440, 200), (392, 200), (349, 200), (392, 300),
|
||||||
|
(440, 200), (392, 200), (349, 200), (330, 400),
|
||||||
|
]
|
||||||
|
|
||||||
|
for freq, duration in pattern:
|
||||||
|
await hub.speaker.beep(freq, duration)
|
||||||
|
await wait(50)
|
||||||
|
async def celebrate_mission_complete(sound_type=CelebrationSound.SUCCESS_CHIME):
|
||||||
|
"""
|
||||||
|
Main celebration function to call after completing a mission.
|
||||||
|
Plays a sound and shows light animation.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
sound_type: CelebrationSound enum value (default: SUCCESS_CHIME)
|
||||||
|
"""
|
||||||
|
# Light show
|
||||||
|
hub.light.on(Color.GREEN)
|
||||||
|
|
||||||
|
# Play the selected celebration sound
|
||||||
|
if sound_type == CelebrationSound.VICTORY_FANFARE:
|
||||||
|
await play_victory_fanfare()
|
||||||
|
elif sound_type == CelebrationSound.LEVEL_UP:
|
||||||
|
await play_level_up()
|
||||||
|
elif sound_type == CelebrationSound.SUCCESS_CHIME:
|
||||||
|
await play_success_chime()
|
||||||
|
elif sound_type == CelebrationSound.TA_DA:
|
||||||
|
await play_ta_da()
|
||||||
|
elif sound_type == CelebrationSound.POWER_UP:
|
||||||
|
await play_power_up()
|
||||||
|
elif sound_type == CelebrationSound.RICKROLL_INSPIRED:
|
||||||
|
await play_rickroll_inspired()
|
||||||
|
else:
|
||||||
|
await play_success_chime() # Default fallback
|
||||||
|
|
||||||
|
# Blink the light
|
||||||
|
for _ in range(3):
|
||||||
|
hub.light.off()
|
||||||
|
await wait(100)
|
||||||
|
hub.light.on(Color.GREEN)
|
||||||
|
await wait(100)
|
||||||
|
|
||||||
|
hub.light.off()
|
||||||
|
|
||||||
|
WALL_DISTANCE = 200 # mm
|
||||||
|
async def drive_forward():
|
||||||
|
"""Drive forward continuously using DriveBase."""
|
||||||
|
#await drive_base.straight(5000)
|
||||||
|
drive_base.drive(400,0)
|
||||||
|
async def monitor_distance():
|
||||||
|
"""Monitor ultrasonic sensor and stop when wall is detected."""
|
||||||
|
while True:
|
||||||
|
distance = await lazer_ranger.distance()
|
||||||
|
print('distancing...',distance)
|
||||||
|
|
||||||
|
if distance < WALL_DISTANCE:
|
||||||
|
# Stop the drivebase
|
||||||
|
await drive_base.turn(-180)
|
||||||
|
drive_base.brake
|
||||||
|
print(f"Wall detected at {distance}mm!")
|
||||||
|
break
|
||||||
|
|
||||||
|
# Small delay to prevent overwhelming the sensor
|
||||||
|
await wait(50)
|
||||||
|
|
||||||
|
async def Run1(): #From M8_5.py
|
||||||
|
left_arm.run_angle(1000, 300)
|
||||||
|
right_arm.run_angle(1000,500)
|
||||||
|
await drive_base.straight(320)
|
||||||
|
|
||||||
|
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000,500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000,-500, Stop.HOLD)
|
||||||
|
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await drive_base.straight(277)
|
||||||
|
await drive_base.turn(20)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
|
||||||
|
await drive_base.turn(-30)
|
||||||
|
right_arm.run_angle(50,500)
|
||||||
|
await drive_base.turn(45)
|
||||||
|
await drive_base.straight(-145)
|
||||||
|
await drive_base.turn(-60)
|
||||||
|
await drive_base.straight(90)
|
||||||
|
await left_arm.run_angle(1000,-450)
|
||||||
|
await drive_base.straight(-145)
|
||||||
|
await left_arm.run_angle(1000,450)
|
||||||
|
await drive_base.straight(10)
|
||||||
|
await drive_base.turn(35)
|
||||||
|
await drive_base.straight(-600)
|
||||||
|
|
||||||
|
async def Run2(): #From Heavy_lifting_final.py
|
||||||
|
await drive_base.straight(200)
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await drive_base.straight(536)
|
||||||
|
await drive_base.turn(60, Stop.HOLD)
|
||||||
|
await drive_base.straight(30)
|
||||||
|
|
||||||
|
await right_arm.run_angle(5000,2900)
|
||||||
|
await drive_base.straight(40)
|
||||||
|
await right_arm.run_angle(5000,-4000)
|
||||||
|
await drive_base.straight(-60)
|
||||||
|
|
||||||
|
await drive_base.turn(-60)
|
||||||
|
await drive_base.straight(-670)
|
||||||
|
|
||||||
|
|
||||||
|
async def Run3(): #tip the scale.py
|
||||||
|
left_arm.run_angle(600,200)
|
||||||
|
right_arm.run_angle(500,200)
|
||||||
|
await drive_base.straight(70)
|
||||||
|
|
||||||
|
await drive_base.turn(-70)
|
||||||
|
await drive_base.straight(900)
|
||||||
|
await drive_base.turn(115)
|
||||||
|
|
||||||
|
await drive_base.straight(75)
|
||||||
|
await drive_base.straight(33)
|
||||||
|
await right_arm.run_angle(500,-250)
|
||||||
|
await right_arm.run_angle(500,250)
|
||||||
|
await drive_base.turn(66)
|
||||||
|
await drive_base.straight(7)
|
||||||
|
|
||||||
|
await left_arm.run_angle(560,-390) #going down
|
||||||
|
await drive_base.turn(40) # turning right
|
||||||
|
await left_arm.run_angle(-410,-400) #lift a little bit
|
||||||
|
|
||||||
|
await drive_base.turn(-46.5) #ma din din din dun
|
||||||
|
await drive_base.turn(-40)
|
||||||
|
await drive_base.straight(900)
|
||||||
|
|
||||||
|
async def Run4(): #From Send_Over_Final.py
|
||||||
|
|
||||||
|
#Get to mission
|
||||||
|
await drive_base.straight(920)
|
||||||
|
await drive_base.turn(-90,Stop.HOLD)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
#Solve mission
|
||||||
|
drive_base.turn(-10)
|
||||||
|
await left_arm.run_angle(10000,-4000)
|
||||||
|
#Get to Red Start
|
||||||
|
await drive_base.straight(-110)
|
||||||
|
await drive_base.turn(90)
|
||||||
|
await drive_base.straight(500)
|
||||||
|
|
||||||
|
# while True:
|
||||||
|
# distance_mm = await lazer_ranger.distance()
|
||||||
|
# print('distancing...',distance_mm)
|
||||||
|
|
||||||
|
# if distance_mm < 300:
|
||||||
|
# drive_base.stop
|
||||||
|
# break
|
||||||
|
# else:
|
||||||
|
# drive_base.straight(300)
|
||||||
|
# print('running...')
|
||||||
|
# await wait(10)
|
||||||
|
await multitask(
|
||||||
|
drive_forward(),
|
||||||
|
monitor_distance()
|
||||||
|
)
|
||||||
|
|
||||||
|
#Add Rishi's code here
|
||||||
|
async def Run5():
|
||||||
|
await drive_base.straight(519)
|
||||||
|
await left_arm.run_angle(-10000, 300)
|
||||||
|
await left_arm.run_angle(10000, 600)
|
||||||
|
await drive_base.straight(160)
|
||||||
|
await drive_base.turn(-30)
|
||||||
|
await drive_base.straight(50)
|
||||||
|
await right_arm.run_angle(3000, 3000)
|
||||||
|
await drive_base.straight(-150)
|
||||||
|
await drive_base.turn(135)
|
||||||
|
await drive_base.straight(25)
|
||||||
|
await right_arm.run_angle(10000, -3000)
|
||||||
|
await drive_base.straight(-100)
|
||||||
|
await drive_base.turn(-55)
|
||||||
|
await right_arm.run_angle(10000, -3000)
|
||||||
|
await drive_base.straight(250)
|
||||||
|
await drive_base.turn(-5)
|
||||||
|
await right_arm.run_angle(10000, 7000)
|
||||||
|
await drive_base.straight(-50)
|
||||||
|
await drive_base.turn(45)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
await drive_base.turn(37)
|
||||||
|
await right_arm.run_angle(10000, -6000)
|
||||||
|
await drive_base.straight(90)
|
||||||
|
await right_arm.run_angle(3000, 3000)
|
||||||
|
await drive_base.turn(-40)
|
||||||
|
|
||||||
|
#Add - Adi's code here
|
||||||
|
async def Run6():
|
||||||
|
await drive_base.straight(420)
|
||||||
|
await right_arm.run_angle(300,-100)
|
||||||
|
await drive_base.straight(-100)
|
||||||
|
await right_arm.run_angle(300, 100)
|
||||||
|
await drive_base.straight(-350)
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
while True:
|
||||||
|
|
||||||
|
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
|
||||||
|
color_reflected_percent = await color_sensor.reflection()
|
||||||
|
print(color_reflected_percent)
|
||||||
|
|
||||||
|
|
||||||
|
if color_reflected_percent > 0: # Make sure we actually have color reflections before checking for color
|
||||||
|
color_detected = await color_sensor.color() # Moved this inside the if clause
|
||||||
|
print(f'Detected color:{color_detected} : {color_detected.h}, {color_detected.s}, {color_detected.v}')
|
||||||
|
|
||||||
|
if color_detected == Color.GREEN:
|
||||||
|
print('Running Mission 1')
|
||||||
|
await Run1()
|
||||||
|
#await celebrate_mission_complete(CelebrationSound.VICTORY_FANFARE)
|
||||||
|
elif color_detected == Color.WHITE:
|
||||||
|
print('Running Mission 2')
|
||||||
|
await Run2()
|
||||||
|
#await celebrate_mission_complete(CelebrationSound.RICKROLL_INSPIRED)
|
||||||
|
elif color_detected == Color.RED:
|
||||||
|
print('Running Mission 3')
|
||||||
|
await Run3()
|
||||||
|
#await celebrate_mission_complete(CelebrationSound.SUCCESS_CHIME )
|
||||||
|
elif color_detected == Color.YELLOW:
|
||||||
|
print('Running Mission 4')
|
||||||
|
await Run4()
|
||||||
|
#await celebrate_mission_complete(CelebrationSound.POWER_UP)
|
||||||
|
elif color_detected == Color.BLUE:
|
||||||
|
print('Running Mission 5')
|
||||||
|
await Run5()
|
||||||
|
#await celebrate_mission_complete(CelebrationSound.LEVEL_UP)
|
||||||
|
elif color_detected == Color.VIOLET:
|
||||||
|
print('Running Mission 6')
|
||||||
|
await Run6()
|
||||||
|
else:
|
||||||
|
hub.light.off()
|
||||||
|
left_motor.stop()
|
||||||
|
right_motor.stop()
|
||||||
|
color_detected = Color.NONE
|
||||||
|
else:
|
||||||
|
color_detected = Color.NONE
|
||||||
|
|
||||||
|
await wait(1000) #prevent loop from iterating fast
|
||||||
|
# Main execution loop
|
||||||
|
run_task(main())
|
||||||
208
codes_for_scrimmage/hazmat/mainhazmatUPD.py
Normal file
208
codes_for_scrimmage/hazmat/mainhazmatUPD.py
Normal file
@@ -0,0 +1,208 @@
|
|||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.tools import run_task, multitask
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
|
||||||
|
# Initialize hub and devices
|
||||||
|
hub = PrimeHub()
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B)
|
||||||
|
left_arm = Motor(Port.C)
|
||||||
|
right_arm = Motor(Port.D)
|
||||||
|
lazer_ranger = UltrasonicSensor(Port.E)
|
||||||
|
color_sensor = ColorSensor(Port.F)
|
||||||
|
|
||||||
|
# DriveBase configuration
|
||||||
|
WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels)
|
||||||
|
AXLE_TRACK = 180 # mm (distance between wheels)
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||||
|
drive_base.settings(600, 500, 300, 200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
WALL_DISTANCE = 300 # mm
|
||||||
|
|
||||||
|
async def drive_forward():
|
||||||
|
"""Drive forward continuously using DriveBase."""
|
||||||
|
drive_base.drive(400, 0)
|
||||||
|
|
||||||
|
|
||||||
|
async def monitor_distance():
|
||||||
|
"""Monitor ultrasonic sensor and stop when wall is detected."""
|
||||||
|
while True:
|
||||||
|
distance = await lazer_ranger.distance()
|
||||||
|
print('Distancing...',distance)
|
||||||
|
|
||||||
|
if distance < WALL_DISTANCE:
|
||||||
|
# Stop the drivebase
|
||||||
|
await drive_base.stop
|
||||||
|
print(f"Wall detected at {distance}mm!")
|
||||||
|
break
|
||||||
|
|
||||||
|
# Small delay to prevent overwhelming the sensor
|
||||||
|
await wait(50)
|
||||||
|
|
||||||
|
# New Section
|
||||||
|
async def Run1(): # From M8_5.py
|
||||||
|
left_arm.run_angle(1000, -300)
|
||||||
|
right_arm.run_angle(1000, 500)
|
||||||
|
await drive_base.straight(320)
|
||||||
|
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000, 500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000, 500, Stop.HOLD)
|
||||||
|
await right_arm.run_angle(5000, -500, Stop.HOLD)
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await drive_base.straight(277)
|
||||||
|
await drive_base.turn(20)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
await drive_base.turn(-30)
|
||||||
|
right_arm.run_angle(50, 500)
|
||||||
|
await drive_base.turn(45)
|
||||||
|
await drive_base.straight(-145)
|
||||||
|
await drive_base.turn(-60)
|
||||||
|
await drive_base.straight(90)
|
||||||
|
await left_arm.run_angle(1000, 450)
|
||||||
|
await drive_base.straight(-145)
|
||||||
|
await left_arm.run_angle(1000, -450)
|
||||||
|
await drive_base.straight(10)
|
||||||
|
await drive_base.turn(35)
|
||||||
|
await drive_base.straight(-600)
|
||||||
|
|
||||||
|
async def Run2(): # From Heavy_lifting_final.py
|
||||||
|
await drive_base.straight(200)
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await drive_base.straight(536)
|
||||||
|
await drive_base.turn(60, Stop.HOLD)
|
||||||
|
await drive_base.straight(30)
|
||||||
|
await right_arm.run_angle(5000, 2900)
|
||||||
|
await drive_base.straight(40)
|
||||||
|
await right_arm.run_angle(5000, -4000)
|
||||||
|
await drive_base.straight(-60)
|
||||||
|
await drive_base.turn(-60)
|
||||||
|
await drive_base.straight(-670)
|
||||||
|
|
||||||
|
|
||||||
|
async def Run3(): # tip the scale.py
|
||||||
|
left_arm.run_angle(600,-200)
|
||||||
|
right_arm.run_angle(500,200)
|
||||||
|
await drive_base.straight(70)
|
||||||
|
|
||||||
|
await drive_base.turn(-70)
|
||||||
|
await drive_base.straight(900)
|
||||||
|
await drive_base.turn(115)
|
||||||
|
|
||||||
|
await drive_base.straight(75)
|
||||||
|
await drive_base.straight(33)
|
||||||
|
await right_arm.run_angle(500,-250)
|
||||||
|
await right_arm.run_angle(500,250)
|
||||||
|
await drive_base.turn(66)
|
||||||
|
await drive_base.straight(7)
|
||||||
|
|
||||||
|
await left_arm.run_angle(560,390) #going down
|
||||||
|
print('turning now...')
|
||||||
|
await drive_base.turn(40) # turning right
|
||||||
|
await left_arm.run_angle(410,-400) #lift a little bit
|
||||||
|
await drive_base.straight(80)
|
||||||
|
await drive_base.turn(-41) #ma din din din dun 67 41 21 69
|
||||||
|
await drive_base.straight(900)
|
||||||
|
|
||||||
|
|
||||||
|
async def Run4(): # From Send_Over_Final.py
|
||||||
|
#Get to mission
|
||||||
|
await drive_base.straight(920)
|
||||||
|
await drive_base.turn(-90,Stop.HOLD)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
#Solve mission
|
||||||
|
drive_base.turn(-10)
|
||||||
|
await left_arm.run_angle(10000, 4000)
|
||||||
|
#Get to Red Start
|
||||||
|
await drive_base.straight(-110)
|
||||||
|
await drive_base.turn(90)
|
||||||
|
# while True:
|
||||||
|
# distance_mm = await lazer_ranger.distance()
|
||||||
|
# print('distancing...',distance_mm)
|
||||||
|
|
||||||
|
# if distance_mm < 300:
|
||||||
|
# drive_base.stop
|
||||||
|
# break
|
||||||
|
# else:
|
||||||
|
# drive_base.straight(300)
|
||||||
|
# print('running...')
|
||||||
|
# await wait(10)
|
||||||
|
await multitask(
|
||||||
|
drive_forward(),
|
||||||
|
monitor_distance()
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# Add Rishi's code here
|
||||||
|
async def Run5():
|
||||||
|
await drive_base.straight(600)
|
||||||
|
await drive_base.straight(-100)
|
||||||
|
await drive_base.straight(150)
|
||||||
|
await drive_base.turn(60)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
await drive_base.turn(-86)
|
||||||
|
await drive_base.straight(120)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(-200)
|
||||||
|
await drive_base.turn(75)
|
||||||
|
|
||||||
|
# Add - Adi's code here
|
||||||
|
async def Run6():
|
||||||
|
await drive_base.straight(750)
|
||||||
|
await drive_base.straight(-650)
|
||||||
|
|
||||||
|
|
||||||
|
# Function to classify color based on HSV
|
||||||
|
def detect_color(h, s, v, reflected):
|
||||||
|
if reflected > 4:
|
||||||
|
if h < 4 or h > 350: # red
|
||||||
|
return "Red"
|
||||||
|
elif 3 < h < 40 and s > 70: # orange
|
||||||
|
return "Orange"
|
||||||
|
elif 47 < h < 56: # yellow
|
||||||
|
return "Yellow"
|
||||||
|
elif 70 < h < 160: # green - do it vertically not horizontally for accuracy
|
||||||
|
return "Green"
|
||||||
|
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
|
||||||
|
return "Blue"
|
||||||
|
elif 260 < h < 350: # purple
|
||||||
|
return "Purple"
|
||||||
|
else:
|
||||||
|
return "Unknown"
|
||||||
|
return "Unknown"
|
||||||
|
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
while True:
|
||||||
|
h, s, v = await color_sensor.hsv()
|
||||||
|
reflected = await color_sensor.reflection()
|
||||||
|
color = detect_color(h, s, v, reflected)
|
||||||
|
|
||||||
|
if color == "Red":
|
||||||
|
print('Running Mission 3')
|
||||||
|
await Run3() #red
|
||||||
|
elif color == "Orange":
|
||||||
|
print('Running Mission 6')
|
||||||
|
await Run6() #orange
|
||||||
|
elif color == "Yellow":
|
||||||
|
print('Running Mission 4')
|
||||||
|
await Run4() #yellow
|
||||||
|
elif color == "Green":
|
||||||
|
print('Running Mission 1')
|
||||||
|
await Run1() #green - vertically
|
||||||
|
elif color == "Blue":
|
||||||
|
print('Running Mission 5')
|
||||||
|
await Run5() #blue - vertically
|
||||||
|
elif color == "Purple":
|
||||||
|
print('Running Mission 2')
|
||||||
|
await Run2() #purple - vertically
|
||||||
|
else:
|
||||||
|
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||||
|
await wait(10)
|
||||||
|
# Run the main function
|
||||||
|
run_task(main())
|
||||||
42
codes_for_scrimmage/hazmat/tip the scale.py
Normal file
42
codes_for_scrimmage/hazmat/tip the scale.py
Normal file
@@ -0,0 +1,42 @@
|
|||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.tools import run_task, multitask
|
||||||
|
|
||||||
|
hub = PrimeHub()
|
||||||
|
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B)
|
||||||
|
|
||||||
|
|
||||||
|
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_arm = Motor(Port.D)
|
||||||
|
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||||
|
drive_base.settings(300,1000,300,200)
|
||||||
|
|
||||||
|
#drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
left_arm.run_angle(500,200)
|
||||||
|
right_arm.run_angle(500,200)
|
||||||
|
await drive_base.straight(70)
|
||||||
|
|
||||||
|
await drive_base.turn(-55)
|
||||||
|
await drive_base.straight(900)
|
||||||
|
await drive_base.turn(92.5)
|
||||||
|
|
||||||
|
await drive_base.straight(75)
|
||||||
|
await drive_base.straight(21)
|
||||||
|
await right_arm.run_angle(500,-250)
|
||||||
|
await right_arm.run_angle(500,250)
|
||||||
|
await drive_base.turn(55)
|
||||||
|
|
||||||
|
await left_arm.run_angle(300,-400)
|
||||||
|
|
||||||
|
await drive_base.turn(46.5)
|
||||||
|
await drive_base.turn(-40)
|
||||||
|
await drive_base.straight(900)
|
||||||
|
run_task(main())
|
||||||
@@ -203,27 +203,29 @@ async def Run3():
|
|||||||
|
|
||||||
async def Run4():
|
async def Run4():
|
||||||
await drive_base.straight(519)
|
await drive_base.straight(519)
|
||||||
await left_arm.run_angle(300, -100)
|
await arm_motor_left.run_angle(-10000, 300)
|
||||||
await left_arm.run_angle(300, 500)
|
await arm_motor_left.run_angle(10000, 600)
|
||||||
await drive_base.straight(180)
|
await drive_base.straight(160)
|
||||||
await drive_base.turn(-37)
|
await drive_base.turn(-30)
|
||||||
await drive_base.straight(50)
|
await drive_base.straight(50)
|
||||||
await right_arm.run_angle(300, -400)
|
await arm_motor.run_angle(3000, 3000)
|
||||||
await drive_base.straight(-150)
|
await drive_base.straight(-150)
|
||||||
await drive_base.turn(125)
|
await drive_base.turn(135)
|
||||||
await drive_base.straight(50)
|
await drive_base.straight(50)
|
||||||
await right_arm.run_angle(300, 400)
|
await arm_motor.run_angle(10000, -3000)
|
||||||
await drive_base.straight(-75)
|
await drive_base.straight(-100)
|
||||||
await right_arm.run_angle(300, 300)
|
await drive_base.turn(-54)
|
||||||
await drive_base.turn(-40)
|
await arm_motor.run_angle(10000, -3000)
|
||||||
await drive_base.straight(250)
|
await drive_base.straight(250)
|
||||||
await right_arm.run_angle(100, -300)
|
await drive_base.turn(-5)
|
||||||
await drive_base.straight(30)
|
await arm_motor.run_angle(10000, 7000)
|
||||||
await right_arm.run_angle(50,-250)
|
await drive_base.straight(-50)
|
||||||
await drive_base.turn(30)
|
await drive_base.turn(68)
|
||||||
await drive_base.straight(-200)
|
await arm_motor.run_angle(10000, -6000)
|
||||||
await drive_base.turn(-50)
|
await drive_base.straight(200)
|
||||||
await drive_base.straight(-800)
|
await arm_motor.run_angle(10000, 4000)
|
||||||
|
await drive_base.turn(-40)
|
||||||
|
await arm_motor.run_angle(10000, 7000)
|
||||||
|
|
||||||
async def Run5():
|
async def Run5():
|
||||||
await drive_base.straight(420)
|
await drive_base.straight(420)
|
||||||
|
|||||||
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())
|
||||||
33
missions/HEAVY_LIFTING_UPD.py
Normal file
33
missions/HEAVY_LIFTING_UPD.py
Normal file
@@ -0,0 +1,33 @@
|
|||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.tools import run_task, multitask
|
||||||
|
|
||||||
|
hub = PrimeHub()
|
||||||
|
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B)
|
||||||
|
|
||||||
|
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_arm = Motor(Port.D)
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||||
|
drive_base.settings(600,500,300,200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
await drive_base.straight(200)
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await drive_base.straight(536)
|
||||||
|
await drive_base.turn(60, Stop.HOLD)
|
||||||
|
await drive_base.straight(30)
|
||||||
|
|
||||||
|
await right_arm.run_angle(5000,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())
|
||||||
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/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())
|
||||||
38
missions/New_MapReveal/New_MapReveal_Mineshaft.py
Normal file
38
missions/New_MapReveal/New_MapReveal_Mineshaft.py
Normal file
@@ -0,0 +1,38 @@
|
|||||||
|
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())
|
||||||
@@ -20,10 +20,10 @@ drive_base.use_gyro(True)
|
|||||||
|
|
||||||
|
|
||||||
async def main():
|
async def main():
|
||||||
await drive_base.straight(420)
|
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 right_arm.run_angle(300,100)
|
||||||
|
await drive_base.straight(-100)
|
||||||
|
await right_arm.run_angle(300,-100)
|
||||||
await drive_base.straight(-350)
|
await drive_base.straight(-350)
|
||||||
|
|
||||||
run_task(main())
|
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())
|
||||||
@@ -23,9 +23,9 @@ async def main():
|
|||||||
await drive_base.straight(915)
|
await drive_base.straight(915)
|
||||||
await drive_base.turn(-90)
|
await drive_base.turn(-90)
|
||||||
await drive_base.straight(60)
|
await drive_base.straight(60)
|
||||||
await left_arm.run_angle(10000,-15000)
|
await left_arm.run_angle(10000,-4000)
|
||||||
await drive_base.straight(-60)
|
await drive_base.straight(-90)
|
||||||
await drive_base.turn(85)
|
await drive_base.turn(80)
|
||||||
await drive_base.straight(2000)
|
await drive_base.straight(2000)
|
||||||
|
|
||||||
run_task(main())
|
run_task(main())
|
||||||
46
missions/Send_Over_Final.py
Normal file
46
missions/Send_Over_Final.py
Normal file
@@ -0,0 +1,46 @@
|
|||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.tools import run_task, multitask
|
||||||
|
|
||||||
|
hub = PrimeHub()
|
||||||
|
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B)
|
||||||
|
|
||||||
|
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_arm = Motor(Port.D)
|
||||||
|
lazer_ranger = UltrasonicSensor(Port.E)
|
||||||
|
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
||||||
|
|
||||||
|
drive_base.settings(600,500,300,200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
#Get to mission
|
||||||
|
await drive_base.straight(920)
|
||||||
|
await drive_base.turn(-90,Stop.HOLD)
|
||||||
|
await drive_base.straight(65)
|
||||||
|
#Solve mission
|
||||||
|
drive_base.turn(-10)
|
||||||
|
await left_arm.run_angle(10000,-4000)
|
||||||
|
#Get to Red Start
|
||||||
|
await drive_base.straight(-110)
|
||||||
|
await drive_base.turn(90)
|
||||||
|
await drive_base.straight(500)
|
||||||
|
while True:
|
||||||
|
distance_mm = await lazer_ranger.distance()
|
||||||
|
print('distancing...',distance_mm)
|
||||||
|
|
||||||
|
if distance_mm < 300:
|
||||||
|
drive_base.stop
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
drive_base.straight(300)
|
||||||
|
print('running...')
|
||||||
|
await wait(10)
|
||||||
|
|
||||||
|
run_task(main())
|
||||||
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())
|
||||||
@@ -15,28 +15,34 @@ left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
|
|||||||
right_arm = Motor(Port.D)
|
right_arm = Motor(Port.D)
|
||||||
|
|
||||||
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)
|
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():
|
async def main():
|
||||||
left_arm.run_angle(500,200)
|
left_arm.run_angle(600,200)
|
||||||
right_arm.run_angle(500,200)
|
right_arm.run_angle(500,200)
|
||||||
await drive_base.straight(70)
|
await drive_base.straight(70)
|
||||||
|
|
||||||
await drive_base.turn(-55)
|
await drive_base.turn(-70)
|
||||||
await drive_base.straight(900)
|
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(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 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.turn(-40)
|
||||||
await drive_base.straight(900)
|
await drive_base.straight(900)
|
||||||
run_task(main())
|
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())
|
||||||
Reference in New Issue
Block a user