Compare commits
91 Commits
ee274df574
...
dev
| Author | SHA1 | Date | |
|---|---|---|---|
| 808f797bc3 | |||
| 2a3bc07074 | |||
| 59ca332b2a | |||
| a98a2186e7 | |||
| acae133c65 | |||
| 896e240935 | |||
| 34d732a5d4 | |||
| befae6c40b | |||
| f4c62b0109 | |||
| 93e64b3e65 | |||
| 64598ef144 | |||
| 3d023f045f | |||
| 384c1732cf | |||
| 12edbcdfd8 | |||
| 83dee33058 | |||
| d41e577b35 | |||
| f6f3ef600a | |||
| b137f382b0 | |||
| 0bc86b032d | |||
| ce94cea83f | |||
| f4f66fe2b4 | |||
| 43e0e93934 | |||
| 71c66259ce | |||
| 8092a8ec79 | |||
| 48719fd6d9 | |||
| 299644e6f5 | |||
| 51a6e31170 | |||
| 5522503b28 | |||
| b4fbd57aa8 | |||
| 73e35a62cf | |||
| bb703e626d | |||
| 3621a355b1 | |||
| 5926554f55 | |||
| 94a6b0d7f1 | |||
| d345fab7f7 | |||
| 962eab1e03 | |||
| 5dd1a7cd56 | |||
| 37e0dd4e51 | |||
| f141e9b517 | |||
| a19ee01d48 | |||
| 296f653670 | |||
| c809f1ae9a | |||
| c0329e4f42 | |||
| e463010a45 | |||
| 1e0fb5eae6 | |||
| ae329f8b91 | |||
| bfc3ef416e | |||
| 111dc0d5aa | |||
| 68ca2b744b | |||
| 4a266358b8 | |||
| 72f26eb741 | |||
| 718d928d78 | |||
| f1e8bdc0ea | |||
| cea616f13e | |||
| 63f7c48a64 | |||
| 9f201d78a1 | |||
| 795831b932 | |||
| 4a1ad1c33e | |||
| 5644ad15e9 | |||
| 3a296edc34 | |||
| 9e1caaabed | |||
| 2b3ab21252 | |||
| 894df75b42 | |||
| 1735e2bc34 | |||
| d85a427371 | |||
| b81c7bab63 | |||
| f2edae8e3f | |||
| e1d1c612c9 | |||
| 8374311800 | |||
| 1ddab4463e | |||
| 81842d5a75 | |||
| c6c117daf7 | |||
| 29a16afbf8 | |||
| 88858f1d39 | |||
| a11bd235a2 | |||
| eead961fa0 | |||
| c54feaad7d | |||
| 8d61cf2019 | |||
| d65be19478 | |||
| 7d13418fea | |||
| 7bfbf49fcf | |||
| 1c03cd11ff | |||
| 9f4cfc193e | |||
| d72cf75118 | |||
| aa0949368c | |||
| 201de8e185 | |||
| 038742e173 | |||
| 53517762ef | |||
| 1c0099bb0a | |||
| ece88cf3ad | |||
| 8f30669980 |
49
hazmat_scrimmage/PynamicsBatteryDiagnostics.py
Normal file
49
hazmat_scrimmage/PynamicsBatteryDiagnostics.py
Normal file
@@ -0,0 +1,49 @@
|
|||||||
|
from pybricks.tools import wait
|
||||||
|
import umath
|
||||||
|
class BatteryDiagnostics:
|
||||||
|
def __init__(self, hub):
|
||||||
|
self.voltage = 0
|
||||||
|
self.current = 0
|
||||||
|
self.hub = hub
|
||||||
|
def printVoltage(self):
|
||||||
|
self.voltage = self.hub.battery.voltage()
|
||||||
|
if self.voltage > 7800:
|
||||||
|
print(f"Battery voltage is sufficient: {self.voltage}")
|
||||||
|
elif self.voltage < 7800 :
|
||||||
|
print(f"Charging needed: {self.voltage}")
|
||||||
|
def printCurrent(self):
|
||||||
|
self.current = self.hub.battery.current()
|
||||||
|
print("Battery current:", self.current)
|
||||||
|
def printAll(self):
|
||||||
|
timeelapsed = 0
|
||||||
|
voltageList = []
|
||||||
|
currentList = []
|
||||||
|
while True:
|
||||||
|
print("\n\n\n\n\n")
|
||||||
|
self.printVoltage()
|
||||||
|
voltageList.append(self.voltage)
|
||||||
|
self.printCurrent()
|
||||||
|
currentList.append(self.current)
|
||||||
|
wait(50)
|
||||||
|
timeelapsed += 50
|
||||||
|
|
||||||
|
if(timeelapsed >= 3000):
|
||||||
|
break
|
||||||
|
print("--------------FINAL RESULTS OF BATTERY DIAGNOSTICS---------------")
|
||||||
|
print("Voltage deviation:", self.stdev(voltageList))
|
||||||
|
print("Current deviation:", self.stdev(currentList))
|
||||||
|
def stdev(self, vals):
|
||||||
|
DATA = vals
|
||||||
|
if len(DATA) < 2:
|
||||||
|
return 0
|
||||||
|
# Calculate the mean
|
||||||
|
MEAN = sum(DATA) / len(DATA)
|
||||||
|
|
||||||
|
# Calculate the variance (sum of squared differences from the mean, divided by n-1 for sample standard deviation)
|
||||||
|
VARIANCE = sum([(x - MEAN) ** 2 for x in DATA]) / float(len(DATA) - 1)
|
||||||
|
|
||||||
|
# Calculate the standard deviation (square root of the variance)
|
||||||
|
STD_DEV_MANUAL = umath.sqrt(VARIANCE)
|
||||||
|
|
||||||
|
|
||||||
|
return (STD_DEV_MANUAL)
|
||||||
48
hazmat_scrimmage/PynamicsColorSensorDiagnostics.py
Normal file
48
hazmat_scrimmage/PynamicsColorSensorDiagnostics.py
Normal file
@@ -0,0 +1,48 @@
|
|||||||
|
from pybricks.parameters import Color, Port, Stop
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
|
||||||
|
class ColorSensorDiagnostics:
|
||||||
|
def __init__(self, hub, colorsensorclass):
|
||||||
|
self.colorsensor = None
|
||||||
|
self.colorsensorclass = colorsensorclass
|
||||||
|
self.PORT_MAP = {
|
||||||
|
"A": Port.A,
|
||||||
|
"B": Port.B,
|
||||||
|
"C": Port.C,
|
||||||
|
"D": Port.D,
|
||||||
|
"E": Port.E,
|
||||||
|
"F": Port.F,
|
||||||
|
}
|
||||||
|
def initializeColorSensor(self):
|
||||||
|
VALID_PORTS = {"A", "B", "C", "D", "E", "F"}
|
||||||
|
while True:
|
||||||
|
colorinput = input(
|
||||||
|
"This will test your color sensor.\n"
|
||||||
|
"Enter the port for the color sensor you would like to test (A, B, C, D, E, or F): "
|
||||||
|
).strip().upper()
|
||||||
|
if colorinput not in VALID_PORTS:
|
||||||
|
print("Invalid port. Please enter A-F.")
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
if self.colorsensor is None:
|
||||||
|
self.colorsensor = self.colorsensorclass(self.PORT_MAP[colorinput])
|
||||||
|
print(f"Color Sensor initialized on port {colorinput}.")
|
||||||
|
else:
|
||||||
|
print(f"Reusing existing color sensor on port {colorinput}.")
|
||||||
|
break
|
||||||
|
|
||||||
|
except OSError as e:
|
||||||
|
if e.errno == 16: # EBUSY
|
||||||
|
print(f"Port {colorinput} is already in use. Reusing existing color sensor.")
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
print(f"Error initializing color sensor on port {colorinput}: {e}")
|
||||||
|
print("Make sure a color sensor is actually connected to this port.")
|
||||||
|
self.colorsensor = None
|
||||||
|
self.colorsensor.detectable_colors([Color.RED, Color.YELLOW, Color.GREEN, Color.BLUE, Color.WHITE, Color.NONE])
|
||||||
|
def printAll(self):
|
||||||
|
self.initializeColorSensor()
|
||||||
|
stopwatch = StopWatch()
|
||||||
|
while stopwatch.time() < 5000:
|
||||||
|
print("HSV output:", self.colorsensor.hsv())
|
||||||
|
print("Detected color:", self.colorsensor.color())
|
||||||
165
hazmat_scrimmage/PynamicsDriveBaseDiagnostics.py
Normal file
165
hazmat_scrimmage/PynamicsDriveBaseDiagnostics.py
Normal file
@@ -0,0 +1,165 @@
|
|||||||
|
from pybricks.parameters import Direction, Port, Side, Stop
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
|
||||||
|
from usys import stdin
|
||||||
|
from uselect import poll
|
||||||
|
|
||||||
|
class DriveBaseDiagnostics:
|
||||||
|
def __init__(self, hub, motorclass, dbclass):
|
||||||
|
self.hub = hub
|
||||||
|
self.motorclass = motorclass
|
||||||
|
self.dbclass = dbclass
|
||||||
|
self.drive_base = None
|
||||||
|
self.PORT_MAP = {
|
||||||
|
"A": Port.A,
|
||||||
|
"B": Port.B,
|
||||||
|
"C": Port.C,
|
||||||
|
"D": Port.D,
|
||||||
|
"E": Port.E,
|
||||||
|
"F": Port.F,
|
||||||
|
}
|
||||||
|
def initializeDriveBase(self):
|
||||||
|
|
||||||
|
print("DriveBase setup:")
|
||||||
|
print("1 = Load from savefile (paste JSON)")
|
||||||
|
print("2 = Use defaults")
|
||||||
|
print("3 = Enter values manually")
|
||||||
|
|
||||||
|
choice = input("Choose an option: ")
|
||||||
|
|
||||||
|
# Default values
|
||||||
|
WHEEL_DIAMETER = 68.8
|
||||||
|
AXLE_TRACK = 180
|
||||||
|
DRIVE_SETTINGS = (600, 2000, 300, 2000)
|
||||||
|
|
||||||
|
# Motor ports (None until set)
|
||||||
|
leftmotorport = Port.A
|
||||||
|
rightmotorport = Port.B
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# OPTION 1: LOAD SAVEFILE
|
||||||
|
# -----------------------------
|
||||||
|
if choice == "1":
|
||||||
|
print("Paste JSON:")
|
||||||
|
raw = input("> ")
|
||||||
|
|
||||||
|
# --- wheel ---
|
||||||
|
if "\"wheel\"" in raw:
|
||||||
|
part = raw.split("\"wheel\"")[1]
|
||||||
|
part = part.split(":")[1]
|
||||||
|
part = part.split(",")[0]
|
||||||
|
WHEEL_DIAMETER = float(part)
|
||||||
|
|
||||||
|
# --- axle ---
|
||||||
|
if "\"axle\"" in raw:
|
||||||
|
part = raw.split("\"axle\"")[1]
|
||||||
|
part = part.split(":")[1]
|
||||||
|
part = part.split(",")[0]
|
||||||
|
AXLE_TRACK = float(part)
|
||||||
|
|
||||||
|
# --- settings ---
|
||||||
|
if "\"settings\"" in raw:
|
||||||
|
part = raw.split("\"settings\"")[1]
|
||||||
|
part = part.split("[")[1]
|
||||||
|
part = part.split("]")[0]
|
||||||
|
nums = part.split(",")
|
||||||
|
DRIVE_SETTINGS = (int(nums[0]), int(nums[1]), int(nums[2]), int(nums[3]))
|
||||||
|
|
||||||
|
# --- left motor port ---
|
||||||
|
if "\"left_port\"" in raw:
|
||||||
|
part = raw.split("\"left_port\"")[1]
|
||||||
|
part = part.split("\"")[1] # first quoted value
|
||||||
|
leftmotorport = part
|
||||||
|
|
||||||
|
# --- right motor port ---
|
||||||
|
if "\"right_port\"" in raw:
|
||||||
|
part = raw.split("\"right_port\"")[1]
|
||||||
|
part = part.split("\"")[1]
|
||||||
|
rightmotorport = part
|
||||||
|
|
||||||
|
|
||||||
|
print("Loaded config.")
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# OPTION 3: MANUAL ENTRY
|
||||||
|
# -----------------------------
|
||||||
|
elif choice == "3":
|
||||||
|
WHEEL_DIAMETER = float(input("Wheel diameter: "))
|
||||||
|
AXLE_TRACK = float(input("Axle track: "))
|
||||||
|
|
||||||
|
print("Enter drive settings:")
|
||||||
|
s1 = int(input("Straight speed: "))
|
||||||
|
s2 = int(input("Straight accel: "))
|
||||||
|
s3 = int(input("Turn rate: "))
|
||||||
|
s4 = int(input("Turn accel: "))
|
||||||
|
DRIVE_SETTINGS = (s1, s2, s3, s4)
|
||||||
|
|
||||||
|
# Ask for motor ports HERE (manual only)
|
||||||
|
leftmotorport = input("Left motor port: ")
|
||||||
|
rightmotorport = input("Right motor port: ")
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# OPTION 2: DEFAULTS
|
||||||
|
# -----------------------------
|
||||||
|
else:
|
||||||
|
pass
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# CREATE MOTORS
|
||||||
|
# -----------------------------
|
||||||
|
left_motor = self.motorclass(leftmotorport, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = self.motorclass(rightmotorport, Direction.CLOCKWISE)
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# CREATE DRIVEBASE
|
||||||
|
# -----------------------------
|
||||||
|
self.drive_base = self.dbclass(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||||
|
self.drive_base.settings(*DRIVE_SETTINGS)
|
||||||
|
self.drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
print("DriveBase initialized.")
|
||||||
|
return DRIVE_SETTINGS
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def printAll(self):
|
||||||
|
self.driveRobot()
|
||||||
|
def driveRobot(self):
|
||||||
|
drivesettings = self.initializeDriveBase()
|
||||||
|
print(drivesettings)
|
||||||
|
keyboard = poll()
|
||||||
|
keyboard.register(stdin)
|
||||||
|
|
||||||
|
last_key_time = StopWatch()
|
||||||
|
last_key_time.reset()
|
||||||
|
|
||||||
|
while True:
|
||||||
|
key = None
|
||||||
|
|
||||||
|
# Check for keypress immediately
|
||||||
|
if keyboard.poll(0):
|
||||||
|
key = stdin.read(1).upper()
|
||||||
|
last_key_time.reset()
|
||||||
|
|
||||||
|
# Process key
|
||||||
|
if key:
|
||||||
|
if key == "W":
|
||||||
|
self.drive_base.drive(drivesettings[0], 0)
|
||||||
|
elif key == "A":
|
||||||
|
self.drive_base.drive(drivesettings[0], -180)
|
||||||
|
elif key == "S":
|
||||||
|
self.drive_base.drive(-drivesettings[0], 0)
|
||||||
|
elif key == "D":
|
||||||
|
self.drive_base.drive(drivesettings[0], 180)
|
||||||
|
elif key == "X":
|
||||||
|
break
|
||||||
|
|
||||||
|
# Auto-stop if no key for 120 ms
|
||||||
|
if last_key_time.time() > 120:
|
||||||
|
self.drive_base.stop()
|
||||||
|
|
||||||
|
# Tiny manual delay to avoid 100% CPU
|
||||||
|
# (much faster than wait())
|
||||||
|
for _ in range(200):
|
||||||
|
pass
|
||||||
74
hazmat_scrimmage/PynamicsFullDiagnostics.py
Normal file
74
hazmat_scrimmage/PynamicsFullDiagnostics.py
Normal file
@@ -0,0 +1,74 @@
|
|||||||
|
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()
|
||||||
|
from BatteryDiagnostics import BatteryDiagnostics
|
||||||
|
from MotorDiagnostics import MotorDiagnostics
|
||||||
|
from ColorSensorDiagnostics import ColorSensorDiagnostics
|
||||||
|
from DriveBaseDiagnostics import DriveBaseDiagnostics
|
||||||
|
from HubDiagnostics import HubDiagnostics
|
||||||
|
battery = BatteryDiagnostics(HUB)
|
||||||
|
motor = MotorDiagnostics(HUB, Motor)
|
||||||
|
colorsensor = ColorSensorDiagnostics(HUB, ColorSensor)
|
||||||
|
hubdiags = HubDiagnostics(HUB)
|
||||||
|
drivebase = DriveBaseDiagnostics(HUB, Motor, DriveBase)
|
||||||
|
CLEARCONFIRM = input("Clear the console before proceeding? Y/N (default: yes): ")
|
||||||
|
if(CLEARCONFIRM == "Y" or CLEARCONFIRM == "y" or CLEARCONFIRM == "yes" or CLEARCONFIRM == ""):
|
||||||
|
print("Clearing console... \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n")
|
||||||
|
print("""
|
||||||
|
███████████ █████ █████ ██████ █████ █████████ ██████ ██████ █████ █████████ █████████
|
||||||
|
▒▒███▒▒▒▒▒███▒▒███ ▒▒███ ▒▒██████ ▒▒███ ███▒▒▒▒▒███ ▒▒██████ ██████ ▒▒███ ███▒▒▒▒▒███ ███▒▒▒▒▒███
|
||||||
|
▒███ ▒███ ▒▒███ ███ ▒███▒███ ▒███ ▒███ ▒███ ▒███▒█████▒███ ▒███ ███ ▒▒▒ ▒███ ▒▒▒
|
||||||
|
▒██████████ ▒▒█████ ▒███▒▒███▒███ ▒███████████ ▒███▒▒███ ▒███ ▒███ ▒███ ▒▒█████████
|
||||||
|
▒███▒▒▒▒▒▒ ▒▒███ ▒███ ▒▒██████ ▒███▒▒▒▒▒███ ▒███ ▒▒▒ ▒███ ▒███ ▒███ ▒▒▒▒▒▒▒▒███
|
||||||
|
▒███ ▒███ ▒███ ▒▒█████ ▒███ ▒███ ▒███ ▒███ ▒███ ▒▒███ ███ ███ ▒███
|
||||||
|
█████ █████ █████ ▒▒█████ █████ █████ █████ █████ █████ ▒▒█████████ ▒▒█████████
|
||||||
|
▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒
|
||||||
|
|
||||||
|
The free and open source diagnostics tool for the LEGO® Education SPIKE™ Prime robots, designed for FIRST Lego League.
|
||||||
|
Developed by Team 65266, Lego Dynamics.
|
||||||
|
Please set your window size to 90% on small screens for best results with the ASCII art.
|
||||||
|
"""
|
||||||
|
)
|
||||||
|
while True:
|
||||||
|
|
||||||
|
print("\nWhich diagnostic do you want to perform?")
|
||||||
|
print("Enter 'b' for battery diagnostics")
|
||||||
|
print("Enter 'm' for motor diagnostics")
|
||||||
|
print("Enter 'cs' for color sensor diagnostics")
|
||||||
|
print("Enter 'h' for hub diagnostics")
|
||||||
|
print("Enter 'db' for drive base diagnostics")
|
||||||
|
print("Enter 'q' to quit")
|
||||||
|
|
||||||
|
choice = input("Your choice: ").strip().lower()
|
||||||
|
|
||||||
|
if choice == "b":
|
||||||
|
print("-----------------------BATTERY DIAGNOSTICS-----------------------")
|
||||||
|
print("This test will check the battery voltage and current. It will measure these over a period of 3 seconds and provide average and deviation values. Your voltage should be above 7800 mV for optimal performance.")
|
||||||
|
input("Press Enter to begin the battery diagnostics.")
|
||||||
|
battery.printAll()
|
||||||
|
print("Battery diagnostics completed.")
|
||||||
|
|
||||||
|
elif choice == "m":
|
||||||
|
print("------------------------MOTOR DIAGNOSTICS------------------------")
|
||||||
|
motor.fullTest()
|
||||||
|
print("Motor diagnostics completed.")
|
||||||
|
elif choice == "h":
|
||||||
|
print("-------------------------HUB DIAGNOSTICS-------------------------")
|
||||||
|
hubdiags.printAll()
|
||||||
|
print("Hub diagnostics completed.")
|
||||||
|
elif choice == "q":
|
||||||
|
print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!")
|
||||||
|
break
|
||||||
|
elif choice == "db":
|
||||||
|
print("----------------------DRIVEBASE DIAGNOSTICS----------------------")
|
||||||
|
drivebase.printAll()
|
||||||
|
elif choice == "cs":
|
||||||
|
print("---------------------COLOR SENSOR DIAGNOSTICS---------------------")
|
||||||
|
colorsensor.printAll()
|
||||||
|
print("Color sensor diagnostics completed.")
|
||||||
|
|
||||||
|
else:
|
||||||
|
print("Invalid choice. Please enter 'b', 'm', or 'q'.")
|
||||||
35
hazmat_scrimmage/PynamicsHubDiagnostics.py
Normal file
35
hazmat_scrimmage/PynamicsHubDiagnostics.py
Normal file
@@ -0,0 +1,35 @@
|
|||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
from pybricks.parameters import Port
|
||||||
|
from pybricks import version
|
||||||
|
import OtherFunctions as debug
|
||||||
|
import usys
|
||||||
|
|
||||||
|
|
||||||
|
class HubDiagnostics:
|
||||||
|
def __init__(self, hub):
|
||||||
|
self.hub = hub
|
||||||
|
self.port_map = {
|
||||||
|
"A": Port.A,
|
||||||
|
"B": Port.B,
|
||||||
|
"C": Port.C,
|
||||||
|
"D": Port.D,
|
||||||
|
"E": Port.E,
|
||||||
|
"F": Port.F,
|
||||||
|
}
|
||||||
|
def printAbout(self):
|
||||||
|
print("Pybricks version information:", version)
|
||||||
|
print("MicroPython information:", usys.implementation)
|
||||||
|
print("MicroPython version:", usys.version)
|
||||||
|
def testLightSources(self, verbose):
|
||||||
|
v = verbose
|
||||||
|
self.hub.display.off()
|
||||||
|
for x in range(5):
|
||||||
|
for y in range(5):
|
||||||
|
debug.log(f"Turning on pixel at position {x}, {y}...", v)
|
||||||
|
self.hub.display.pixel(x, y, brightness=100)
|
||||||
|
wait(100)
|
||||||
|
debug.log(f"Turning off pixel at position {x}, {y}...", v)
|
||||||
|
self.hub.display.pixel(x, y, brightness=0)
|
||||||
|
def printAll(self):
|
||||||
|
self.printAbout()
|
||||||
|
self.testLightSources(False)
|
||||||
136
hazmat_scrimmage/PynamicsMotorDiagnostics.py
Normal file
136
hazmat_scrimmage/PynamicsMotorDiagnostics.py
Normal file
@@ -0,0 +1,136 @@
|
|||||||
|
from pybricks.parameters import Direction, Port, Stop
|
||||||
|
from pybricks.tools import wait, StopWatch
|
||||||
|
import umath
|
||||||
|
class MotorDiagnostics:
|
||||||
|
def __init__(self, hub, motorclass):
|
||||||
|
self.testmotor = None
|
||||||
|
self.motorclass = motorclass
|
||||||
|
self.port_map = {
|
||||||
|
"A": Port.A,
|
||||||
|
"B": Port.B,
|
||||||
|
"C": Port.C,
|
||||||
|
"D": Port.D,
|
||||||
|
"E": Port.E,
|
||||||
|
"F": Port.F,
|
||||||
|
}
|
||||||
|
|
||||||
|
def stdev(self, vals):
|
||||||
|
DATA = vals
|
||||||
|
if len(DATA) < 2:
|
||||||
|
return 0
|
||||||
|
# Calculate the mean
|
||||||
|
MEAN = sum(DATA) / len(DATA)
|
||||||
|
|
||||||
|
# Calculate the variance (sum of squared differences from the mean, divided by n-1 for sample standard deviation)
|
||||||
|
VARIANCE = sum([(x - MEAN) ** 2 for x in DATA]) / float(len(DATA) - 1)
|
||||||
|
|
||||||
|
# Calculate the standard deviation (square root of the variance)
|
||||||
|
STD_DEV_MANUAL = umath.sqrt(VARIANCE)
|
||||||
|
|
||||||
|
|
||||||
|
return (STD_DEV_MANUAL)
|
||||||
|
def health_score(self, desired, avg_speed, stdev_speed, avg_load):
|
||||||
|
# Speed accuracy: penalize % error
|
||||||
|
ACCURACY = max(0, 100 - abs(avg_speed - desired) / desired * 100)
|
||||||
|
|
||||||
|
# Stability: penalize deviation relative to desired
|
||||||
|
STABILITY = max(0, 100 - (stdev_speed / desired) * 100)
|
||||||
|
|
||||||
|
# Normalize load: map 10 to 20 as baseline (around 0%), 200 as max (around 100%)
|
||||||
|
BASELINE = 15 # midpoint of idle range
|
||||||
|
MAX_OBSERVED = 200 # heavy load/stall
|
||||||
|
NORMALIZED_LOAD = max(0, avg_load - BASELINE)
|
||||||
|
LOAD_PCT = min(100, (NORMALIZED_LOAD / (MAX_OBSERVED - BASELINE)) * 100)
|
||||||
|
|
||||||
|
LOAD_SCORE = max(0, 100 - LOAD_PCT)
|
||||||
|
|
||||||
|
# Final score: average of the three
|
||||||
|
return (ACCURACY + STABILITY + LOAD_SCORE) / 3
|
||||||
|
|
||||||
|
def initializeMotor(self):
|
||||||
|
VALID_PORTS = {"A", "B", "C", "D", "E", "F"}
|
||||||
|
while True:
|
||||||
|
motorinput = input(
|
||||||
|
"This test will run your motor at 3 speeds: 180, 540, and 1000 degrees per second.\n"
|
||||||
|
"Please make sure your motor is not under any load (for example, your hand) during the test.\n"
|
||||||
|
"If you want to test the wheel's load, note that this will affect the load measurements.\n"
|
||||||
|
"Enter the port for the motor you would like to test (A, B, C, D, E, or F): "
|
||||||
|
).strip().upper()
|
||||||
|
if motorinput not in VALID_PORTS:
|
||||||
|
print("That is not a valid port. Please enter A-F.")
|
||||||
|
continue
|
||||||
|
try:
|
||||||
|
# Only create a new Motor if we don't already have one
|
||||||
|
if self.testmotor is None:
|
||||||
|
self.testmotor = self.motorclass(self.port_map[motorinput])
|
||||||
|
print(f"Motor initialized on port {motorinput}.")
|
||||||
|
else:
|
||||||
|
print(f"Reusing existing motor on port {motorinput}.")
|
||||||
|
break
|
||||||
|
|
||||||
|
except OSError as e:
|
||||||
|
if e.errno == 16: # EBUSY
|
||||||
|
print(f"Port {motorinput} is already in use. Reusing existing motor.")
|
||||||
|
# Do not overwrite self.testmotor here — keep the existing reference
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
print(f"Error initializing motor on port {motorinput}: {e}")
|
||||||
|
print("Make sure a motor is actually connected to this port.")
|
||||||
|
self.testmotor = None
|
||||||
|
|
||||||
|
def testSpeed(self, speed):
|
||||||
|
self.testmotor.reset_angle(0)
|
||||||
|
|
||||||
|
motorspeeds = []
|
||||||
|
motorloads = []
|
||||||
|
TARGET_ANGLE = speed * 3
|
||||||
|
print("\n", speed, "DEGREES PER SECOND TEST")
|
||||||
|
self.testmotor.run_angle(speed, TARGET_ANGLE, Stop.HOLD, False)
|
||||||
|
stopwatchmotor = StopWatch()
|
||||||
|
while stopwatchmotor.time() < 3000:
|
||||||
|
wait(20)
|
||||||
|
motorspeeds.append(self.testmotor.speed())
|
||||||
|
motorloads.append(self.testmotor.load())
|
||||||
|
|
||||||
|
MAX_SPEED, MAX_ACCEL, MAX_TORQUE = self.testmotor.control.limits()
|
||||||
|
|
||||||
|
|
||||||
|
print("Desired motor speed: ", str(speed))
|
||||||
|
if motorspeeds:
|
||||||
|
avg = sum(motorspeeds) / len(motorspeeds)
|
||||||
|
print("Average motor speed:", avg)
|
||||||
|
print("Motor speed deviation:", str(self.stdev(motorspeeds)))
|
||||||
|
else:
|
||||||
|
print("No speed samples collected.")
|
||||||
|
avg = 0
|
||||||
|
if motorloads:
|
||||||
|
avgload = sum(motorloads) / len(motorloads)
|
||||||
|
|
||||||
|
print("Average motor load:", avgload)
|
||||||
|
print("Motor load deviation:", str(self.stdev(motorloads)))
|
||||||
|
else:
|
||||||
|
print("No load samples collected.")
|
||||||
|
avgload = 0
|
||||||
|
SCORE = self.health_score(speed, avg, self.stdev(motorspeeds), avgload)
|
||||||
|
print("Health score for this test:", str(SCORE) + "%")
|
||||||
|
return SCORE
|
||||||
|
def fullTest(self):
|
||||||
|
self.initializeMotor()
|
||||||
|
print("Load measurements are in mNm. Speed measurements are in degrees per second.")
|
||||||
|
MAX_SPEED, MAX_ACCEL, MAX_TORQUE = self.testmotor.control.limits()
|
||||||
|
print("Maximum motor speed:", MAX_SPEED)
|
||||||
|
test180 = self.testSpeed(180)
|
||||||
|
test540 = self.testSpeed(540)
|
||||||
|
test1000 = self.testSpeed(1000)
|
||||||
|
print("\n FINAL MOTOR STATISTICS")
|
||||||
|
final = (test180 + test540 + test1000) / 3
|
||||||
|
print("Final motor health score:", str(final) + "%")
|
||||||
|
if final < 65:
|
||||||
|
print("Your motor is in need of attention. Make sure to clean it regularly and charge the Prime Hub.")
|
||||||
|
elif final < 85:
|
||||||
|
print("Your motor is in OK condition. Make sure to clean it regularly and charge the Prime Hub.")
|
||||||
|
elif final < 95:
|
||||||
|
print("Your motor is in great condition!")
|
||||||
|
else:
|
||||||
|
print("Your motor is in AMAZING condition!!!")
|
||||||
|
self.testmotor.stop()
|
||||||
111
hazmat_scrimmage/bioglow_hazmat_scrimmage_main.py
Normal file
111
hazmat_scrimmage/bioglow_hazmat_scrimmage_main.py
Normal file
@@ -0,0 +1,111 @@
|
|||||||
|
#####################################
|
||||||
|
# Main.py #
|
||||||
|
# File for running all MISSIONS #
|
||||||
|
# together #
|
||||||
|
# #
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
import bioglow_robot_settings as robot
|
||||||
|
import bioglow_missions_3_4_5 as Run_1 # import here the name of files containing mission runs Run_1 for run 1,
|
||||||
|
import bioglow_mission_2 as Run_2 # Run_2 for run 2, and so on..
|
||||||
|
import bioglow_mission_1 as Run_3
|
||||||
|
import bioglow_mission_11_experimental as Run_4
|
||||||
|
import bioglow_mission_13 as Run_5
|
||||||
|
import bioglow_mission_6_7_13_14 as Run_6
|
||||||
|
import bioglow_missions_14_15 as Run_7
|
||||||
|
import bioglow_mission_8 as Run_8
|
||||||
|
|
||||||
|
# Function to classify color based on HSV
|
||||||
|
def detect_color(h, s, v, reflected):
|
||||||
|
if reflected > 4:
|
||||||
|
if 1 < h < 11 and s <50: # brown
|
||||||
|
return "Brown"
|
||||||
|
elif 11 < h < 24 and s > 80: # orange
|
||||||
|
return "Orange"
|
||||||
|
elif 30 < h < 60 and v > 50: # yellow
|
||||||
|
return "Yellow"
|
||||||
|
elif 70 < h < 90 and s > 60: # neon green
|
||||||
|
return "Lime"
|
||||||
|
elif 100 < h < 160: # green - do it vertically not horizontally for accuracy
|
||||||
|
return "Green"
|
||||||
|
elif 195 < h < 198: # light blue
|
||||||
|
return "Cyan"
|
||||||
|
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
|
||||||
|
return "Blue"
|
||||||
|
elif 260 < h < 350 and s > 80: # purple
|
||||||
|
return "Purple"
|
||||||
|
elif 350 < h and s > 50: #red
|
||||||
|
return "Red"
|
||||||
|
elif s < 15 and v > 70: # white
|
||||||
|
return "White"
|
||||||
|
else:
|
||||||
|
return "Unknown"
|
||||||
|
return "Unknown"
|
||||||
|
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
while True:
|
||||||
|
pressed = robot.hub.buttons.pressed()
|
||||||
|
h, s, v = await robot.color_sensor.hsv()
|
||||||
|
reflected = await robot.color_sensor.reflection()
|
||||||
|
color = detect_color(h, s, v, reflected)
|
||||||
|
if helper.DEBUG :
|
||||||
|
#print(color_sensor.color())
|
||||||
|
#print(h,s,v)
|
||||||
|
#print(color)
|
||||||
|
print(f"button pressed: {pressed}")
|
||||||
|
|
||||||
|
#TODO: Add the solve_missions() function of the run number for each color here
|
||||||
|
if color == "Green":
|
||||||
|
print('Running Mission 1, Detect Green')
|
||||||
|
await Run_1.solve_missions()
|
||||||
|
elif color == "Red":
|
||||||
|
print('Running Mission 2, Detect Red')
|
||||||
|
await Run_2.solve_missions()
|
||||||
|
elif color == "Yellow":
|
||||||
|
print('Running Mission 3, Detect Yellow')
|
||||||
|
await Run_3.solve_missions()
|
||||||
|
elif color == "Blue":
|
||||||
|
print('Running Mission 4, Detect Blue')
|
||||||
|
await Run_4.solve_missions()
|
||||||
|
elif color == "Purple":
|
||||||
|
print('Running Mission 5, Detect Purple')
|
||||||
|
await Run_5.solve_missions()
|
||||||
|
elif color == "Orange":
|
||||||
|
print('Running Mission 6, Detect Orange')
|
||||||
|
await Run_6.solve_missions()
|
||||||
|
elif color == "Cyan":
|
||||||
|
print('Running Mission 7, Detect Cyan')
|
||||||
|
await Run_7.solve_missions()
|
||||||
|
elif color == "Lime":
|
||||||
|
print('Running Mission 8, Detect Lime')
|
||||||
|
await Run_8.solve_missions()
|
||||||
|
elif color == "Brown":
|
||||||
|
print('Running Mission Experimental, Detect Brown')
|
||||||
|
await robot.wait(500)
|
||||||
|
elif color == "White":
|
||||||
|
print('Detect White')
|
||||||
|
await robot.wait(500)
|
||||||
|
else:
|
||||||
|
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||||
|
#pass
|
||||||
|
|
||||||
|
# Show battery % for debugging
|
||||||
|
if robot.Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor
|
||||||
|
# Get the battery voltage in millivolts (mV)
|
||||||
|
battery_voltage_mV = robot.hub.battery.voltage()
|
||||||
|
# Use the function with your voltage reading
|
||||||
|
percentage = await helper.get_battery_percentage(float(battery_voltage_mV))
|
||||||
|
if helper.DEBUG:
|
||||||
|
print(f"Battery voltage: {battery_voltage_mV} mV")
|
||||||
|
print(f"Battery level: {percentage:.3f}%")
|
||||||
|
print("FLL Robot System Ready!")
|
||||||
|
await robot.hub.display.text(f"{percentage:.0f}")
|
||||||
|
break
|
||||||
|
elif pressed == None:
|
||||||
|
continue
|
||||||
|
|
||||||
|
await robot.wait(10)
|
||||||
|
# Run the main function
|
||||||
|
robot.run_task(main())
|
||||||
178
hazmat_scrimmage/bioglow_helpers.py
Normal file
178
hazmat_scrimmage/bioglow_helpers.py
Normal file
@@ -0,0 +1,178 @@
|
|||||||
|
########################################################
|
||||||
|
# bioglow_helpers.py
|
||||||
|
# - This file contain all the helper functions and tools
|
||||||
|
# - This is where we add special or custom functionality codes
|
||||||
|
########################################################
|
||||||
|
|
||||||
|
"""
|
||||||
|
PID Straight Drive — heading-corrected forward / backward movement.
|
||||||
|
|
||||||
|
Improvements over baseline:
|
||||||
|
• Derivative on measurement (not error) — no derivative kick on setpoint change
|
||||||
|
• Fixed-interval dt — loop sleeps a constant 20 ms, so /dt is dropped from the
|
||||||
|
derivative term (dividing by 0.02 was silently multiplying KD by 50)
|
||||||
|
• prev_error seeded from first real heading — eliminates first-tick spike
|
||||||
|
• Per-motor speed clamping — prevents one side pivoting at low speeds
|
||||||
|
• Deceleration ramp — smooth stop in the last RAMP_MM millimetres
|
||||||
|
• Dual-motor exit + timeout — survives stall on either wheel
|
||||||
|
• Integral freeze near target — stops windup right where it matters most
|
||||||
|
"""
|
||||||
|
|
||||||
|
from bioglow_robot_settings import * # hub, motors, sensors, drive_base, wait, umath, ...
|
||||||
|
|
||||||
|
LOOP_MS = 20 # fixed control loop period
|
||||||
|
RAMP_MM = 50 # begin decelerating this far from target
|
||||||
|
MIN_SPEED = 80 # floor speed during ramp (avoid stall)
|
||||||
|
TIMEOUT_MS = 8_000 # safety abort
|
||||||
|
# Tuning sequence
|
||||||
|
|
||||||
|
KP = 4.8 # Wide chassis = stable platform, can afford higher P gain
|
||||||
|
# without oscillation. Pushes harder through corrections.
|
||||||
|
|
||||||
|
KI = 0.012 # Slightly lower than default — large wheels cover distance
|
||||||
|
# fast so the integrator has less time to build up per run.
|
||||||
|
# Raise to 0.012 if you see consistent sideways drift at end.
|
||||||
|
|
||||||
|
KD = 7.5 # Higher than default. Big wheels = more rotational momentum,
|
||||||
|
# needs stronger damping to prevent overshoot on corrections.
|
||||||
|
|
||||||
|
def _mm_to_deg(mm: float) -> float:
|
||||||
|
return (abs(mm) / (umath.pi * WHEEL_DIAMETER)) * 360.0
|
||||||
|
|
||||||
|
|
||||||
|
def _motor_avg_angle(m1, m2) -> float:
|
||||||
|
return (abs(m1.angle()) + abs(m2.angle())) / 2.0
|
||||||
|
|
||||||
|
|
||||||
|
async def pid_straight(distance_mm: float, speed: int = 600):
|
||||||
|
"""
|
||||||
|
Drive straight for distance_mm at speed (deg/s).
|
||||||
|
Positive = forward, negative = reverse.
|
||||||
|
"""
|
||||||
|
direction = 1 if distance_mm >= 0 else -1
|
||||||
|
degrees_need = _mm_to_deg(distance_mm)
|
||||||
|
target_hdg = hub.imu.heading()
|
||||||
|
|
||||||
|
left_motor.reset_angle(0)
|
||||||
|
right_motor.reset_angle(0)
|
||||||
|
|
||||||
|
# ── seed integral and prev_error from the real opening heading ──
|
||||||
|
first_error = target_hdg - hub.imu.heading() # 0.0 on a perfect start,
|
||||||
|
integral = 0.0 # non-zero if robot is tilted
|
||||||
|
prev_heading = hub.imu.heading() # for derivative-on-measurement
|
||||||
|
|
||||||
|
watchdog = StopWatch()
|
||||||
|
|
||||||
|
while True:
|
||||||
|
traveled = _motor_avg_angle(left_motor, right_motor)
|
||||||
|
|
||||||
|
# ── exit: distance reached or timeout ──
|
||||||
|
if traveled >= degrees_need:
|
||||||
|
break
|
||||||
|
if watchdog.time() > TIMEOUT_MS:
|
||||||
|
break
|
||||||
|
|
||||||
|
# ── heading error with wrap ──
|
||||||
|
heading = hub.imu.heading()
|
||||||
|
error = target_hdg - heading
|
||||||
|
if error > 180: error -= 360
|
||||||
|
if error < -180: error += 360
|
||||||
|
|
||||||
|
# ── derivative on measurement — no kick when target changes ──
|
||||||
|
d_heading = heading - prev_heading # raw delta (deg / loop)
|
||||||
|
if d_heading > 180: d_heading -= 360
|
||||||
|
if d_heading < -180: d_heading += 360
|
||||||
|
prev_heading = heading
|
||||||
|
|
||||||
|
# ── integral with freeze near target ──
|
||||||
|
near_target = (degrees_need - traveled) < _mm_to_deg(RAMP_MM / 2)
|
||||||
|
if not near_target:
|
||||||
|
integral = max(-50.0, min(50.0, integral + error)) # dt=const → no *dt
|
||||||
|
|
||||||
|
# ── PID output ──
|
||||||
|
# derivative term: -KD * d_heading (measurement, not error difference)
|
||||||
|
correction = KP * error + KI * integral - KD * d_heading
|
||||||
|
correction = max(-200.0, min(200.0, correction))
|
||||||
|
|
||||||
|
# ── ramp speed near end ──
|
||||||
|
remaining = degrees_need - traveled
|
||||||
|
ramp_degrees = _mm_to_deg(RAMP_MM)
|
||||||
|
if remaining < ramp_degrees and ramp_degrees > 0:
|
||||||
|
ramp_factor = max(float(MIN_SPEED) / speed,
|
||||||
|
remaining / ramp_degrees)
|
||||||
|
run_speed = int(speed * ramp_factor)
|
||||||
|
else:
|
||||||
|
run_speed = speed
|
||||||
|
|
||||||
|
# ── per-motor clamping — prevents pivot at low speeds ──
|
||||||
|
l_cmd = max(-speed, min(speed, direction * run_speed + int(correction)))
|
||||||
|
r_cmd = max(-speed, min(speed, direction * run_speed - int(correction)))
|
||||||
|
|
||||||
|
left_motor.run(l_cmd)
|
||||||
|
right_motor.run(r_cmd)
|
||||||
|
await wait(LOOP_MS)
|
||||||
|
|
||||||
|
left_motor.brake()
|
||||||
|
right_motor.brake()
|
||||||
|
|
||||||
|
|
||||||
|
"""
|
||||||
|
Debugging helps
|
||||||
|
"""
|
||||||
|
DEBUG = 1 # Enable when you want to show logs
|
||||||
|
# Example conversion function (adjust min/max values as needed for your hub)
|
||||||
|
async def get_battery_percentage(voltage_mV:float):
|
||||||
|
max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us
|
||||||
|
min_voltage = 5000.0 # min battery level
|
||||||
|
percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100
|
||||||
|
return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100
|
||||||
|
|
||||||
|
async def wait_button_release():
|
||||||
|
"""Wait for all buttons to be released"""
|
||||||
|
while hub.buttons.pressed():
|
||||||
|
await wait(500)
|
||||||
|
await wait(1000) # Debounce delay
|
||||||
|
|
||||||
|
WALL_DISTANCE = 300 # mm
|
||||||
|
async def drive_forward():
|
||||||
|
"""Drive forward continuously using DriveBase."""
|
||||||
|
drive_base.drive(1000,0)
|
||||||
|
|
||||||
|
async def drive_backward():
|
||||||
|
"""Drive backward continuously using DriveBase."""
|
||||||
|
drive_base.drive(-400, 0) # FIXED: was +400, which drove forward
|
||||||
|
|
||||||
|
|
||||||
|
async def monitor_distance(stop_mm: int = WALL_DISTANCE):
|
||||||
|
"""Monitor ultrasonic sensor and stop when wall is detected."""
|
||||||
|
while True:
|
||||||
|
distance = await lazer_ranger.distance()
|
||||||
|
print('Distancing...', distance)
|
||||||
|
|
||||||
|
if distance is None:
|
||||||
|
await wait(50)
|
||||||
|
continue
|
||||||
|
if distance < stop_mm:
|
||||||
|
drive_base.stop()
|
||||||
|
print(f"Wall detected at {distance}mm!")
|
||||||
|
break
|
||||||
|
|
||||||
|
# Small delay to prevent overwhelming the sensor
|
||||||
|
await wait(50)
|
||||||
|
|
||||||
|
# Use this to set default
|
||||||
|
def set_default_speed():
|
||||||
|
drive_base.settings(800, 600, 300, 200)
|
||||||
|
|
||||||
|
# Use this to change drive base movement
|
||||||
|
def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
|
||||||
|
drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
|
||||||
|
|
||||||
|
async def multilift(rotation_angle):
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(400,rotation_angle),
|
||||||
|
left_arm.run_angle(400,-1*rotation_angle)
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
31
hazmat_scrimmage/bioglow_mission_1.py
Normal file
31
hazmat_scrimmage/bioglow_mission_1.py
Normal file
@@ -0,0 +1,31 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 1
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Drone Survey - Johannes
|
||||||
|
|
||||||
|
await multitask(
|
||||||
|
left_arm.run_angle(400,-300),
|
||||||
|
right_arm.run_angle(400,100),
|
||||||
|
drive_base.straight(900)
|
||||||
|
)
|
||||||
|
await drive_base.turn(-25)
|
||||||
|
await drive_base.straight(110)
|
||||||
|
await drive_base.turn(-35)
|
||||||
|
await drive_base.turn(35)
|
||||||
|
await drive_base.straight(-30)
|
||||||
|
await drive_base.turn(25)
|
||||||
|
await drive_base.straight(-200)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(500)
|
||||||
|
await drive_base.turn(75)
|
||||||
|
await drive_base.straight(700)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
36
hazmat_scrimmage/bioglow_mission_11_experimental.py
Normal file
36
hazmat_scrimmage/bioglow_mission_11_experimental.py
Normal file
@@ -0,0 +1,36 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 11
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Windows to the past
|
||||||
|
|
||||||
|
drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns
|
||||||
|
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(800,-90),
|
||||||
|
drive_base.straight(100)
|
||||||
|
)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(420)
|
||||||
|
helper.set_speed(100,100,100,100)
|
||||||
|
await multitask(
|
||||||
|
left_arm.run_angle(500,45),
|
||||||
|
right_arm.run_angle(800,140)
|
||||||
|
)
|
||||||
|
helper.set_default_speed()
|
||||||
|
|
||||||
|
#return home
|
||||||
|
await drive_base.straight(-500)
|
||||||
|
await multitask(
|
||||||
|
left_arm.run_angle(100,-45),
|
||||||
|
right_arm.run_angle(800,-90)
|
||||||
|
)
|
||||||
|
#helper.reset_arms()
|
||||||
|
helper.set_default_speed()
|
||||||
|
drive_base.stop()
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
29
hazmat_scrimmage/bioglow_mission_12.py
Normal file
29
hazmat_scrimmage/bioglow_mission_12.py
Normal file
@@ -0,0 +1,29 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 12
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Forest Elder - Parthiv
|
||||||
|
|
||||||
|
await drive_base.straight(475)
|
||||||
|
await right_arm.run_angle(300,25)
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await left_arm.run_angle(200,245)
|
||||||
|
await drive_base.straight(60)
|
||||||
|
await drive_base.turn(25)
|
||||||
|
await left_arm.run_angle(100,-56.5)
|
||||||
|
await drive_base.straight(-50)
|
||||||
|
await drive_base.straight(50)
|
||||||
|
await drive_base.turn(-25)
|
||||||
|
await left_arm.run_angle(200,-200)
|
||||||
|
await drive_base.straight(-240)
|
||||||
|
await drive_base.turn(15)
|
||||||
|
await right_arm.run_angle(200,-200)
|
||||||
|
await drive_base.straight(-200)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
29
hazmat_scrimmage/bioglow_mission_13.py
Normal file
29
hazmat_scrimmage/bioglow_mission_13.py
Normal file
@@ -0,0 +1,29 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 13
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Interchangeable Mission - Parthiv
|
||||||
|
|
||||||
|
await left_arm.run_angle(200,-80)
|
||||||
|
await drive_base.arc(-350,angle=90)
|
||||||
|
await drive_base.turn(50)
|
||||||
|
await drive_base.straight(238)
|
||||||
|
await drive_base.turn(-33)
|
||||||
|
await drive_base.straight(85)
|
||||||
|
await drive_base.straight(-33)
|
||||||
|
await multitask(
|
||||||
|
left_arm.run_angle(200,80),
|
||||||
|
right_arm.run_angle(200,-100)
|
||||||
|
)
|
||||||
|
await drive_base.straight(-60)
|
||||||
|
await right_arm.run_angle(200,100)
|
||||||
|
await drive_base.turn(30)
|
||||||
|
await drive_base.straight(-775)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
48
hazmat_scrimmage/bioglow_mission_2.py
Normal file
48
hazmat_scrimmage/bioglow_mission_2.py
Normal file
@@ -0,0 +1,48 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 2
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Exploding Seeds - Johannes
|
||||||
|
|
||||||
|
drive_base.stop()
|
||||||
|
drive_base.reset(0,0)
|
||||||
|
helper.set_speed(800,400,200,100)
|
||||||
|
await drive_base.straight(20, then=Stop.NONE)
|
||||||
|
await drive_base.arc(150,None,115, then=Stop.NONE)
|
||||||
|
await drive_base.straight(270, then=Stop.HOLD)
|
||||||
|
await left_arm.run_angle(300,70)
|
||||||
|
await drive_base.straight(-30)
|
||||||
|
await left_arm.run_angle(100,20)
|
||||||
|
await drive_base.straight(-300, then=Stop.COAST),
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(500,-110),
|
||||||
|
left_arm.run_angle(500,-100),
|
||||||
|
drive_base.turn(-40, then=Stop.HOLD)
|
||||||
|
)
|
||||||
|
await drive_base.arc(610,70,None, then=Stop.HOLD)
|
||||||
|
await drive_base.turn(-78, then=Stop.HOLD)
|
||||||
|
helper.set_speed(100,200,300,200)
|
||||||
|
await drive_base.straight(120, then=Stop.COAST)
|
||||||
|
await right_arm.run_angle(1000,70)
|
||||||
|
await drive_base.straight(-130, then=Stop.COAST)
|
||||||
|
helper.set_speed(800,400,200,100)
|
||||||
|
await multitask(
|
||||||
|
drive_base.turn(95, then=Stop.HOLD),
|
||||||
|
right_arm.run_angle(500,-40)
|
||||||
|
)
|
||||||
|
await multitask(
|
||||||
|
drive_base.straight(160, then=Stop.COAST),
|
||||||
|
right_arm.run_angle(500,110)
|
||||||
|
)
|
||||||
|
await drive_base.straight(-400, then=Stop.COAST)
|
||||||
|
await drive_base.turn(100, then=Stop.NONE)
|
||||||
|
await drive_base.straight(700, then=Stop.COAST)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
37
hazmat_scrimmage/bioglow_mission_3_4_5_experimental.py
Normal file
37
hazmat_scrimmage/bioglow_mission_3_4_5_experimental.py
Normal file
@@ -0,0 +1,37 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 3,4,5
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Flip the rock,etc
|
||||||
|
|
||||||
|
drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns
|
||||||
|
|
||||||
|
print (left_arm.angle(),right_arm.angle())
|
||||||
|
helper.set_speed(600,200,300,300)
|
||||||
|
await drive_base.straight(400,Stop.COAST)
|
||||||
|
|
||||||
|
helper.set_speed(800,300,300,300)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
|
||||||
|
helper.set_speed(100,100,100,100)
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(100,-60),
|
||||||
|
left_arm.run_angle(100,60),
|
||||||
|
drive_base.straight(-60)
|
||||||
|
)
|
||||||
|
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(50,50),
|
||||||
|
left_arm.run_angle(50,-50),
|
||||||
|
drive_base.straight(-40)
|
||||||
|
)
|
||||||
|
helper.set_default_speed()
|
||||||
|
await drive_base.straight(-300)
|
||||||
|
await drive_base.straight(-100)
|
||||||
|
|
||||||
|
drive_base.stop()
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
26
hazmat_scrimmage/bioglow_mission_8.py
Normal file
26
hazmat_scrimmage/bioglow_mission_8.py
Normal file
@@ -0,0 +1,26 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 8
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Research Platform - Johannes
|
||||||
|
|
||||||
|
drive_base.reset(0,0)
|
||||||
|
await drive_base.arc(1280,None,340)
|
||||||
|
await drive_base.arc(-1400,None,355)
|
||||||
|
helper.set_speed(100,400,500,500)
|
||||||
|
await drive_base.straight(145)
|
||||||
|
await helper.multilift_up(900)
|
||||||
|
await helper.multilift_down(300)
|
||||||
|
await drive_base.straight(-150)
|
||||||
|
await drive_base.turn(20)
|
||||||
|
helper.set_speed(1000,500,300,200)
|
||||||
|
await drive_base.straight(63)
|
||||||
|
await drive_base.arc(-1000,None,-1100)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
51
hazmat_scrimmage/bioglow_missions_11_6_7.py
Normal file
51
hazmat_scrimmage/bioglow_missions_11_6_7.py
Normal file
@@ -0,0 +1,51 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Missions 11,6,7
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Window to the Past, Lead Cutter, Humungous Fungus - Johannes
|
||||||
|
|
||||||
|
await drive_base.straight(110)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(280)
|
||||||
|
await right_arm.run_angle(300,120)
|
||||||
|
await left_arm.run_angle(400,200)
|
||||||
|
await drive_base.straight(-70)
|
||||||
|
await left_arm.run_angle(600,-110)
|
||||||
|
await drive_base.straight(370)
|
||||||
|
await drive_base.turn(50)
|
||||||
|
await multitask(
|
||||||
|
drive_base.straight(250),
|
||||||
|
left_arm.run_angle(600,-60)
|
||||||
|
)
|
||||||
|
await drive_base.turn(40)
|
||||||
|
helper.set_speed(100,200,300,200)
|
||||||
|
await drive_base.straight(170)
|
||||||
|
await drive_base.straight(-135)
|
||||||
|
await drive_base.turn(80)
|
||||||
|
await drive_base.straight(85)
|
||||||
|
await multitask(
|
||||||
|
drive_base.turn(-36),
|
||||||
|
right_arm.run_angle(200,-140)
|
||||||
|
)
|
||||||
|
helper.set_speed(100,200,300,200)
|
||||||
|
await drive_base.straight(230)
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(300,160),
|
||||||
|
left_arm.run_angle(300,150)
|
||||||
|
)
|
||||||
|
await wait(200)
|
||||||
|
await drive_base.straight(30)
|
||||||
|
await drive_base.turn(-90)
|
||||||
|
await drive_base.straight(-170)
|
||||||
|
helper.set_speed(600,500,300,200)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
await drive_base.arc(800,None,-900)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
51
hazmat_scrimmage/bioglow_missions_15_14_ant
Normal file
51
hazmat_scrimmage/bioglow_missions_15_14_ant
Normal file
@@ -0,0 +1,51 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 14,15
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Seed of Renewal and Keystone Species - Parthiv
|
||||||
|
# BLUE COLOR CODE
|
||||||
|
drive_base.stop()
|
||||||
|
drive_base.reset(0,0)
|
||||||
|
|
||||||
|
helper.set_speed(400,200,200,100)
|
||||||
|
await left_arm.run_angle(1000,-25)
|
||||||
|
await drive_base.arc(-315,angle=90, then=Stop.HOLD)
|
||||||
|
await drive_base.straight(150,then=Stop.HOLD)
|
||||||
|
await drive_base.turn(90,then=Stop.HOLD)
|
||||||
|
await drive_base.straight(235,then=Stop.NONE)
|
||||||
|
helper.set_speed(100,200,300,200)
|
||||||
|
await drive_base.turn(-85,then=Stop.HOLD)
|
||||||
|
helper.set_default_speed
|
||||||
|
await drive_base.straight(90,then=Stop.COAST)
|
||||||
|
await drive_base.straight(-95)
|
||||||
|
await drive_base.turn(-85)
|
||||||
|
await drive_base.straight(-250)
|
||||||
|
await drive_base.turn(60)
|
||||||
|
helper.set_speed(50,200,250,200)
|
||||||
|
await drive_base.straight(-175)
|
||||||
|
helper.set_default_speed
|
||||||
|
await drive_base.straight(100)
|
||||||
|
await drive_base.turn(-125)
|
||||||
|
await left_arm.run_angle(200,21)
|
||||||
|
await drive_base.turn(-21)
|
||||||
|
await drive_base.straight(25)
|
||||||
|
await left_arm.run_angle(1000,-25)
|
||||||
|
helper.set_speed(500,200,200,100)
|
||||||
|
await drive_base.straight(250)
|
||||||
|
await drive_base.turn(-42)
|
||||||
|
await drive_base.straight(150)
|
||||||
|
await right_arm.run_angle(300,-40)
|
||||||
|
await drive_base.straight(-150,then=Stop.NONE)
|
||||||
|
await drive_base.turn(93,then=Stop.HOLD)
|
||||||
|
await drive_base.straight(550,then=Stop.NONE)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
29
hazmat_scrimmage/bioglow_missions_3_4_5.py
Normal file
29
hazmat_scrimmage/bioglow_missions_3_4_5.py
Normal file
@@ -0,0 +1,29 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Missions 3,4,5
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Flip The Rock, Lucky Leaves, Reaching Roots - Rishabh
|
||||||
|
|
||||||
|
await drive_base.straight(455)
|
||||||
|
await drive_base.straight(-180)
|
||||||
|
await drive_base.turn(45)
|
||||||
|
await drive_base.straight(508)
|
||||||
|
await drive_base.turn(-80)
|
||||||
|
await multitask(
|
||||||
|
left_arm.run_angle(200,100),
|
||||||
|
right_arm.run_angle(200,-120)
|
||||||
|
)
|
||||||
|
|
||||||
|
await drive_base.straight(250)
|
||||||
|
await left_arm.run_angle(200,-300)
|
||||||
|
await drive_base.straight(-38)
|
||||||
|
await drive_base.turn(30)
|
||||||
|
await right_arm.run_angle(2000,120)#Was 5000
|
||||||
|
await drive_base.straight(40)
|
||||||
|
await drive_base.arc(-700,angle=-150)
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
73
hazmat_scrimmage/bioglow_missions_6_7_13_14_experimental.py
Normal file
73
hazmat_scrimmage/bioglow_missions_6_7_13_14_experimental.py
Normal file
@@ -0,0 +1,73 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 6,7,13,14
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Leaf cutter frenzy, humungous fungus, etc
|
||||||
|
|
||||||
|
drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns
|
||||||
|
|
||||||
|
# Forward while raising arms
|
||||||
|
await multitask(
|
||||||
|
drive_base.straight(500),
|
||||||
|
right_arm.run_angle(800,20),
|
||||||
|
left_arm.run_angle(800,-10)
|
||||||
|
|
||||||
|
)
|
||||||
|
helper.set_speed(600,200,200,100)
|
||||||
|
# Turn left and lower right arm
|
||||||
|
await multitask(
|
||||||
|
drive_base.turn(-45),
|
||||||
|
right_arm.run_angle(800,-70)
|
||||||
|
)
|
||||||
|
#helper.set_default_speed()
|
||||||
|
# approach leaf cutter mission
|
||||||
|
await multitask(
|
||||||
|
drive_base.straight(350),
|
||||||
|
right_arm.run_angle(800,65)
|
||||||
|
)
|
||||||
|
await drive_base.turn(45)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
|
||||||
|
# solving mycelium
|
||||||
|
# await multitask(
|
||||||
|
# right_arm.run_angle(800,70),
|
||||||
|
# drive_base.straight(10)
|
||||||
|
# )
|
||||||
|
await right_arm.run_angle(800,70)
|
||||||
|
|
||||||
|
helper.set_default_speed()
|
||||||
|
print('Moving back')
|
||||||
|
await drive_base.straight(-260)
|
||||||
|
await drive_base.straight(40)
|
||||||
|
await multitask(
|
||||||
|
drive_base.turn(55),
|
||||||
|
right_arm.run_angle(800,-200)
|
||||||
|
)
|
||||||
|
|
||||||
|
#approach Elder tree
|
||||||
|
await drive_base.straight(220)
|
||||||
|
#await drive_base.turn(-40)
|
||||||
|
#await drive_base.straight(140)
|
||||||
|
|
||||||
|
#attemp to raise cane
|
||||||
|
await multitask(
|
||||||
|
#drive_base.turn(5),
|
||||||
|
right_arm.run_angle(500,200),
|
||||||
|
drive_base.straight(30),
|
||||||
|
)
|
||||||
|
await drive_base.straight(60)
|
||||||
|
await left_arm.run_angle(1000,200)
|
||||||
|
|
||||||
|
await drive_base.straight(-50)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(-600)
|
||||||
|
|
||||||
|
#helper.reset_arms()
|
||||||
|
|
||||||
|
drive_base.stop()
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
58
hazmat_scrimmage/bioglow_missions_6_7_experimental.py
Normal file
58
hazmat_scrimmage/bioglow_missions_6_7_experimental.py
Normal file
@@ -0,0 +1,58 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 6,7
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Windows to the past
|
||||||
|
|
||||||
|
drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns
|
||||||
|
|
||||||
|
# Forward while raising arms
|
||||||
|
await multitask(
|
||||||
|
drive_base.straight(500),
|
||||||
|
right_arm.run_angle(800,20)
|
||||||
|
)
|
||||||
|
helper.set_speed(500,200,200,100)
|
||||||
|
# Turn left and lower arms
|
||||||
|
await multitask(
|
||||||
|
drive_base.turn(-45),
|
||||||
|
right_arm.run_angle(800,-70)
|
||||||
|
)
|
||||||
|
#helper.set_default_speed()
|
||||||
|
# approach leaf cutter mission
|
||||||
|
await drive_base.straight(315)
|
||||||
|
await right_arm.run_angle(800,65)
|
||||||
|
await drive_base.turn(45)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
|
||||||
|
# solving mycelium
|
||||||
|
# await multitask(
|
||||||
|
# right_arm.run_angle(800,70),
|
||||||
|
# drive_base.straight(10)
|
||||||
|
# )
|
||||||
|
await right_arm.run_angle(800,70)
|
||||||
|
|
||||||
|
print('Moving back')
|
||||||
|
await drive_base.straight(-260)
|
||||||
|
await drive_base.straight(40)
|
||||||
|
await multitask(
|
||||||
|
drive_base.turn(90),
|
||||||
|
right_arm.run_angle(800,-200)
|
||||||
|
)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(150)
|
||||||
|
await right_arm.run_angle(800,100)
|
||||||
|
|
||||||
|
await drive_base.straight(-50)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(-600)
|
||||||
|
|
||||||
|
#helper.reset_arms()
|
||||||
|
|
||||||
|
drive_base.stop()
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
33
hazmat_scrimmage/bioglow_robot_settings.py
Normal file
33
hazmat_scrimmage/bioglow_robot_settings.py
Normal file
@@ -0,0 +1,33 @@
|
|||||||
|
####################################################
|
||||||
|
# bioglow_robot_settings.py
|
||||||
|
# This file contains all the common robot settings
|
||||||
|
# Only change this when necessary
|
||||||
|
#####################################################
|
||||||
|
|
||||||
|
#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge.
|
||||||
|
|
||||||
|
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
|
||||||
|
import umath
|
||||||
|
|
||||||
|
# Initialize hub and devices
|
||||||
|
hub = PrimeHub()
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
|
||||||
|
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[20,28],[12,20]]) # FIXED: both gear trains in ONE list
|
||||||
|
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[20,28],[12,20]]) #Added gear train list for gear ration
|
||||||
|
lazer_ranger = UltrasonicSensor(Port.E)
|
||||||
|
color_sensor = ColorSensor(Port.F)
|
||||||
|
|
||||||
|
# DriveBase configuration
|
||||||
|
WHEEL_DIAMETER = 62.4 # mm (adjust for your wheels)
|
||||||
|
AXLE_TRACK = 150 # mm (distance between wheels)
|
||||||
|
|
||||||
|
# The ONE drive base. Every other file imports it from here — never make another.
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||||
|
drive_base.settings(600, 500, 300, 200)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
1
hazmat_scrimmage/readme.md
Normal file
1
hazmat_scrimmage/readme.md
Normal file
@@ -0,0 +1 @@
|
|||||||
|
# this is is our code for hazmat scrimmage
|
||||||
134
regionals/arm.py
Normal file
134
regionals/arm.py
Normal file
@@ -0,0 +1,134 @@
|
|||||||
|
"""
|
||||||
|
arm.py — easy arm moves with % speed, named positions and homing.
|
||||||
|
|
||||||
|
Set up each arm ONCE (in robot.py):
|
||||||
|
from arm import Arm
|
||||||
|
left_arm = Arm(Motor(Port.C, ...), speed=100,
|
||||||
|
positions={"down": 0, "up": 270},
|
||||||
|
home_backward=True)
|
||||||
|
|
||||||
|
Use it in any mission (always with await):
|
||||||
|
await robot.left_arm.home() # push to the hard stop, call it 0
|
||||||
|
await robot.left_arm.move_to("up") # go to a named position
|
||||||
|
await robot.left_arm.move_to(135) # go to an exact angle
|
||||||
|
await robot.left_arm.move_by(-45) # move 45 degrees from where it is
|
||||||
|
await robot.left_arm.move_for_seconds(1) # run for 1 second
|
||||||
|
await robot.left_arm.move_until_stalled() # run until it's blocked
|
||||||
|
robot.left_arm.reset() # call where it is now 0
|
||||||
|
|
||||||
|
Speed is a PERCENT of this arm's top speed (it's geared, so that's slow).
|
||||||
|
Leave speed out to use the arm's default speed.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from pybricks.parameters import Stop
|
||||||
|
from pybricks.tools import wait, multitask
|
||||||
|
|
||||||
|
|
||||||
|
class Arm:
|
||||||
|
def __init__(self, motor, speed=50, positions=None,
|
||||||
|
home_backward=True, home_angle=0, home_speed=30, home_power=40):
|
||||||
|
"""
|
||||||
|
motor: the Motor(...) for this arm.
|
||||||
|
speed: default % speed when a move gives no speed.
|
||||||
|
positions: names for angles, like {"down": 0, "up": 270}.
|
||||||
|
Angles are measured from the home spot.
|
||||||
|
home_backward: which way the arm turns to reach its hard stop.
|
||||||
|
Check this on the robot before using home()!
|
||||||
|
home_angle: what to call the angle at the hard stop (usually 0).
|
||||||
|
home_speed: % speed for homing. Slow is gentle.
|
||||||
|
home_power: % power limit for homing, so it pushes softly.
|
||||||
|
"""
|
||||||
|
self.motor = motor
|
||||||
|
self.top_speed = motor.control.limits()[0] # deg/s after the gears = 100%
|
||||||
|
self.positions = positions or {}
|
||||||
|
self.home_backward = home_backward
|
||||||
|
self.home_angle = home_angle
|
||||||
|
self.home_speed = home_speed
|
||||||
|
self.home_power = home_power
|
||||||
|
self.set_default_speed(speed)
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ helpers
|
||||||
|
def set_default_speed(self, speed):
|
||||||
|
"""Change this arm's default % speed from now on."""
|
||||||
|
self._deg_s(speed) # just checks it's 1-100
|
||||||
|
self.default_speed = speed
|
||||||
|
|
||||||
|
def _deg_s(self, percent):
|
||||||
|
if percent is None:
|
||||||
|
percent = self.default_speed
|
||||||
|
if not 1 <= percent <= 100:
|
||||||
|
raise ValueError("arm speed is a percent from 1 to 100, but got %s" % percent)
|
||||||
|
return max(1, int(self.top_speed * percent / 100))
|
||||||
|
|
||||||
|
def _target(self, target):
|
||||||
|
if isinstance(target, str):
|
||||||
|
if target not in self.positions:
|
||||||
|
raise ValueError("no arm position named '%s'. Known: %s"
|
||||||
|
% (target, ", ".join(self.positions)))
|
||||||
|
return self.positions[target]
|
||||||
|
return target
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ moves
|
||||||
|
async def move_to(self, target, speed=None):
|
||||||
|
"""Go to a named position ("up") or an exact angle (135), then hold there."""
|
||||||
|
await self.motor.run_target(self._deg_s(speed), self._target(target))
|
||||||
|
|
||||||
|
async def move_by(self, degrees, speed=None):
|
||||||
|
"""Move this many degrees from where it is now (negative = other way)."""
|
||||||
|
await self.motor.run_angle(self._deg_s(speed), degrees)
|
||||||
|
|
||||||
|
async def move_for_seconds(self, seconds, speed=None, backward=False):
|
||||||
|
"""Run for a set time, then hold."""
|
||||||
|
deg_s = self._deg_s(speed)
|
||||||
|
await self.motor.run_time(-deg_s if backward else deg_s, int(seconds * 1000))
|
||||||
|
|
||||||
|
async def move_until_stalled(self, speed=None, backward=False, power=None, max_seconds=3):
|
||||||
|
"""Run until something blocks the arm, then let it relax.
|
||||||
|
|
||||||
|
power: % power limit (1-100), so it pushes softly. None = full power.
|
||||||
|
max_seconds: give up after this long.
|
||||||
|
Returns True if it got blocked, False if it gave up.
|
||||||
|
"""
|
||||||
|
deg_s = self._deg_s(speed)
|
||||||
|
results = await multitask(
|
||||||
|
self.motor.run_until_stalled(-deg_s if backward else deg_s,
|
||||||
|
then=Stop.COAST, duty_limit=power),
|
||||||
|
wait(int(max_seconds * 1000)),
|
||||||
|
race=True,
|
||||||
|
)
|
||||||
|
return results[0] is not None # the stall finished first
|
||||||
|
|
||||||
|
async def home(self):
|
||||||
|
"""Push gently against the hard stop and call that spot home_angle (usually 0).
|
||||||
|
|
||||||
|
Do this at the start, so "up" and "down" mean the same thing every run.
|
||||||
|
Returns True if it found the stop.
|
||||||
|
"""
|
||||||
|
found = await self.move_until_stalled(speed=self.home_speed,
|
||||||
|
backward=self.home_backward,
|
||||||
|
power=self.home_power)
|
||||||
|
self.reset(self.home_angle)
|
||||||
|
if not found:
|
||||||
|
print("Arm never reached its hard stop. Check home_backward in robot.py.")
|
||||||
|
return found
|
||||||
|
|
||||||
|
# ------------------------------------------------------------ info and stopping
|
||||||
|
def reset(self, angle=0):
|
||||||
|
"""Say "the arm is at this angle right now" (default 0).
|
||||||
|
|
||||||
|
Always give a number: Pybricks' own reset_angle() with no number
|
||||||
|
does something different (it uses the motor's built-in marker).
|
||||||
|
"""
|
||||||
|
self.motor.reset_angle(angle)
|
||||||
|
|
||||||
|
def angle(self):
|
||||||
|
"""Where the arm is now, in degrees."""
|
||||||
|
return self.motor.angle()
|
||||||
|
|
||||||
|
def stop(self):
|
||||||
|
"""Let the arm go limp."""
|
||||||
|
self.motor.stop()
|
||||||
|
|
||||||
|
def hold(self):
|
||||||
|
"""Lock the arm where it is."""
|
||||||
|
self.motor.hold()
|
||||||
30
regionals/arm_calibrate.py
Normal file
30
regionals/arm_calibrate.py
Normal file
@@ -0,0 +1,30 @@
|
|||||||
|
"""
|
||||||
|
arm_calibrate.py — find the real angles for "up", "down", and any other arm position.
|
||||||
|
|
||||||
|
How to use:
|
||||||
|
1. Open this file in Pybricks Code and press Run.
|
||||||
|
2. Each arm pushes gently to its hard stop (that spot becomes 0).
|
||||||
|
If an arm spins the wrong way, stop the program and flip home_backward in robot.py.
|
||||||
|
3. The arms go limp. Move them BY HAND to each position you want.
|
||||||
|
4. Read the angles printed below and copy them into positions={...} in robot.py.
|
||||||
|
5. Press the hub's center button (or Stop) to finish.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import robot
|
||||||
|
|
||||||
|
|
||||||
|
async def calibrate():
|
||||||
|
print("Homing arms...")
|
||||||
|
left_ok, right_ok = await robot.multitask(robot.left_arm.home(), robot.right_arm.home())
|
||||||
|
print("Left arm found its stop:", left_ok, " Right arm found its stop:", right_ok)
|
||||||
|
|
||||||
|
robot.left_arm.stop() # go limp so you can move them by hand
|
||||||
|
robot.right_arm.stop()
|
||||||
|
print("Move the arms by hand. Angles:")
|
||||||
|
|
||||||
|
while True:
|
||||||
|
print("left_arm:", robot.left_arm.angle(), " right_arm:", robot.right_arm.angle())
|
||||||
|
await robot.wait(500)
|
||||||
|
|
||||||
|
|
||||||
|
robot.run_task(calibrate())
|
||||||
165
regionals/bioglow_helpers.py
Normal file
165
regionals/bioglow_helpers.py
Normal file
@@ -0,0 +1,165 @@
|
|||||||
|
########################################################
|
||||||
|
# bioglow_helpers.py
|
||||||
|
# - This file contain all the helper functions and tools
|
||||||
|
# - This is where we add special or custom functionality codes
|
||||||
|
########################################################
|
||||||
|
|
||||||
|
"""
|
||||||
|
PID Straight Drive — heading-corrected forward / backward movement.
|
||||||
|
|
||||||
|
Improvements over baseline:
|
||||||
|
• Derivative on measurement (not error) — no derivative kick on setpoint change
|
||||||
|
• Fixed-interval dt — loop sleeps a constant 20 ms, so /dt is dropped from the
|
||||||
|
derivative term (dividing by 0.02 was silently multiplying KD by 50)
|
||||||
|
• prev_error seeded from first real heading — eliminates first-tick spike
|
||||||
|
• Per-motor speed clamping — prevents one side pivoting at low speeds
|
||||||
|
• Deceleration ramp — smooth stop in the last RAMP_MM millimetres
|
||||||
|
• Dual-motor exit + timeout — survives stall on either wheel
|
||||||
|
• Integral freeze near target — stops windup right where it matters most
|
||||||
|
"""
|
||||||
|
|
||||||
|
from robot import * # hub, motors, sensors, drive_base, wait, umath, ... all come from robot.py
|
||||||
|
import robot
|
||||||
|
|
||||||
|
LOOP_MS = 20 # fixed control loop period
|
||||||
|
RAMP_MM = 50 # begin decelerating this far from target
|
||||||
|
MIN_SPEED = 80 # floor speed during ramp (avoid stall)
|
||||||
|
TIMEOUT_MS = 8_000 # safety abort
|
||||||
|
# Tuning sequence
|
||||||
|
|
||||||
|
KP = 4.8 # Wide chassis = stable platform, can afford higher P gain
|
||||||
|
# without oscillation. Pushes harder through corrections.
|
||||||
|
|
||||||
|
KI = 0.012 # Slightly lower than default — large wheels cover distance
|
||||||
|
# fast so the integrator has less time to build up per run.
|
||||||
|
# Raise to 0.012 if you see consistent sideways drift at end.
|
||||||
|
|
||||||
|
KD = 7.5 # Higher than default. Big wheels = more rotational momentum,
|
||||||
|
# needs stronger damping to prevent overshoot on corrections.
|
||||||
|
|
||||||
|
def _mm_to_deg(mm: float) -> float:
|
||||||
|
return (abs(mm) / (umath.pi * WHEEL_DIAMETER)) * 360.0
|
||||||
|
|
||||||
|
|
||||||
|
def _motor_avg_angle(m1, m2) -> float:
|
||||||
|
return (abs(m1.angle()) + abs(m2.angle())) / 2.0
|
||||||
|
|
||||||
|
|
||||||
|
async def pid_straight(distance_mm: float, speed: int = 600):
|
||||||
|
"""
|
||||||
|
Drive straight for distance_mm at speed (deg/s).
|
||||||
|
Positive = forward, negative = reverse.
|
||||||
|
"""
|
||||||
|
direction = 1 if distance_mm >= 0 else -1
|
||||||
|
degrees_need = _mm_to_deg(distance_mm)
|
||||||
|
target_hdg = hub.imu.heading()
|
||||||
|
|
||||||
|
left_motor.reset_angle(0)
|
||||||
|
right_motor.reset_angle(0)
|
||||||
|
|
||||||
|
# ── seed integral and prev_error from the real opening heading ──
|
||||||
|
first_error = target_hdg - hub.imu.heading() # 0.0 on a perfect start,
|
||||||
|
integral = 0.0 # non-zero if robot is tilted
|
||||||
|
prev_heading = hub.imu.heading() # for derivative-on-measurement
|
||||||
|
|
||||||
|
watchdog = StopWatch()
|
||||||
|
|
||||||
|
while True:
|
||||||
|
traveled = _motor_avg_angle(left_motor, right_motor)
|
||||||
|
|
||||||
|
# ── exit: distance reached or timeout ──
|
||||||
|
if traveled >= degrees_need:
|
||||||
|
break
|
||||||
|
if watchdog.time() > TIMEOUT_MS:
|
||||||
|
break
|
||||||
|
|
||||||
|
# ── heading error with wrap ──
|
||||||
|
heading = hub.imu.heading()
|
||||||
|
error = target_hdg - heading
|
||||||
|
if error > 180: error -= 360
|
||||||
|
if error < -180: error += 360
|
||||||
|
|
||||||
|
# ── derivative on measurement — no kick when target changes ──
|
||||||
|
d_heading = heading - prev_heading # raw delta (deg / loop)
|
||||||
|
if d_heading > 180: d_heading -= 360
|
||||||
|
if d_heading < -180: d_heading += 360
|
||||||
|
prev_heading = heading
|
||||||
|
|
||||||
|
# ── integral with freeze near target ──
|
||||||
|
near_target = (degrees_need - traveled) < _mm_to_deg(RAMP_MM / 2)
|
||||||
|
if not near_target:
|
||||||
|
integral = max(-50.0, min(50.0, integral + error)) # dt=const → no *dt
|
||||||
|
|
||||||
|
# ── PID output ──
|
||||||
|
# derivative term: -KD * d_heading (measurement, not error difference)
|
||||||
|
correction = KP * error + KI * integral - KD * d_heading
|
||||||
|
correction = max(-200.0, min(200.0, correction))
|
||||||
|
|
||||||
|
# ── ramp speed near end ──
|
||||||
|
remaining = degrees_need - traveled
|
||||||
|
ramp_degrees = _mm_to_deg(RAMP_MM)
|
||||||
|
if remaining < ramp_degrees and ramp_degrees > 0:
|
||||||
|
ramp_factor = max(float(MIN_SPEED) / speed,
|
||||||
|
remaining / ramp_degrees)
|
||||||
|
run_speed = int(speed * ramp_factor)
|
||||||
|
else:
|
||||||
|
run_speed = speed
|
||||||
|
|
||||||
|
# ── per-motor clamping — prevents pivot at low speeds ──
|
||||||
|
l_cmd = max(-speed, min(speed, direction * run_speed + int(correction)))
|
||||||
|
r_cmd = max(-speed, min(speed, direction * run_speed - int(correction)))
|
||||||
|
|
||||||
|
left_motor.run(l_cmd)
|
||||||
|
right_motor.run(r_cmd)
|
||||||
|
await wait(LOOP_MS)
|
||||||
|
|
||||||
|
left_motor.brake()
|
||||||
|
right_motor.brake()
|
||||||
|
|
||||||
|
|
||||||
|
"""
|
||||||
|
Debugging helps
|
||||||
|
"""
|
||||||
|
DEBUG = 1 # Enable when you want to show logs
|
||||||
|
# Example conversion function (adjust min/max values as needed for your hub)
|
||||||
|
async def get_battery_percentage(voltage_mV:float):
|
||||||
|
max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us
|
||||||
|
min_voltage = 5000.0 # min battery level
|
||||||
|
percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100
|
||||||
|
return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100
|
||||||
|
|
||||||
|
async def wait_button_release():
|
||||||
|
"""Wait for all buttons to be released"""
|
||||||
|
while hub.buttons.pressed():
|
||||||
|
await wait(500)
|
||||||
|
await wait(1000) # Debounce delay
|
||||||
|
|
||||||
|
WALL_DISTANCE = 300 # mm
|
||||||
|
async def drive_forward():
|
||||||
|
"""Drive forward continuously using DriveBase."""
|
||||||
|
drive_base.drive(1000,0)
|
||||||
|
|
||||||
|
async def drive_backward():
|
||||||
|
"""Drive backward continuously using DriveBase."""
|
||||||
|
drive_base.drive(-400, 0) # FIXED: was +400, which drove forward
|
||||||
|
|
||||||
|
|
||||||
|
async def monitor_distance(stop_mm: int = WALL_DISTANCE):
|
||||||
|
"""Monitor ultrasonic sensor and stop when wall is detected."""
|
||||||
|
while True:
|
||||||
|
distance = await lazer_ranger.distance()
|
||||||
|
print('Distancing...', distance)
|
||||||
|
|
||||||
|
if distance is None:
|
||||||
|
await wait(50)
|
||||||
|
continue
|
||||||
|
if distance < stop_mm:
|
||||||
|
drive_base.stop()
|
||||||
|
print(f"Wall detected at {distance}mm!")
|
||||||
|
break
|
||||||
|
|
||||||
|
# Small delay to prevent overwhelming the sensor
|
||||||
|
await wait(50)
|
||||||
|
|
||||||
|
# Speeds: use speed= and acceleration= on each move, e.g. robot.move(12, speed=18, acceleration=200).
|
||||||
|
# Both arms together: robot.lift_up(degrees, speed=...) and robot.lift_down(degrees, speed=...).
|
||||||
122
regionals/bioglow_main.py
Normal file
122
regionals/bioglow_main.py
Normal file
@@ -0,0 +1,122 @@
|
|||||||
|
#####################################
|
||||||
|
# Main.py #
|
||||||
|
# File for running all MISSIONS #
|
||||||
|
# together #
|
||||||
|
# #
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
import robot
|
||||||
|
import bioglow_missions_3_4_5 as Run_1 # Reaching Roots + Lucky Leaves + Flip the Rock - 50 points
|
||||||
|
import bioglow_mission_2 as Run_2 # Exploding Seeds + Tangled Vine + Garden Skylight(Biometric Architecture ) - 70 points
|
||||||
|
import bioglow_mission_8 as Run_3 # Research Platform - 30 points
|
||||||
|
import bioglow_mission_1 as Run_4 # Send Over to Blue side
|
||||||
|
import bioglow_mission_11 as Run_5 # Windows to the past - 20 points
|
||||||
|
import bioglow_mission_keystone as Run_6 # Keystone species - 30 points
|
||||||
|
import bioglow_mission_7 as Run_7 # Humungous Fungus + Tree vine - 40 points
|
||||||
|
import bioglow_missions_15_14_ant as Run_8 # Nesting Canopy (Biometric Architecture) + Leaf Cutter Frenzy - 50 points
|
||||||
|
|
||||||
|
|
||||||
|
async def start(run):
|
||||||
|
"""Run one mission, starting from the normal speeds in robot.py.
|
||||||
|
|
||||||
|
Some runs change speeds with robot.set_default_speed(). Resetting first
|
||||||
|
means each run drives exactly like it did when tested on its own.
|
||||||
|
"""
|
||||||
|
robot.reset_settings()
|
||||||
|
await run.solve_missions()
|
||||||
|
|
||||||
|
|
||||||
|
# Function to classify color based on HSV
|
||||||
|
def detect_color(h, s, v, reflected):
|
||||||
|
if reflected > 4:
|
||||||
|
if 1 < h < 11 and s <50: # brown
|
||||||
|
return "Brown"
|
||||||
|
elif 11 < h < 24 and s > 80: # orange
|
||||||
|
return "Orange"
|
||||||
|
elif 30 < h < 60 and v > 50: # yellow
|
||||||
|
return "Yellow"
|
||||||
|
elif 70 < h < 95 and s > 55: # neon green
|
||||||
|
return "Lime"
|
||||||
|
elif 100 < h < 160: # green - do it vertically not horizontally for accuracy
|
||||||
|
return "Green"
|
||||||
|
elif 195 < h < 198: # light blue
|
||||||
|
return "Cyan"
|
||||||
|
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
|
||||||
|
return "Blue"
|
||||||
|
elif 260 < h < 350 and s > 80: # purple
|
||||||
|
return "Purple"
|
||||||
|
elif 350 < h and s > 50: #red
|
||||||
|
return "Red"
|
||||||
|
elif s < 15 and v > 70: # white
|
||||||
|
return "White"
|
||||||
|
else:
|
||||||
|
return "Unknown"
|
||||||
|
return "Unknown"
|
||||||
|
|
||||||
|
|
||||||
|
async def main():
|
||||||
|
while True:
|
||||||
|
pressed = robot.hub.buttons.pressed()
|
||||||
|
h, s, v = await robot.color_sensor.hsv()
|
||||||
|
reflected = await robot.color_sensor.reflection()
|
||||||
|
color = detect_color(h, s, v, reflected)
|
||||||
|
if helper.DEBUG :
|
||||||
|
#print(color_sensor.color())
|
||||||
|
#print(h,s,v)
|
||||||
|
#print(color)
|
||||||
|
print(f"button pressed: {pressed}")
|
||||||
|
|
||||||
|
#TODO: Add the solve_missions() function of the run number for each color here
|
||||||
|
if color == "Green":
|
||||||
|
print('Running Mission 1, Detect Green')
|
||||||
|
await start(Run_1)
|
||||||
|
elif color == "Red":
|
||||||
|
print('Running Mission 2, Detect Red')
|
||||||
|
await start(Run_2)
|
||||||
|
elif color == "Lime":
|
||||||
|
print('Running Mission 3, Detect Lime')
|
||||||
|
await start(Run_3)
|
||||||
|
elif color == "Yellow":
|
||||||
|
print('Running Mission 4, Detect Yellow')
|
||||||
|
await start(Run_4)
|
||||||
|
elif color == "Purple":
|
||||||
|
print('Running Mission 5, Detect Purple')
|
||||||
|
await start(Run_5)
|
||||||
|
elif color == "Orange":
|
||||||
|
print('Running Mission 6, Detect Orange')
|
||||||
|
await start(Run_6)
|
||||||
|
elif color == "Cyan":
|
||||||
|
print('Running Mission 7, Detect Cyan')
|
||||||
|
await start(Run_7)
|
||||||
|
elif color == "Blue":
|
||||||
|
print('Running Mission 8, Detect Blue')
|
||||||
|
await start(Run_8)
|
||||||
|
elif color == "Brown":
|
||||||
|
print('Running Mission Experimental, Detect Brown')
|
||||||
|
await robot.wait(500)
|
||||||
|
elif color == "White":
|
||||||
|
print('Detect White')
|
||||||
|
await robot.wait(500)
|
||||||
|
else:
|
||||||
|
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||||
|
#pass
|
||||||
|
|
||||||
|
# Show battery % for debugging
|
||||||
|
if robot.Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor
|
||||||
|
# Get the battery voltage in millivolts (mV)
|
||||||
|
battery_voltage_mV = robot.hub.battery.voltage()
|
||||||
|
# Use the function with your voltage reading
|
||||||
|
percentage = await helper.get_battery_percentage(float(battery_voltage_mV))
|
||||||
|
if helper.DEBUG:
|
||||||
|
print(f"Battery voltage: {battery_voltage_mV} mV")
|
||||||
|
print(f"Battery level: {percentage:.3f}%")
|
||||||
|
print("FLL Robot System Ready!")
|
||||||
|
await robot.hub.display.text(f"{percentage:.0f}")
|
||||||
|
break
|
||||||
|
elif pressed == None:
|
||||||
|
continue
|
||||||
|
|
||||||
|
await robot.wait(10)
|
||||||
|
# Run the main function
|
||||||
|
robot.run_task(main())
|
||||||
46
regionals/bioglow_mission_1.py
Normal file
46
regionals/bioglow_mission_1.py
Normal file
@@ -0,0 +1,46 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 1
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import robot # drive base, arms, sensors and moves all come from robot.py
|
||||||
|
|
||||||
|
# UNITS used in this file -----------------------------------------------------
|
||||||
|
# robot.move(cm) distance in cm (+ forward, - backward)
|
||||||
|
# robot.turn(degrees) degrees (+ right / clockwise, - left)
|
||||||
|
# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left),
|
||||||
|
# then degrees= around the circle or cm= along it (+ forward, - backward)
|
||||||
|
# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn)
|
||||||
|
# acceleration= how fast it speeds up and slows down:
|
||||||
|
# mm/s² for move/arc (normal 500), °/s² for turn (normal 200)
|
||||||
|
# (² means "per second": 200 mm/s² = 200 mm/s faster every second)
|
||||||
|
# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed
|
||||||
|
# ------------------------------------------------------------------------------
|
||||||
|
|
||||||
|
async def solve_missions(): # Drone Survey - Johannes
|
||||||
|
|
||||||
|
# --- ARM HOMING (optional) -------------------------------------------
|
||||||
|
# Uncomment the next line to push both arms gently against their hard stops
|
||||||
|
# and call that spot 0, so every arm move starts from the same place each run.
|
||||||
|
# Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py).
|
||||||
|
# await robot.home_arms()
|
||||||
|
|
||||||
|
# --- LET THE GYRO SETTLE ----------------------------------------------
|
||||||
|
# Wait half a second (and until the hub is perfectly still) before moving,
|
||||||
|
# so turns are measured from a steady heading.
|
||||||
|
#await robot.settle_gyro()
|
||||||
|
|
||||||
|
await robot.move(60, then=robot.Stop.NONE) # 60 cm forward
|
||||||
|
await robot.move(-5, then=robot.Stop.NONE)
|
||||||
|
await robot.turn(-31, then=robot.Stop.HOLD) # 35° left
|
||||||
|
await robot.move(50, then=robot.Stop.HOLD) # 3 cm backward
|
||||||
|
await robot.turn(-27, then=robot.Stop.HOLD) # 35° left
|
||||||
|
await robot.move(13, speed=18, acceleration=100)
|
||||||
|
await robot.move(-4, speed=18, acceleration=200)
|
||||||
|
await robot.turn(65, then=robot.Stop.HOLD) # 75° right
|
||||||
|
await robot.move(85, then=robot.Stop.NONE) # 70 cm forward
|
||||||
|
robot.stop() # FIXED: was `await drive_base.stop` (crashed)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
robot.run_task(solve_missions())
|
||||||
63
regionals/bioglow_mission_11.py
Normal file
63
regionals/bioglow_mission_11.py
Normal file
@@ -0,0 +1,63 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 11
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import robot # drive base, arms, sensors and moves all come from robot.py
|
||||||
|
|
||||||
|
# UNITS used in this file -----------------------------------------------------
|
||||||
|
# robot.move(cm) distance in cm (+ forward, - backward)
|
||||||
|
# robot.turn(degrees) degrees (+ right / clockwise, - left)
|
||||||
|
# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left),
|
||||||
|
# then degrees= around the circle or cm= along it (+ forward, - backward)
|
||||||
|
# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn)
|
||||||
|
# acceleration= how fast it speeds up and slows down:
|
||||||
|
# mm/s² for move/arc (normal 500), °/s² for turn (normal 200)
|
||||||
|
# (² means "per second": 200 mm/s² = 200 mm/s faster every second)
|
||||||
|
# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed
|
||||||
|
# ------------------------------------------------------------------------------
|
||||||
|
|
||||||
|
async def solve_missions(): # Windows to the past
|
||||||
|
|
||||||
|
# --- ARM HOMING (optional) -------------------------------------------
|
||||||
|
# Uncomment the next line to push both arms gently against their hard stops
|
||||||
|
# and call that spot 0, so every arm move starts from the same place each run.
|
||||||
|
# Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py).
|
||||||
|
# await robot.home_arms()
|
||||||
|
|
||||||
|
# --- LET THE GYRO SETTLE ----------------------------------------------
|
||||||
|
# Wait half a second (and until the hub is perfectly still) before moving,
|
||||||
|
# so turns are measured from a steady heading.
|
||||||
|
await robot.settle_gyro(ms=200)
|
||||||
|
|
||||||
|
robot.reset() # always reset drivebase for heading reset especially with turns
|
||||||
|
|
||||||
|
await robot.multitask(
|
||||||
|
robot.right_arm.move_by(-90, speed=100), # right arm -90°, 100% arm speed
|
||||||
|
robot.move(10) # 10 cm forward
|
||||||
|
)
|
||||||
|
await robot.turn(-45) # 45° left
|
||||||
|
await robot.move(35) # 42 cm forward
|
||||||
|
await robot.multitask(
|
||||||
|
robot.left_arm.move_by(45, speed=100), # left arm 45°, 100% arm speed
|
||||||
|
robot.right_arm.move_by(140, speed=100) # right arm 140°, 100% arm speed
|
||||||
|
)
|
||||||
|
|
||||||
|
#return home
|
||||||
|
await robot.move(-30) # 50 cm backward
|
||||||
|
await robot.turn(46)
|
||||||
|
await robot.settle_gyro()
|
||||||
|
await robot.move(29)
|
||||||
|
await robot.turn(34)
|
||||||
|
await robot.turn(-34)
|
||||||
|
await robot.move(-60)
|
||||||
|
await robot.multitask(
|
||||||
|
robot.left_arm.move_by(-45, speed=23), # left arm -45°, 23% arm speed
|
||||||
|
robot.right_arm.move_by(-90, speed=100) # right arm -90°, 100% arm speed
|
||||||
|
)
|
||||||
|
#helper.reset_arms()
|
||||||
|
robot.stop()
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
robot.run_task(solve_missions())
|
||||||
|
robot.run_task(solve_missions())
|
||||||
|
|
||||||
80
regionals/bioglow_mission_2.py
Normal file
80
regionals/bioglow_mission_2.py
Normal file
@@ -0,0 +1,80 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 2
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import robot # drive base, arms, sensors and moves all come from robot.py
|
||||||
|
|
||||||
|
# UNITS used in this file -----------------------------------------------------
|
||||||
|
# robot.move(cm) distance in cm (+ forward, - backward)
|
||||||
|
# robot.turn(degrees) degrees (+ right / clockwise, - left)
|
||||||
|
# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left),
|
||||||
|
# then degrees= around the circle or cm= along it (+ forward, - backward)
|
||||||
|
# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn)
|
||||||
|
# acceleration= how fast it speeds up and slows down:
|
||||||
|
# mm/s² for move/arc (normal 500), °/s² for turn (normal 200)
|
||||||
|
# (² means "per second": 200 mm/s² = 200 mm/s faster every second)
|
||||||
|
# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed
|
||||||
|
# ------------------------------------------------------------------------------
|
||||||
|
|
||||||
|
async def solve_missions(): # Exploding Seeds - Johannes
|
||||||
|
|
||||||
|
# --- LET THE GYRO SETTLE ----------------------------------------------
|
||||||
|
# Wait half a second (and until the hub is perfectly still) before moving,
|
||||||
|
# so turns are measured from a steady heading.
|
||||||
|
#await robot.settle_gyro()
|
||||||
|
|
||||||
|
robot.stop()
|
||||||
|
robot.reset()
|
||||||
|
|
||||||
|
# Solving Exploding seeds
|
||||||
|
await robot.move(1, speed=100, acceleration=400, then=robot.Stop.NONE) # 2 cm forward, 100% speed, 400 mm/s² accel
|
||||||
|
await robot.arc(15, cm=11.5, speed=100, turn_speed=48, acceleration=400, turn_acceleration=100, then=robot.Stop.NONE) # arc: circle on the robot's RIGHT (15 cm radius), FORWARD 11.5 cm along it (nose turns right) | 100% speed, 48% turn speed, 400 mm/s² accel, 100 °/s² turn accel
|
||||||
|
await robot.move(27, speed=100, acceleration=400, then=robot.Stop.HOLD)
|
||||||
|
await robot.left_arm.move_by(70, speed=70) # left arm 70°, 70% arm speed
|
||||||
|
await robot.move(-3, speed=100, acceleration=400) # 3 cm backward, 100% speed, 400 mm/s² accel
|
||||||
|
await robot.left_arm.move_by(15, speed=23) # left arm 20°, 23% arm speed
|
||||||
|
|
||||||
|
await robot.move(-30, speed=100, acceleration=400, then=robot.Stop.COAST) # 30 cm backward, 100% speed, 400 mm/s² accel
|
||||||
|
await robot.multitask(
|
||||||
|
#robot.right_arm.move_by(-110, speed=100), # Don't drop the right arm
|
||||||
|
robot.left_arm.move_by(-100, speed=100),
|
||||||
|
robot.turn(-40, speed=48, acceleration=100, then=robot.Stop.HOLD) # 40° left, 48% turn speed, 100 °/s² turn accel
|
||||||
|
)
|
||||||
|
|
||||||
|
# Towards the vine
|
||||||
|
await robot.arc(68, degrees=65, speed=80, turn_speed=48, acceleration=400, turn_acceleration=100, then=robot.Stop.HOLD) # arc: circle on the robot's RIGHT (61 cm radius), FORWARD 70° (nose turns right) | 100% speed, 48% turn speed, 400 mm/s² accel, 100 °/s² turn accel
|
||||||
|
|
||||||
|
# This where we drop the right arm
|
||||||
|
await robot.multitask(
|
||||||
|
robot.right_arm.move_by(-110, speed=100),
|
||||||
|
robot.turn(-70, speed=70, acceleration=100, then=robot.Stop.HOLD)
|
||||||
|
)
|
||||||
|
|
||||||
|
# Untangled the vine
|
||||||
|
await robot.move(11, speed=70, acceleration=200, then=robot.Stop.COAST)
|
||||||
|
await robot.right_arm.move_by(70, speed=100) # right arm 70°, 100% arm speed
|
||||||
|
await robot.move(-10, speed=60, acceleration=200, then=robot.Stop.COAST) # 13 cm backward, 18% speed, 200 mm/s² accel
|
||||||
|
|
||||||
|
# Solving Sunlight roof
|
||||||
|
await robot.multitask(
|
||||||
|
robot.turn(95, speed=48, acceleration=100, then=robot.Stop.HOLD), # 95° right, 48% turn speed, 100 °/s² turn accel
|
||||||
|
robot.right_arm.move_by(100, speed=100) # right arm -40°, 100% arm speed
|
||||||
|
)
|
||||||
|
await robot.move(19, speed=100, acceleration=400, then=robot.Stop.COAST), # 16 cm forward, 100% speed, 400 mm/s² accel
|
||||||
|
|
||||||
|
# Return to barracks
|
||||||
|
await robot.move(-20, speed=100, acceleration=400, then=robot.Stop.NONE)
|
||||||
|
await robot.arc(40, degrees=-90,then = robot.Stop.NONE)
|
||||||
|
await robot.move(-40, speed=100, acceleration=400, then=robot.Stop.BRAKE)
|
||||||
|
robot.stop()
|
||||||
|
|
||||||
|
# For testing a part of a solution
|
||||||
|
def run_test_something():
|
||||||
|
await robot.arc(30, degrees=-90,then = robot.Stop.NONE)
|
||||||
|
await robot.move(-40, speed=100, acceleration=400, then=robot.Stop.BRAKE)
|
||||||
|
return
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
robot.run_task(solve_missions())
|
||||||
|
#robot.run_task(run_test_something()) -- Uncomment this for testing and comment the line above
|
||||||
69
regionals/bioglow_mission_3_4_5.py
Normal file
69
regionals/bioglow_mission_3_4_5.py
Normal file
@@ -0,0 +1,69 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Missions 3,4,5
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import robot # drive base, arms, sensors and moves all come from robot.py
|
||||||
|
|
||||||
|
# UNITS used in this file -----------------------------------------------------
|
||||||
|
# robot.move(cm) distance in cm (+ forward, - backward)
|
||||||
|
# robot.turn(degrees) degrees (+ right / clockwise, - left)
|
||||||
|
# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left),
|
||||||
|
# then degrees= around the circle or cm= along it (+ forward, - backward)
|
||||||
|
# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn)
|
||||||
|
# acceleration= how fast it speeds up and slows down:
|
||||||
|
# mm/s² for move/arc (normal 500), °/s² for turn (normal 200)
|
||||||
|
# (² means "per second": 200 mm/s² = 200 mm/s faster every second)
|
||||||
|
# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed
|
||||||
|
# ------------------------------------------------------------------------------
|
||||||
|
|
||||||
|
async def solve_missions(): # Flip The Rock, Lucky Leaves, Reaching Roots - Rishabh
|
||||||
|
|
||||||
|
|
||||||
|
#await robot.home_arms() # arms: where they are now = 0
|
||||||
|
robot.drive_base.reset() # reset drive base
|
||||||
|
|
||||||
|
# --- LET THE GYRO SETTLE ----------------------------------------------
|
||||||
|
# Wait half a second (and until the hub is perfectly still) before moving,
|
||||||
|
# so turns are measured from a steady heading.
|
||||||
|
#await robot.settle_gyro(ms=10)
|
||||||
|
|
||||||
|
#await robot.right_arm.move_by(6, speed=50) # Lower the right arm to align to the root
|
||||||
|
#Wawait robot.move(90,speed=70,then=robot.Stop.BRAKE) # Move straight towards the root 90 cm
|
||||||
|
#await robot.right_arm.move_by(270, speed=100) # Solve the root by spinning the arm
|
||||||
|
#await robot.wait(150)
|
||||||
|
#await robot.right_arm.move_by(180, speed=100)
|
||||||
|
await robot.move(47) #move back
|
||||||
|
await robot.multitask(
|
||||||
|
robot.move(-30),
|
||||||
|
robot.right_arm.move_by(100, speed=100)
|
||||||
|
)
|
||||||
|
#await robot.right_arm.move_by(100, speed=100) # Solve the root by spinning the arm
|
||||||
|
#await robot.move(50) #move back
|
||||||
|
#await robot.move(-30) #move back
|
||||||
|
#await robot.right_arm.move_by(180, speed=100)
|
||||||
|
await robot.turn(35) #move back
|
||||||
|
|
||||||
|
await robot.move(60)
|
||||||
|
|
||||||
|
# Aligning to Lucky leaves
|
||||||
|
#await robot.multitask(
|
||||||
|
await robot.turn(-73) , # 30° left
|
||||||
|
await robot.left_arm.move_by(90, speed=80)
|
||||||
|
#)
|
||||||
|
# Collect leaves
|
||||||
|
await robot.move(25,speed=40)
|
||||||
|
await robot.left_arm.move_by(-90, speed=47) # left arm -90°, 47% arm speed
|
||||||
|
|
||||||
|
#await robot.move(-10,then=robot.Stop.COAST) # 15cm backward
|
||||||
|
await robot.arc(-30, degrees=-87) # arc: circle on the robot's LEFT (30 cm radius), BACKWARD 90° (back end swings left, nose turns right)
|
||||||
|
|
||||||
|
# Turns to Flip the rock and solve it
|
||||||
|
#await robot.turn(-75,then=robot.Stop.HOLD) # 75° left
|
||||||
|
#await robot.move(20)
|
||||||
|
|
||||||
|
# Return to home area and stop the robot
|
||||||
|
await robot.move(-50)
|
||||||
|
robot.stop()
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
robot.run_task(solve_missions())
|
||||||
92
regionals/bioglow_mission_7.py
Normal file
92
regionals/bioglow_mission_7.py
Normal file
@@ -0,0 +1,92 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Missions 7,12
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import robot # drive base, arms, sensors and moves all come from robot.py
|
||||||
|
|
||||||
|
# UNITS used in this file -----------------------------------------------------
|
||||||
|
# robot.move(cm) distance in cm (+ forward, - backward)
|
||||||
|
# robot.turn(degrees) degrees (+ right / clockwise, - left)
|
||||||
|
# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left),
|
||||||
|
# then degrees= around the circle or cm= along it (+ forward, - backward)
|
||||||
|
# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn)
|
||||||
|
# acceleration= how fast it speeds up and slows down:
|
||||||
|
# mm/s² for move/arc (normal 500), °/s² for turn (normal 200)
|
||||||
|
# (² means "per second": 200 mm/s² = 200 mm/s faster every second)
|
||||||
|
# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed
|
||||||
|
# ------------------------------------------------------------------------------
|
||||||
|
|
||||||
|
async def solve_missions(): # Humongous Fungus + Tree Cane
|
||||||
|
|
||||||
|
robot.stop()
|
||||||
|
robot.reset()
|
||||||
|
robot.drive_base.reset()
|
||||||
|
|
||||||
|
# --- LET THE GYRO SETTLE ----------------------------------------------
|
||||||
|
# Wait half a second (and until the hub is perfectly still) before moving,
|
||||||
|
# so turns are measured from a steady heading.
|
||||||
|
#await robot.settle_gyro(ms=10)
|
||||||
|
|
||||||
|
# Move towards Mycelium,lower left arm
|
||||||
|
await robot.multitask(
|
||||||
|
robot.move(70,speed=70,then=robot.Stop.BRAKE),
|
||||||
|
robot.left_arm.move_by(-45, speed=50)
|
||||||
|
)
|
||||||
|
|
||||||
|
# Lower right arm
|
||||||
|
await robot.right_arm.move_by(-60, speed=80)
|
||||||
|
await robot.wait(100)
|
||||||
|
|
||||||
|
# Extend mycelium
|
||||||
|
await robot.multitask(
|
||||||
|
robot.left_arm.move_by(90, speed=80),
|
||||||
|
robot.move(5,speed=30,then=robot.Stop.BRAKE)
|
||||||
|
)
|
||||||
|
await robot.wait(100)
|
||||||
|
|
||||||
|
# Move backward and raise tree support by turning right slightly
|
||||||
|
await robot.move(-5,speed=50,then=robot.Stop.HOLD)
|
||||||
|
await robot.turn(26,then=robot.Stop.HOLD)
|
||||||
|
|
||||||
|
# Turn and raise right arm or another way is to move forward
|
||||||
|
# and raise the arm before turning
|
||||||
|
await robot.turn(-25,then=robot.Stop.BRAKE)
|
||||||
|
await robot.wait(100)
|
||||||
|
await robot.right_arm.move_by(60, speed=80)
|
||||||
|
await robot.wait(100)
|
||||||
|
|
||||||
|
# Turn left and attempt to solve ants
|
||||||
|
await robot.turn(-90,then=robot.Stop.HOLD)
|
||||||
|
# go straight beyond the ants
|
||||||
|
await robot.move(35,then=robot.Stop.BRAKE)
|
||||||
|
|
||||||
|
#turn and align to ant then move forward
|
||||||
|
await robot.turn(130,then=robot.Stop.BRAKE)
|
||||||
|
await robot.move(20,speed=25,then=robot.Stop.HOLD)
|
||||||
|
|
||||||
|
#move back and turn towards home, attemp the windows to the past
|
||||||
|
await robot.move(-4,speed=60,then=robot.Stop.BRAKE)
|
||||||
|
await robot.turn(80,then=robot.Stop.HOLD)
|
||||||
|
|
||||||
|
#Return to home area and stop the robot
|
||||||
|
await robot.move(20,then = robot.Stop.NONE)
|
||||||
|
await robot.arc(60, degrees=60,then = robot.Stop.NONE)
|
||||||
|
robot.stop()
|
||||||
|
|
||||||
|
def test_something():
|
||||||
|
await robot.multitask(
|
||||||
|
#robot.move(10,speed=30,then=robot.Stop.BRAKE),
|
||||||
|
robot.left_arm.move_by(-45, speed=80),
|
||||||
|
robot.right_arm.move_by(-60, speed=80)
|
||||||
|
)
|
||||||
|
def test_ants_approach():
|
||||||
|
await robot.turn(135,then=robot.Stop.HOLD)
|
||||||
|
await robot.move(15,speed=30,then=robot.Sto)
|
||||||
|
await robot.move(-10,speed=60,then=robot.Stop.BRAKE)
|
||||||
|
await robot.turn(60,then=robot.Stop.HOLD)
|
||||||
|
await robot.move(90)
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
robot.run_task(solve_missions())
|
||||||
|
#robot.run_task(test_something())
|
||||||
|
#robot.run_task(test_ants_approach())
|
||||||
42
regionals/bioglow_mission_8.py
Normal file
42
regionals/bioglow_mission_8.py
Normal file
@@ -0,0 +1,42 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 8
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import robot # drive base, arms, sensors and moves all come from robot.py
|
||||||
|
|
||||||
|
# UNITS used in this file -----------------------------------------------------
|
||||||
|
# robot.move(cm) distance in cm (+ forward, - backward)
|
||||||
|
# robot.turn(degrees) degrees (+ right / clockwise, - left)
|
||||||
|
# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left),
|
||||||
|
# then degrees= around the circle or cm= along it (+ forward, - backward)
|
||||||
|
# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn)
|
||||||
|
# acceleration= how fast it speeds up and slows down:
|
||||||
|
# mm/s² for move/arc (normal 500), °/s² for turn (normal 200)
|
||||||
|
# (² means "per second": 200 mm/s² = 200 mm/s faster every second)
|
||||||
|
# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed
|
||||||
|
# ------------------------------------------------------------------------------
|
||||||
|
|
||||||
|
async def solve_missions(): # Research Platform - Johannes
|
||||||
|
|
||||||
|
# --- ARM HOMING (optional) -------------------------------------------
|
||||||
|
# Uncomment the next line to push both arms gently against their hard stops
|
||||||
|
# and call that spot 0, so every arm move starts from the same place each run.
|
||||||
|
# Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py).
|
||||||
|
# await robot.home_arms()
|
||||||
|
|
||||||
|
# --- LET THE GYRO SETTLE ----------------------------------------------
|
||||||
|
# Wait half a second (and until the hub is perfectly still) before moving,
|
||||||
|
# so turns are measured from a steady heading.
|
||||||
|
robot.reset()
|
||||||
|
|
||||||
|
await robot.move(71.5, speed=50, then=robot.Stop.HOLD)
|
||||||
|
await robot.turn(30, speed=30, then=robot.Stop.COAST) # circle on the RIGHT, forward 34.2 cm
|
||||||
|
await robot.move(10, speed=80)
|
||||||
|
await robot.lift_up(1050, speed=67) # was helper.multilift(1500)
|
||||||
|
await robot.lift_down(450, speed=93) # was helper.multilift(-250)
|
||||||
|
await robot.arc(85, cm=-70, speed=100, turn_speed=100, turn_acceleration=500, then=robot.Stop.NONE)
|
||||||
|
robot.stop()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
robot.run_task(solve_missions())
|
||||||
54
regionals/bioglow_mission_keystone.py
Normal file
54
regionals/bioglow_mission_keystone.py
Normal file
@@ -0,0 +1,54 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 14,15
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import robot # drive base, arms, sensors and moves all come from robot.py
|
||||||
|
|
||||||
|
# UNITS used in this file -----------------------------------------------------
|
||||||
|
# robot.move(cm) distance in cm (+ forward, - backward)
|
||||||
|
# robot.turn(degrees) degrees (+ right / clockwise, - left)
|
||||||
|
# robot.arc(radius_cm, ...) radius in cm (+ circle on the right, - on the left),
|
||||||
|
# then degrees= around the circle or cm= along it (+ forward, - backward)
|
||||||
|
# speed= % of top speed, 1-100 (normal: 80% drive, 72% turn)
|
||||||
|
# acceleration= how fast it speeds up and slows down:
|
||||||
|
# mm/s² for move/arc (normal 500), °/s² for turn (normal 200)
|
||||||
|
# (² means "per second": 200 mm/s² = 200 mm/s faster every second)
|
||||||
|
# arm.move_by(degrees, speed=) arm turn in degrees, speed in % of the arm's top speed
|
||||||
|
# ------------------------------------------------------------------------------
|
||||||
|
|
||||||
|
async def solve_missions(): # Seed of Renewal and Keystone Species - Parthiv
|
||||||
|
|
||||||
|
# --- ARM HOMING (optional) -------------------------------------------
|
||||||
|
# Uncomment the next line to push both arms gently against their hard stops
|
||||||
|
# and call that spot 0, so every arm move starts from the same place each run.
|
||||||
|
# Before using it: set home_backward for each arm in robot.py (run arm_calibrate.py).
|
||||||
|
# await robot.home_arms()
|
||||||
|
|
||||||
|
# --- LET THE GYRO SETTLE ----------------------------------------------
|
||||||
|
# Wait half a second (and until the hub is perfectly still) before moving,
|
||||||
|
# so turns are measured from a steady heading.
|
||||||
|
await robot.settle_gyro()
|
||||||
|
# BLUE COLOR CODE
|
||||||
|
robot.stop()
|
||||||
|
robot.reset()
|
||||||
|
|
||||||
|
await robot.move(65,then=robot.Stop.NONE)
|
||||||
|
await robot.turn(-90,then=robot.Stop.HOLD)
|
||||||
|
await robot.move(5,then=robot.Stop.NONE)
|
||||||
|
await robot.right_arm.move_by(-65,75)
|
||||||
|
await robot.move(-6.3,then=robot.Stop.HOLD)
|
||||||
|
await robot.right_arm.move_by(65,75)
|
||||||
|
await robot.move(-5,then=robot.Stop.NONE)
|
||||||
|
await robot.turn(137,then=robot.Stop.HOLD)
|
||||||
|
await robot.move(15.7,then=robot.Stop.HOLD)
|
||||||
|
await robot.move(-15.7,then=robot.Stop.HOLD)
|
||||||
|
await robot.turn(-35,then=robot.Stop.NONE)
|
||||||
|
await robot.move(-65,then=robot.Stop.HOLD)
|
||||||
|
await robot.stop()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
robot.run_task(solve_missions())
|
||||||
317
regionals/movement.py
Normal file
317
regionals/movement.py
Normal file
@@ -0,0 +1,317 @@
|
|||||||
|
"""
|
||||||
|
movement.py — easy robot moves in centimeters, degrees and % speed.
|
||||||
|
|
||||||
|
Set it up ONCE (in robot.py, after drive_base is made):
|
||||||
|
import movement
|
||||||
|
movement.initialize(drive_base, speed=80, turn_speed=72,
|
||||||
|
acceleration=500, turn_acceleration=200)
|
||||||
|
|
||||||
|
Use it in any mission (always with await):
|
||||||
|
await robot.move(30) # forward 30 cm, normal speed
|
||||||
|
await robot.move(-10, speed=40) # backward 10 cm at 40%
|
||||||
|
await robot.move(12, speed=18, acceleration=200) # slow and gentle
|
||||||
|
await robot.turn(90) # turn right 90 degrees
|
||||||
|
await robot.turn(-45, speed=30) # turn left 45 degrees, slowly
|
||||||
|
await robot.arc(20, degrees=90) # quarter circle to the right, 20 cm radius
|
||||||
|
await robot.arc(-35, cm=34) # curve left for 34 cm
|
||||||
|
await robot.move(2, then=robot.Stop.NONE) # don't stop, flow into the next move
|
||||||
|
await robot.move(12, speed=18, max_seconds=2) # push a model, but give up after 2 s
|
||||||
|
|
||||||
|
SPEED is a PERCENT of the robot's top speed: 1 = crawl, 100 = as fast as it goes.
|
||||||
|
move/arc: % of about 544 mm/s. turn: % of about 416 degrees/s.
|
||||||
|
ACCELERATION is how quickly it speeds up and slows down:
|
||||||
|
move/arc: mm/s per second. turn: degrees/s per second.
|
||||||
|
Lower = gentler (less slipping), higher = snappier.
|
||||||
|
Leave any of them out to use the normal values from robot.py.
|
||||||
|
A value you give is for THAT move only. The next move is back to normal.
|
||||||
|
|
||||||
|
It's speed, not raw power: the hub keeps the wheels at that speed and
|
||||||
|
still stops at exactly the right distance, even when the battery is low.
|
||||||
|
|
||||||
|
max_seconds= is a safety time limit for move, turn and arc. If a model or wall
|
||||||
|
blocks the robot, the move gives up after that many seconds instead of getting
|
||||||
|
stuck, and the run carries on. The move returns True if it finished, False if
|
||||||
|
it ran out of time. Leave it out for no time limit.
|
||||||
|
|
||||||
|
then= says what to do at the end of a move (same as Pybricks):
|
||||||
|
Stop.HOLD (default) stop and hold the position
|
||||||
|
Stop.COAST stop and roll freely
|
||||||
|
Stop.NONE don't stop, flow straight into the next move
|
||||||
|
"""
|
||||||
|
|
||||||
|
from pybricks.parameters import Stop
|
||||||
|
from pybricks.tools import wait, StopWatch, multitask
|
||||||
|
|
||||||
|
# The DriveBase. Set by initialize(), not here.
|
||||||
|
drive_base = None
|
||||||
|
|
||||||
|
# Normal values, used when a move doesn't give its own (set in robot.py).
|
||||||
|
_default_speed = 80 # % for move/arc
|
||||||
|
_default_turn = 72 # % for turn
|
||||||
|
_default_acc = 500 # mm/s per second for move/arc
|
||||||
|
_default_turn_acc = 200 # deg/s per second for turn
|
||||||
|
_max_speed = None # mm/s, the fastest this robot can drive straight (= 100%)
|
||||||
|
_max_turn = None # deg/s, the fastest it can turn (= 100%)
|
||||||
|
_start = None # the values from robot.py, for reset_settings()
|
||||||
|
|
||||||
|
|
||||||
|
def initialize(drive, speed=80, turn_speed=72, acceleration=500, turn_acceleration=200):
|
||||||
|
"""Connect the movement library to the robot's DriveBase and set the normal values."""
|
||||||
|
global drive_base, _max_speed, _max_turn, _start
|
||||||
|
drive_base = drive
|
||||||
|
# Top speeds come from the motors and wheel size, measured by Pybricks itself.
|
||||||
|
_max_speed = drive.distance_control.limits()[0]
|
||||||
|
_max_turn = drive.heading_control.limits()[0]
|
||||||
|
_start = (speed, turn_speed, acceleration, turn_acceleration)
|
||||||
|
set_default_speed(*_start)
|
||||||
|
|
||||||
|
|
||||||
|
def set_default_speed(speed=None, turn_speed=None, acceleration=None, turn_acceleration=None):
|
||||||
|
"""Change the NORMAL values for the rest of this run. Usually not needed:
|
||||||
|
give speed= or acceleration= on the move itself instead.
|
||||||
|
"""
|
||||||
|
global _default_speed, _default_turn, _default_acc, _default_turn_acc
|
||||||
|
if speed is not None:
|
||||||
|
_from_percent(speed, 1) # just checks it's 1-100
|
||||||
|
_default_speed = speed
|
||||||
|
if turn_speed is not None:
|
||||||
|
_from_percent(turn_speed, 1)
|
||||||
|
_default_turn = turn_speed
|
||||||
|
if acceleration is not None:
|
||||||
|
_default_acc = acceleration
|
||||||
|
if turn_acceleration is not None:
|
||||||
|
_default_turn_acc = turn_acceleration
|
||||||
|
_apply()
|
||||||
|
|
||||||
|
|
||||||
|
def reset_settings():
|
||||||
|
"""Go back to the normal values from robot.py. The menu does this before every run."""
|
||||||
|
_check()
|
||||||
|
set_default_speed(*_start)
|
||||||
|
|
||||||
|
|
||||||
|
def _apply(speed=None, turn_speed=None, acceleration=None, turn_acceleration=None):
|
||||||
|
"""Send the given values (or the normal ones) to the drive base."""
|
||||||
|
drive_base.settings(
|
||||||
|
straight_speed=_from_percent(_default_speed if speed is None else speed, _max_speed),
|
||||||
|
straight_acceleration=_default_acc if acceleration is None else acceleration,
|
||||||
|
turn_rate=_from_percent(_default_turn if turn_speed is None else turn_speed, _max_turn),
|
||||||
|
turn_acceleration=_default_turn_acc if turn_acceleration is None else turn_acceleration,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _check():
|
||||||
|
if drive_base is None:
|
||||||
|
raise RuntimeError(
|
||||||
|
"movement isn't set up. Add movement.initialize(drive_base) "
|
||||||
|
"in robot.py, after drive_base.settings(...)"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _mm(cm):
|
||||||
|
# Pybricks drops decimals, so round instead: 57.5 cm -> 575 mm
|
||||||
|
return int(round(cm * 10))
|
||||||
|
|
||||||
|
|
||||||
|
def _from_percent(percent, top, allow_zero=False):
|
||||||
|
"""Turn a percent (like 50) into a real speed (like 272 mm/s)."""
|
||||||
|
low = 0 if allow_zero else 1
|
||||||
|
if not low <= abs(percent) <= 100:
|
||||||
|
raise ValueError(
|
||||||
|
"speed is a percent from %d to 100, but got %s" % (low, percent)
|
||||||
|
)
|
||||||
|
return int(top * percent / 100)
|
||||||
|
|
||||||
|
|
||||||
|
async def _limited(motion, max_seconds):
|
||||||
|
"""Wait for a drive base move, or give up after max_seconds. True = finished."""
|
||||||
|
if max_seconds is None:
|
||||||
|
await motion
|
||||||
|
return True
|
||||||
|
|
||||||
|
async def finish():
|
||||||
|
await motion
|
||||||
|
return True
|
||||||
|
|
||||||
|
done, _ = await multitask(finish(), wait(int(max_seconds * 1000)), race=True)
|
||||||
|
if done is True:
|
||||||
|
return True
|
||||||
|
drive_base.stop() # ran out of time: stop pushing and carry on
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
async def move(cm, speed=None, acceleration=None, then=Stop.HOLD, max_seconds=None):
|
||||||
|
"""Drive straight.
|
||||||
|
|
||||||
|
cm: distance in centimeters. Positive = forward, negative = backward.
|
||||||
|
speed: % of top speed (1-100). Leave out for normal.
|
||||||
|
acceleration: mm/s per second. Leave out for normal.
|
||||||
|
then: Stop.HOLD (default), Stop.COAST, or Stop.NONE to flow into the next move.
|
||||||
|
max_seconds: give up after this long (for pushes that might get blocked).
|
||||||
|
"""
|
||||||
|
_check()
|
||||||
|
_apply(speed=speed, acceleration=acceleration)
|
||||||
|
return await _limited(drive_base.straight(_mm(cm), then=then), max_seconds)
|
||||||
|
|
||||||
|
|
||||||
|
async def turn(degrees, speed=None, acceleration=None, then=Stop.HOLD, max_seconds=None):
|
||||||
|
"""Turn in place.
|
||||||
|
|
||||||
|
degrees: positive = right (clockwise), negative = left.
|
||||||
|
speed: % of top turning speed (1-100). Leave out for normal.
|
||||||
|
acceleration: degrees/s per second. Leave out for normal.
|
||||||
|
then: Stop.HOLD (default), Stop.COAST, or Stop.NONE.
|
||||||
|
max_seconds: give up after this long.
|
||||||
|
"""
|
||||||
|
_check()
|
||||||
|
_apply(turn_speed=speed, turn_acceleration=acceleration)
|
||||||
|
return await _limited(drive_base.turn(degrees, then=then), max_seconds)
|
||||||
|
|
||||||
|
|
||||||
|
async def arc(radius_cm, degrees=None, cm=None, speed=None, turn_speed=None,
|
||||||
|
acceleration=None, turn_acceleration=None, then=Stop.HOLD, max_seconds=None):
|
||||||
|
"""Drive along part of a circle. Give degrees OR cm, not both.
|
||||||
|
|
||||||
|
radius_cm: size of the circle in cm.
|
||||||
|
Positive = circle to the RIGHT, negative = circle to the LEFT.
|
||||||
|
degrees: how far around the circle to go.
|
||||||
|
Positive = forward, negative = backward.
|
||||||
|
cm: or: how far to drive along the circle, in cm.
|
||||||
|
Positive = forward, negative = backward.
|
||||||
|
speed, acceleration: limits for driving along the circle.
|
||||||
|
turn_speed, turn_acceleration: limits for how fast it turns while curving.
|
||||||
|
then: Stop.HOLD (default), Stop.COAST, or Stop.NONE.
|
||||||
|
max_seconds: give up after this long.
|
||||||
|
|
||||||
|
Examples:
|
||||||
|
await robot.arc(20, degrees=90) # quarter circle forward, curving right
|
||||||
|
await robot.arc(-35, degrees=90) # quarter circle forward, curving left
|
||||||
|
await robot.arc(128, cm=34) # 34 cm along a big right-hand curve
|
||||||
|
"""
|
||||||
|
_check()
|
||||||
|
if (degrees is None) == (cm is None):
|
||||||
|
raise ValueError("arc needs degrees=... or cm=..., but not both")
|
||||||
|
_apply(speed, turn_speed, acceleration, turn_acceleration)
|
||||||
|
if cm is None:
|
||||||
|
motion = drive_base.arc(_mm(radius_cm), angle=degrees, then=then)
|
||||||
|
else:
|
||||||
|
motion = drive_base.arc(_mm(radius_cm), distance=_mm(cm), then=then)
|
||||||
|
return await _limited(motion, max_seconds)
|
||||||
|
|
||||||
|
|
||||||
|
async def move_for_seconds(seconds, speed=None, backward=False, acceleration=None):
|
||||||
|
"""Drive straight for a set time, then stop.
|
||||||
|
|
||||||
|
seconds: how long to drive, like 1.5
|
||||||
|
speed: % of top speed (1-100). Leave out for normal.
|
||||||
|
backward: True to drive backward.
|
||||||
|
acceleration: mm/s per second. Leave out for normal.
|
||||||
|
|
||||||
|
Good for pushing a mission model. For exact distances, use move() instead.
|
||||||
|
"""
|
||||||
|
_check()
|
||||||
|
_apply(acceleration=acceleration)
|
||||||
|
mm_s = _from_percent(_default_speed if speed is None else speed, _max_speed)
|
||||||
|
drive_base.drive(-mm_s if backward else mm_s, 0)
|
||||||
|
try:
|
||||||
|
await wait(int(seconds * 1000))
|
||||||
|
finally:
|
||||||
|
drive_base.brake() # also stops if a multitask race cancels us
|
||||||
|
|
||||||
|
|
||||||
|
async def move_until_stalled(speed=None, backward=False, max_seconds=5, acceleration=None):
|
||||||
|
"""Drive straight until something blocks the robot, then stop.
|
||||||
|
|
||||||
|
"Blocked" means EITHER Pybricks says the wheels are stalled, OR the robot
|
||||||
|
has almost stopped moving forward (so it also works on light, springy
|
||||||
|
models that never make the motors work at full power).
|
||||||
|
It can't notice wheels spinning in place, so max_seconds always applies.
|
||||||
|
|
||||||
|
speed: % of top speed (1-100). Slower is gentler: try 20-40.
|
||||||
|
backward: True to drive backward.
|
||||||
|
max_seconds: give up after this long.
|
||||||
|
acceleration: mm/s per second. Leave out for normal.
|
||||||
|
|
||||||
|
Returns True if it got blocked, False if it gave up.
|
||||||
|
Example:
|
||||||
|
hit = await move_until_stalled(speed=30, backward=True)
|
||||||
|
if not hit:
|
||||||
|
print("Never reached the wall!")
|
||||||
|
"""
|
||||||
|
_check()
|
||||||
|
_apply(acceleration=acceleration)
|
||||||
|
mm_s = _from_percent(_default_speed if speed is None else speed, _max_speed)
|
||||||
|
drive_base.drive(-mm_s if backward else mm_s, 0)
|
||||||
|
|
||||||
|
acc = _default_acc if acceleration is None else acceleration
|
||||||
|
ramp_ms = int(1000 * mm_s / acc) + 100 # time to reach full speed, plus a moment
|
||||||
|
window_ms = 200 # check progress every 0.2 s
|
||||||
|
min_progress = max(1, mm_s * window_ms / 1000 / 4) # under 1/4 of normal = stuck
|
||||||
|
|
||||||
|
timer = StopWatch()
|
||||||
|
window = StopWatch()
|
||||||
|
last = drive_base.distance()
|
||||||
|
try:
|
||||||
|
await wait(100) # give the wheels a moment to start rolling
|
||||||
|
while timer.time() < max_seconds * 1000:
|
||||||
|
if drive_base.stalled():
|
||||||
|
return True
|
||||||
|
if window.time() >= window_ms:
|
||||||
|
now = drive_base.distance()
|
||||||
|
if timer.time() > ramp_ms and abs(now - last) < min_progress:
|
||||||
|
return True # barely moving: something is in the way
|
||||||
|
last = now
|
||||||
|
window.reset()
|
||||||
|
await wait(10)
|
||||||
|
return False
|
||||||
|
finally:
|
||||||
|
drive_base.stop() # stop pushing, even if a race cancels us
|
||||||
|
|
||||||
|
|
||||||
|
def drive(speed, steer=0):
|
||||||
|
"""Start driving and keep going until stop() is called.
|
||||||
|
|
||||||
|
speed: percent of top speed, -100 to 100. Negative = backward.
|
||||||
|
steer: percent of top turning speed, -100 to 100. Positive = steer right.
|
||||||
|
Doesn't wait, so don't use await on it. Example:
|
||||||
|
drive(40)
|
||||||
|
while await lazer_ranger.distance() > 100:
|
||||||
|
await wait(10)
|
||||||
|
stop()
|
||||||
|
"""
|
||||||
|
_check()
|
||||||
|
_apply()
|
||||||
|
drive_base.drive(
|
||||||
|
_from_percent(speed, _max_speed, allow_zero=True),
|
||||||
|
_from_percent(steer, _max_turn, allow_zero=True),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def stop():
|
||||||
|
"""Stop the wheels and let the robot roll to a stop."""
|
||||||
|
_check()
|
||||||
|
drive_base.stop()
|
||||||
|
|
||||||
|
|
||||||
|
def brake():
|
||||||
|
"""Stop the wheels quickly."""
|
||||||
|
_check()
|
||||||
|
drive_base.brake()
|
||||||
|
|
||||||
|
|
||||||
|
def reset():
|
||||||
|
"""Set the distance and angle counters back to 0 (same as drive_base.reset(0, 0))."""
|
||||||
|
_check()
|
||||||
|
drive_base.reset()
|
||||||
|
|
||||||
|
|
||||||
|
def distance_cm():
|
||||||
|
"""How far the robot has driven since the last reset(), in cm."""
|
||||||
|
_check()
|
||||||
|
return drive_base.distance() / 10
|
||||||
|
|
||||||
|
|
||||||
|
def angle():
|
||||||
|
"""How far the robot has turned since the last reset(), in degrees."""
|
||||||
|
_check()
|
||||||
|
return drive_base.angle()
|
||||||
116
regionals/robot.py
Normal file
116
regionals/robot.py
Normal file
@@ -0,0 +1,116 @@
|
|||||||
|
"""
|
||||||
|
robot.py — everything about OUR robot, in one place.
|
||||||
|
(Replaces bioglow_robot_settings.py.)
|
||||||
|
|
||||||
|
Every mission starts with:
|
||||||
|
import robot
|
||||||
|
|
||||||
|
Then:
|
||||||
|
await robot.move(55) # forward 55 cm
|
||||||
|
await robot.turn(-90) # left 90 degrees
|
||||||
|
await robot.arc(-35, degrees=90) # curve left
|
||||||
|
await robot.left_arm.move_by(100, speed=47) # arms (see arm.py)
|
||||||
|
robot.color_sensor, robot.lazer_ranger, robot.hub
|
||||||
|
|
||||||
|
RULE: motors and sensors are created ONLY in this file.
|
||||||
|
Creating Motor(Port.A) anywhere else gives a "port already in use" error.
|
||||||
|
|
||||||
|
Important: test all code with the battery at 100%, and make all updates
|
||||||
|
with the robot at full charge.
|
||||||
|
"""
|
||||||
|
|
||||||
|
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, wait, StopWatch
|
||||||
|
from pybricks.robotics import DriveBase
|
||||||
|
from pybricks.hubs import PrimeHub
|
||||||
|
import umath
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------- hub
|
||||||
|
hub = PrimeHub()
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------- wheels
|
||||||
|
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||||
|
right_motor = Motor(Port.B, Direction.CLOCKWISE)
|
||||||
|
|
||||||
|
WHEEL_DIAMETER = 62.4 # mm (measure your wheels)
|
||||||
|
AXLE_TRACK = 150 # mm (distance between the two wheels)
|
||||||
|
|
||||||
|
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||||
|
drive_base.use_gyro(True)
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------- arms
|
||||||
|
# Gear trains: both trains go in ONE list. 2.33:1 reduction, so top speed is ~430 deg/s.
|
||||||
|
left_arm_motor = Motor(Port.C, Direction.CLOCKWISE, [[20, 28], [12, 20]])
|
||||||
|
right_arm_motor = Motor(Port.D, Direction.CLOCKWISE, [[20, 28], [12, 20]])
|
||||||
|
|
||||||
|
# Each arm gets % speeds, named positions and homing (see arm.py).
|
||||||
|
# Before using home() or move_to(), on the robot:
|
||||||
|
# 1. set home_backward: which way each arm turns to reach its hard stop
|
||||||
|
# 2. add positions, measured from that stop (run arm_calibrate.py),
|
||||||
|
# for example positions={"down": 0, "up": 120}
|
||||||
|
from arm import Arm
|
||||||
|
left_arm = Arm(left_arm_motor, speed=50, positions={}, home_backward=True)
|
||||||
|
right_arm = Arm(right_arm_motor, speed=50, positions={}, home_backward=False)
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------- sensors
|
||||||
|
lazer_ranger = UltrasonicSensor(Port.E) # mounted at the BACK of the robot
|
||||||
|
color_sensor = ColorSensor(Port.F)
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------- moves
|
||||||
|
# movement.py does the work, so robot.move(55) means "drive 55 cm".
|
||||||
|
#
|
||||||
|
# NORMAL SPEEDS AND ACCELERATIONS. Used whenever a move doesn't give its own:
|
||||||
|
# await robot.move(30) -> normal values
|
||||||
|
# await robot.move(30, speed=40, acceleration=200) -> just for this move
|
||||||
|
# Speeds are % of top speed: 100% straight = about 544 mm/s, 100% turning = about 416 deg/s.
|
||||||
|
DEFAULT_SPEED = 80 # % for move() and arc() (about 435 mm/s)
|
||||||
|
DEFAULT_TURN_SPEED = 72 # % for turn() (about 300 deg/s)
|
||||||
|
DEFAULT_ACCELERATION = 500 # mm/s per second, move() and arc()
|
||||||
|
DEFAULT_TURN_ACCELERATION = 200 # deg/s per second, turn()
|
||||||
|
|
||||||
|
import movement
|
||||||
|
movement.initialize(drive_base, speed=DEFAULT_SPEED, turn_speed=DEFAULT_TURN_SPEED,
|
||||||
|
acceleration=DEFAULT_ACCELERATION,
|
||||||
|
turn_acceleration=DEFAULT_TURN_ACCELERATION)
|
||||||
|
from movement import (move, turn, arc, move_for_seconds, move_until_stalled,
|
||||||
|
drive, stop, brake, reset, distance_cm, angle,
|
||||||
|
set_default_speed, reset_settings)
|
||||||
|
|
||||||
|
|
||||||
|
async def settle_gyro(ms=250, max_seconds=2):
|
||||||
|
"""Let the gyro settle before a run starts.
|
||||||
|
|
||||||
|
Waits ms milliseconds (default half a second), then keeps waiting until the
|
||||||
|
hub says it's perfectly still, so the first turn starts from a steady heading.
|
||||||
|
Gives up after max_seconds, so a run never gets stuck here.
|
||||||
|
"""
|
||||||
|
await wait(ms)
|
||||||
|
timer = StopWatch()
|
||||||
|
while timer.time() < max_seconds * 1000:
|
||||||
|
still = hub.imu.stationary()
|
||||||
|
ready = hub.imu.ready() if hasattr(hub.imu, "ready") else True
|
||||||
|
if still and ready:
|
||||||
|
return
|
||||||
|
await wait(10)
|
||||||
|
|
||||||
|
|
||||||
|
async def home_arms():
|
||||||
|
"""Find both arms' starting positions. Check home_backward first!"""
|
||||||
|
await multitask(left_arm.home(), right_arm.home())
|
||||||
|
|
||||||
|
|
||||||
|
async def lift_up(degrees, speed=None):
|
||||||
|
"""Raise both arms together (they face opposite ways). speed is in %."""
|
||||||
|
await multitask(
|
||||||
|
right_arm.move_by(degrees, speed),
|
||||||
|
left_arm.move_by(-degrees, speed),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
async def lift_down(degrees, speed=None):
|
||||||
|
"""Lower both arms together. speed is in %."""
|
||||||
|
await multitask(
|
||||||
|
right_arm.move_by(-degrees, speed),
|
||||||
|
left_arm.move_by(degrees, speed),
|
||||||
|
)
|
||||||
@@ -1,3 +1,9 @@
|
|||||||
|
########################################################
|
||||||
|
# bioglow_helpers.py
|
||||||
|
# - This file contain all the helper functions and tools
|
||||||
|
# - This is where we add special or custom functionality codes
|
||||||
|
########################################################
|
||||||
|
|
||||||
"""
|
"""
|
||||||
PID Straight Drive — heading-corrected forward / backward movement.
|
PID Straight Drive — heading-corrected forward / backward movement.
|
||||||
|
|
||||||
@@ -12,7 +18,7 @@ Improvements over baseline:
|
|||||||
• Integral freeze near target — stops windup right where it matters most
|
• Integral freeze near target — stops windup right where it matters most
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from bioglow_settings import * # hub, motors, sensors, drive_base, wait, umath, ...
|
from bioglow_robot_settings import * # hub, motors, sensors, drive_base, wait, umath, ...
|
||||||
|
|
||||||
LOOP_MS = 20 # fixed control loop period
|
LOOP_MS = 20 # fixed control loop period
|
||||||
RAMP_MM = 50 # begin decelerating this far from target
|
RAMP_MM = 50 # begin decelerating this far from target
|
||||||
@@ -173,4 +179,35 @@ async def multilift_down(rotation_angle):
|
|||||||
right_arm.run_angle(1000,-1*rotation_angle),
|
right_arm.run_angle(1000,-1*rotation_angle),
|
||||||
left_arm.run_angle(1000,rotation_angle)
|
left_arm.run_angle(1000,rotation_angle)
|
||||||
)
|
)
|
||||||
|
|
||||||
|
async def drive_until_stalled(max_distance):
|
||||||
|
stalled_flag = False
|
||||||
|
|
||||||
|
async def drive_task():
|
||||||
|
await drive_base.straight(max_distance)
|
||||||
|
|
||||||
|
async def stall_watch():
|
||||||
|
nonlocal stalled_flag
|
||||||
|
while not (left_motor.control.stalled() and right_motor.control.stalled()):
|
||||||
|
await wait(10)
|
||||||
|
stalled_flag = True
|
||||||
|
drive_base.stop()
|
||||||
|
print("Both wheels stalled — robot is stuck!")
|
||||||
|
|
||||||
|
await multitask(drive_task(), stall_watch(), race=True)
|
||||||
|
return stalled_flag
|
||||||
|
|
||||||
|
def reset_arms():
|
||||||
|
left_arm.reset_angle(0)
|
||||||
|
right_arm.reset_angle(0)
|
||||||
|
print('limits before:')
|
||||||
|
print(left_arm.control.limits())
|
||||||
|
print(left_arm.control.limits())
|
||||||
|
|
||||||
|
left_arm.control.limits(speed=1100, acceleration=2000)
|
||||||
|
right_arm.control.limits(speed=1100, acceleration=2000)
|
||||||
|
|
||||||
|
print('limits after:')
|
||||||
|
print(left_arm.control.limits())
|
||||||
|
print(left_arm.control.limits())
|
||||||
|
|
||||||
|
|||||||
31
shout-fll-scrimmage/bioglow_mission_1.py
Normal file
31
shout-fll-scrimmage/bioglow_mission_1.py
Normal file
@@ -0,0 +1,31 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 1
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Drone Survey - Johannes
|
||||||
|
|
||||||
|
await multitask(
|
||||||
|
left_arm.run_angle(400,-300),
|
||||||
|
right_arm.run_angle(400,100),
|
||||||
|
drive_base.straight(900)
|
||||||
|
)
|
||||||
|
await drive_base.turn(-25)
|
||||||
|
await drive_base.straight(110)
|
||||||
|
await drive_base.turn(-35)
|
||||||
|
await drive_base.turn(35)
|
||||||
|
await drive_base.straight(-30)
|
||||||
|
await drive_base.turn(25)
|
||||||
|
await drive_base.straight(-200)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(500)
|
||||||
|
await drive_base.turn(75)
|
||||||
|
await drive_base.straight(700)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
36
shout-fll-scrimmage/bioglow_mission_11_experimental.py
Normal file
36
shout-fll-scrimmage/bioglow_mission_11_experimental.py
Normal file
@@ -0,0 +1,36 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 11
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Windows to the past
|
||||||
|
|
||||||
|
drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns
|
||||||
|
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(800,-90),
|
||||||
|
drive_base.straight(100)
|
||||||
|
)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(420)
|
||||||
|
helper.set_speed(100,100,100,100)
|
||||||
|
await multitask(
|
||||||
|
left_arm.run_angle(500,45),
|
||||||
|
right_arm.run_angle(800,140)
|
||||||
|
)
|
||||||
|
helper.set_default_speed()
|
||||||
|
|
||||||
|
#return home
|
||||||
|
await drive_base.straight(-500)
|
||||||
|
await multitask(
|
||||||
|
left_arm.run_angle(100,-45),
|
||||||
|
right_arm.run_angle(800,-90)
|
||||||
|
)
|
||||||
|
#helper.reset_arms()
|
||||||
|
helper.set_default_speed()
|
||||||
|
drive_base.stop()
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
29
shout-fll-scrimmage/bioglow_mission_12.py
Normal file
29
shout-fll-scrimmage/bioglow_mission_12.py
Normal file
@@ -0,0 +1,29 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 12
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Forest Elder - Parthiv
|
||||||
|
|
||||||
|
await drive_base.straight(475)
|
||||||
|
await right_arm.run_angle(300,25)
|
||||||
|
await drive_base.turn(-20)
|
||||||
|
await left_arm.run_angle(200,245)
|
||||||
|
await drive_base.straight(60)
|
||||||
|
await drive_base.turn(25)
|
||||||
|
await left_arm.run_angle(100,-56.5)
|
||||||
|
await drive_base.straight(-50)
|
||||||
|
await drive_base.straight(50)
|
||||||
|
await drive_base.turn(-25)
|
||||||
|
await left_arm.run_angle(200,-200)
|
||||||
|
await drive_base.straight(-240)
|
||||||
|
await drive_base.turn(15)
|
||||||
|
await right_arm.run_angle(200,-200)
|
||||||
|
await drive_base.straight(-200)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
29
shout-fll-scrimmage/bioglow_mission_13.py
Normal file
29
shout-fll-scrimmage/bioglow_mission_13.py
Normal file
@@ -0,0 +1,29 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 13
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Interchangeable Mission - Parthiv
|
||||||
|
|
||||||
|
await left_arm.run_angle(200,-80)
|
||||||
|
await drive_base.arc(-350,angle=90)
|
||||||
|
await drive_base.turn(50)
|
||||||
|
await drive_base.straight(238)
|
||||||
|
await drive_base.turn(-33)
|
||||||
|
await drive_base.straight(85)
|
||||||
|
await drive_base.straight(-33)
|
||||||
|
await multitask(
|
||||||
|
left_arm.run_angle(200,80),
|
||||||
|
right_arm.run_angle(200,-100)
|
||||||
|
)
|
||||||
|
await drive_base.straight(-60)
|
||||||
|
await right_arm.run_angle(200,100)
|
||||||
|
await drive_base.turn(30)
|
||||||
|
await drive_base.straight(-775)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
58
shout-fll-scrimmage/bioglow_mission_2.py
Normal file
58
shout-fll-scrimmage/bioglow_mission_2.py
Normal file
@@ -0,0 +1,58 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 2
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Exploding Seeds - Johannes
|
||||||
|
|
||||||
|
drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns
|
||||||
|
await multitask(
|
||||||
|
drive_base.arc(150,None,115),
|
||||||
|
left_arm.run_angle(300,40),
|
||||||
|
right_arm.run_angle(300,90)
|
||||||
|
)
|
||||||
|
|
||||||
|
# drive to mission until robot will stall
|
||||||
|
dist_to_model = 350
|
||||||
|
rev_move = -1 * dist_to_model
|
||||||
|
|
||||||
|
reach_mission = await helper.drive_until_stalled(dist_to_model)
|
||||||
|
if reach_mission:
|
||||||
|
print("Exploding seeds reach.Solving now...")
|
||||||
|
else:
|
||||||
|
print(f"Drove full {dist_to_model}, no stall")
|
||||||
|
|
||||||
|
await left_arm.run_angle(100,-40)
|
||||||
|
#await left_arm.run_until_stalled(-100, then=Stop.HOLD, duty_limit=5)
|
||||||
|
await drive_base.straight(-60)
|
||||||
|
await left_arm.run_angle(300,-90)
|
||||||
|
await multitask(
|
||||||
|
drive_base.straight(rev_move),
|
||||||
|
right_arm.run_angle(500,-90),
|
||||||
|
left_arm.run_angle(500,-80)
|
||||||
|
)
|
||||||
|
# Go towards tangled mission
|
||||||
|
helper.set_default_speed()
|
||||||
|
#helper.set_speed(600,200,100,100)
|
||||||
|
await drive_base.turn(-23)
|
||||||
|
await drive_base.straight(700,Stop.COAST) # move fast
|
||||||
|
|
||||||
|
#slowing down here
|
||||||
|
helper.set_speed(100,200,300,200)
|
||||||
|
await drive_base.straight(120)
|
||||||
|
|
||||||
|
await right_arm.run_angle(700,70) #raise arm to untangled
|
||||||
|
|
||||||
|
await drive_base.straight(-100,Stop.COAST) # moving back
|
||||||
|
helper.set_speed(800,500,300,200)
|
||||||
|
|
||||||
|
await multitask(
|
||||||
|
drive_base.straight(-800),
|
||||||
|
right_arm.run_angle(300,90)
|
||||||
|
)
|
||||||
|
drive_base.stop()
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
37
shout-fll-scrimmage/bioglow_mission_3_4_5_experimental.py
Normal file
37
shout-fll-scrimmage/bioglow_mission_3_4_5_experimental.py
Normal file
@@ -0,0 +1,37 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 3,4,5
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Flip the rock,etc
|
||||||
|
|
||||||
|
drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns
|
||||||
|
|
||||||
|
print (left_arm.angle(),right_arm.angle())
|
||||||
|
helper.set_speed(600,200,300,300)
|
||||||
|
await drive_base.straight(400,Stop.COAST)
|
||||||
|
|
||||||
|
helper.set_speed(800,300,300,300)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
|
||||||
|
helper.set_speed(100,100,100,100)
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(100,-60),
|
||||||
|
left_arm.run_angle(100,60),
|
||||||
|
drive_base.straight(-60)
|
||||||
|
)
|
||||||
|
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(50,50),
|
||||||
|
left_arm.run_angle(50,-50),
|
||||||
|
drive_base.straight(-40)
|
||||||
|
)
|
||||||
|
helper.set_default_speed()
|
||||||
|
await drive_base.straight(-300)
|
||||||
|
await drive_base.straight(-100)
|
||||||
|
|
||||||
|
drive_base.stop()
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
22
shout-fll-scrimmage/bioglow_mission_8.py
Normal file
22
shout-fll-scrimmage/bioglow_mission_8.py
Normal file
@@ -0,0 +1,22 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 8
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Research Platform - Johannes
|
||||||
|
|
||||||
|
await drive_base.arc(900,None,400)
|
||||||
|
await drive_base.arc(-500,None,250)
|
||||||
|
helper.set_speed(100,400,300,200)
|
||||||
|
await drive_base.straight(200)
|
||||||
|
await helper.multilift_up(1950)
|
||||||
|
await helper.multilift_down(500)
|
||||||
|
helper.set_speed(1000,500,300,200)
|
||||||
|
await drive_base.arc(-1000,None,-900)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
51
shout-fll-scrimmage/bioglow_missions_11_6_7.py
Normal file
51
shout-fll-scrimmage/bioglow_missions_11_6_7.py
Normal file
@@ -0,0 +1,51 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Missions 11,6,7
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Window to the Past, Lead Cutter, Humungous Fungus - Johannes
|
||||||
|
|
||||||
|
await drive_base.straight(110)
|
||||||
|
await drive_base.turn(-45)
|
||||||
|
await drive_base.straight(280)
|
||||||
|
await right_arm.run_angle(300,120)
|
||||||
|
await left_arm.run_angle(400,200)
|
||||||
|
await drive_base.straight(-70)
|
||||||
|
await left_arm.run_angle(600,-110)
|
||||||
|
await drive_base.straight(370)
|
||||||
|
await drive_base.turn(50)
|
||||||
|
await multitask(
|
||||||
|
drive_base.straight(250),
|
||||||
|
left_arm.run_angle(600,-60)
|
||||||
|
)
|
||||||
|
await drive_base.turn(40)
|
||||||
|
helper.set_speed(100,200,300,200)
|
||||||
|
await drive_base.straight(170)
|
||||||
|
await drive_base.straight(-135)
|
||||||
|
await drive_base.turn(80)
|
||||||
|
await drive_base.straight(85)
|
||||||
|
await multitask(
|
||||||
|
drive_base.turn(-36),
|
||||||
|
right_arm.run_angle(200,-140)
|
||||||
|
)
|
||||||
|
helper.set_speed(100,200,300,200)
|
||||||
|
await drive_base.straight(230)
|
||||||
|
await multitask(
|
||||||
|
right_arm.run_angle(300,160),
|
||||||
|
left_arm.run_angle(300,150)
|
||||||
|
)
|
||||||
|
await wait(200)
|
||||||
|
await drive_base.straight(30)
|
||||||
|
await drive_base.turn(-90)
|
||||||
|
await drive_base.straight(-170)
|
||||||
|
helper.set_speed(600,500,300,200)
|
||||||
|
await drive_base.straight(100)
|
||||||
|
await drive_base.arc(800,None,-900)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
25
shout-fll-scrimmage/bioglow_missions_14_15.py
Normal file
25
shout-fll-scrimmage/bioglow_missions_14_15.py
Normal file
@@ -0,0 +1,25 @@
|
|||||||
|
#####################################
|
||||||
|
# Solution for BioGlow Mission 14,15
|
||||||
|
#####################################
|
||||||
|
|
||||||
|
import bioglow_helpers as helper
|
||||||
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
|
async def solve_missions(): # Seed of Renewal and Keystone Species - Parthiv
|
||||||
|
|
||||||
|
await drive_base.straight(600)
|
||||||
|
await drive_base.arc(400,angle=35)
|
||||||
|
await drive_base.straight(-40)
|
||||||
|
await right_arm.run_angle(150,-70)
|
||||||
|
await drive_base.arc(400,angle=-35)
|
||||||
|
await drive_base.straight(-540)
|
||||||
|
await drive_base.turn(-90)
|
||||||
|
await drive_base.straight(200)
|
||||||
|
await left_arm.run_angle(100,100)
|
||||||
|
await drive_base.straight(-350)
|
||||||
|
await drive_base.stop
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
|
run_task(solve_missions())
|
||||||
@@ -5,23 +5,26 @@
|
|||||||
import bioglow_helpers as helper
|
import bioglow_helpers as helper
|
||||||
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings
|
||||||
|
|
||||||
async def solve_missions(): # Flip The Rock, Lucky Leaves, Reaching Roots - Rishabh
|
async def solve_missions(): # Flip The Rock, Lucky Leaves, Reaching Roots - Parthiv
|
||||||
|
|
||||||
await drive_base.straight(550)#Works (600(Previous))
|
await drive_base.straight(415)
|
||||||
await drive_base.straight(-200)
|
await drive_base.straight(-150)
|
||||||
await drive_base.turn(45)
|
await drive_base.turn(45)
|
||||||
await drive_base.straight(575)
|
await drive_base.straight(600)
|
||||||
await drive_base.turn(-90)#-90 for Coach Cisco table
|
await drive_base.turn(-90)
|
||||||
await left_arm.run_angle(200,270)
|
await multitask(
|
||||||
|
left_arm.run_angle(200,300),
|
||||||
|
right_arm.run_angle(200,-290)
|
||||||
|
)
|
||||||
|
set_speed(100,500,300,200)
|
||||||
await drive_base.straight(300)
|
await drive_base.straight(300)
|
||||||
await left_arm.run_angle(200,-265)
|
await left_arm.run_angle(200,-300)
|
||||||
await right_arm.run_angle(200, -200)
|
await drive_base.straight(-95)
|
||||||
await drive_base.straight(-105)#Use -105 for Coach Cisco table(-105 for school)
|
await drive_base.turn(22)
|
||||||
await drive_base.turn(32)#was 24-Use 24 for Coach Cisco's table(32 for school)
|
await right_arm.run_angle(5000,100)
|
||||||
await right_arm.run_angle(1500, 180)
|
await drive_base.straight(90)
|
||||||
await drive_base.turn(30)
|
set_speed(700,500,300,200)
|
||||||
await drive_base.straight(-1000)
|
await drive_base.arc(-700,angle=-150)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py
|
||||||
run_task(solve_missions())
|
run_task(solve_missions())
|
||||||
@@ -1,3 +1,9 @@
|
|||||||
|
####################################################
|
||||||
|
# bioglow_robot_settings.py
|
||||||
|
# This file contains all the common robot settings
|
||||||
|
# Only change this when necessary
|
||||||
|
#####################################################
|
||||||
|
|
||||||
#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge.
|
#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge.
|
||||||
|
|
||||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||||
|
|||||||
@@ -1,24 +1,42 @@
|
|||||||
|
#####################################
|
||||||
|
# Main.py #
|
||||||
|
# File for running all MISSIONS #
|
||||||
|
# together #
|
||||||
|
# #
|
||||||
|
#####################################
|
||||||
|
|
||||||
import bioglow_helpers as helper
|
import bioglow_helpers as helper
|
||||||
import bioglow_settings as robot
|
import bioglow_robot_settings as robot
|
||||||
import bioglow_mission_3_4_5 as Run_1
|
import bioglow_missions_3_4_5 as Run_1 # import here the name of files containing mission runs Run_1 for run 1,
|
||||||
|
import bioglow_mission_2 as Run_2 # Run_2 for run 2, and so on..
|
||||||
|
import bioglow_mission_1 as Run_3
|
||||||
|
import bioglow_missions_11_6_7 as Run_4
|
||||||
|
import bioglow_mission_13 as Run_5
|
||||||
|
import bioglow_mission_12 as Run_6
|
||||||
|
import bioglow_missions_14_15 as Run_7
|
||||||
|
import bioglow_mission_8 as Run_8
|
||||||
|
|
||||||
# Function to classify color based on HSV
|
# Function to classify color based on HSV
|
||||||
def detect_color(h, s, v, reflected):
|
def detect_color(h, s, v, reflected):
|
||||||
if reflected > 4:
|
if reflected > 4:
|
||||||
if h < 4 or h > 350: # red
|
if 1 < h < 11 and s <50: # brown
|
||||||
return "Red"
|
return "Brown"
|
||||||
elif 3 < h < 24 and s > 80: # orange
|
elif 11 < h < 24 and s > 80: # orange
|
||||||
return "Orange"
|
return "Orange"
|
||||||
elif 30 < h < 60 and v > 50: # yellow
|
elif 30 < h < 60 and v > 50: # yellow
|
||||||
return "Yellow"
|
return "Yellow"
|
||||||
elif 70 < h < 160: # green - do it vertically not horizontally for accuracy
|
elif 70 < h < 90 and s > 60: # neon green
|
||||||
|
return "Lime"
|
||||||
|
elif 100 < h < 160: # green - do it vertically not horizontally for accuracy
|
||||||
return "Green"
|
return "Green"
|
||||||
elif 195 < h < 198: # light blue
|
elif 195 < h < 198: # light blue
|
||||||
return "Light_Blue"
|
return "Cyan"
|
||||||
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
|
elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy
|
||||||
return "Blue"
|
return "Blue"
|
||||||
elif 260 < h < 350: # purple
|
elif 260 < h < 350 and s > 80: # purple
|
||||||
return "Purple"
|
return "Purple"
|
||||||
|
elif 350 < h and s > 50: #red
|
||||||
|
return "Red"
|
||||||
elif s < 15 and v > 70: # white
|
elif s < 15 and v > 70: # white
|
||||||
return "White"
|
return "White"
|
||||||
else:
|
else:
|
||||||
@@ -38,30 +56,37 @@ async def main():
|
|||||||
#print(color)
|
#print(color)
|
||||||
print(f"button pressed: {pressed}")
|
print(f"button pressed: {pressed}")
|
||||||
|
|
||||||
|
#TODO: Add the solve_missions() function of the run number for each color here
|
||||||
if color == "Green":
|
if color == "Green":
|
||||||
print('Running Mission 1, Detect Green')
|
print('Running Mission 1, Detect Green')
|
||||||
await Run_1.solve_missions()
|
await Run_1.solve_missions()
|
||||||
elif color == "Red":
|
elif color == "Red":
|
||||||
print('Running Mission 2, Detect Red')
|
print('Running Mission 2, Detect Red')
|
||||||
pass # TODO: Run2 not written yet (it crashed with NameError)
|
await Run_2.solve_missions()
|
||||||
elif color == "Yellow":
|
elif color == "Yellow":
|
||||||
print('Running Mission 3, Detect Yellow')
|
print('Running Mission 3, Detect Yellow')
|
||||||
pass # TODO: Run3 not written yet (it crashed with NameError)
|
await Run_3.solve_missions()
|
||||||
elif color == "Blue":
|
elif color == "Blue":
|
||||||
print('Running Mission 4, Detect Blue')
|
print('Running Mission 4, Detect Blue')
|
||||||
pass # TODO: Run4 not written yet (it crashed with NameError)
|
await Run_4.solve_missions()
|
||||||
elif color == "Orange":
|
|
||||||
print('Running Mission 5, Detect Orange')
|
|
||||||
pass # TODO: Run5 not written yet (it crashed with NameError)
|
|
||||||
elif color == "Purple":
|
elif color == "Purple":
|
||||||
print('Running Mission 6, Detect Purple')
|
print('Running Mission 5, Detect Purple')
|
||||||
pass # TODO: Run6 not written yet (it crashed with NameError)
|
await Run_5.solve_missions()
|
||||||
elif color == "Light_Blue":
|
elif color == "Orange":
|
||||||
|
print('Running Mission 6, Detect Orange')
|
||||||
|
await Run_6.solve_missions()
|
||||||
|
elif color == "Cyan":
|
||||||
print('Running Mission 7, Detect Cyan')
|
print('Running Mission 7, Detect Cyan')
|
||||||
pass # TODO: Run7 not written yet (it crashed with NameError)
|
await Run_7.solve_missions()
|
||||||
|
elif color == "Lime":
|
||||||
|
print('Running Mission 8, Detect Lime')
|
||||||
|
await Run_8.solve_missions()
|
||||||
|
elif color == "Brown":
|
||||||
|
print('Running Mission Experimental, Detect Brown')
|
||||||
|
await robot.wait(500)
|
||||||
elif color == "White":
|
elif color == "White":
|
||||||
print('Detect White')
|
print('Detect White')
|
||||||
|
await robot.wait(500)
|
||||||
else:
|
else:
|
||||||
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
|
||||||
#pass
|
#pass
|
||||||
|
|||||||
Reference in New Issue
Block a user