94 Commits

Author SHA1 Message Date
808f797bc3 Update regionals/bioglow_mission_keystone.py 2026-11-18 01:17:53 +00:00
2a3bc07074 Update regionals/bioglow_mission_1.py 2026-11-18 01:09:33 +00:00
59ca332b2a Update regionals/bioglow_mission_keystone.py 2026-11-17 00:18:21 +00:00
a98a2186e7 Delete regionals/bioglow_missions_15_14_ant.py
El Cleanup.
2026-11-17 00:12:00 +00:00
acae133c65 Delete regionals/bioglow_missions_11_6_7.py
El Cleanup.
2026-11-17 00:11:46 +00:00
896e240935 Delete regionals/bioglow_missions_6_7_experimental.py
El Cleanup.
2026-11-17 00:11:26 +00:00
34d732a5d4 Delete regionals/bioglow_missions_6_7_13_14_experimental.py
El Cleanup.
2026-11-17 00:11:10 +00:00
befae6c40b Delete regionals/bioglow_mission_13.py
El Cleanup.
2026-11-17 00:10:28 +00:00
f4c62b0109 Delete regionals/bioglow_mission_12.py
El Cleanup.
2026-11-17 00:10:09 +00:00
93e64b3e65 Update regionals/bioglow_mission_3_4_5.py 2026-11-17 00:02:44 +00:00
64598ef144 Delete regionals/bioglow_missions_3_4_5.py
El Confusion.
2026-11-17 00:02:10 +00:00
3d023f045f Update regionals/bioglow_mission_3_4_5_experimental.py 2026-11-15 22:48:10 +00:00
384c1732cf Update regionals/bioglow_mission_2.py 2026-11-15 22:45:54 +00:00
12edbcdfd8 Update regionals/bioglow_mission_7.py 2026-11-15 22:29:27 +00:00
83dee33058 Update regionals/bioglow_mission_keystone.py 2026-11-15 22:27:41 +00:00
d41e577b35 Update regionals/bioglow_mission_3_4_5_experimental.py 2026-11-15 22:04:18 +00:00
f6f3ef600a Update regionals/bioglow_mission_2.py 2026-11-15 22:04:04 +00:00
b137f382b0 Update regionals/bioglow_mission_1.py 2026-11-15 22:03:46 +00:00
0bc86b032d Update regionals/bioglow_mission_8.py 2026-11-15 21:55:38 +00:00
ce94cea83f Update regionals/bioglow_mission_3_4_5_experimental.py 2026-11-15 21:55:17 +00:00
f4f66fe2b4 Update regionals/bioglow_mission_1.py 2026-11-15 21:54:55 +00:00
43e0e93934 Update regionals/bioglow_mission_2.py 2026-11-15 21:53:22 +00:00
71c66259ce Update regionals/bioglow_main.py
Changed color ordering
2026-11-10 13:32:02 +00:00
8092a8ec79 Update regionals/bioglow_main.py 2026-11-10 13:27:08 +00:00
48719fd6d9 Update regionals/bioglow_missions_3_4_5.py
Updates from experimental
2026-11-10 13:22:17 +00:00
299644e6f5 Update regionals/bioglow_mission_7.py 2026-11-10 13:21:15 +00:00
51a6e31170 Update regionals/bioglow_main.py
removed experimental
2026-11-10 13:20:48 +00:00
5522503b28 Update regionals/bioglow_mission_11.py
removed duplicate
2026-11-10 13:18:45 +00:00
b4fbd57aa8 Update regionals/bioglow_main.py 2026-11-10 13:12:23 +00:00
73e35a62cf Update regionals/bioglow_mission_3_4_5_experimental.py 2026-11-10 12:52:27 +00:00
bb703e626d Update regionals/bioglow_mission_7_experimental.py 2026-11-09 22:12:24 +00:00
3621a355b1 Update regionals/bioglow_mission_7_experimental.py 2026-11-09 22:11:46 +00:00
5926554f55 Add regionals/bioglow_mission_7_experimental.py.py 2026-11-09 22:11:03 +00:00
94a6b0d7f1 Update regionals/bioglow_mission_2.py 2026-11-08 17:16:13 +00:00
d345fab7f7 Update regionals/bioglow_mission_3_4_5_experimental.py 2026-11-08 17:15:36 +00:00
962eab1e03 Update regionals/bioglow_mission_8.py 2026-11-08 17:14:50 +00:00
5dd1a7cd56 Update regionals/bioglow_mission_11.py 2026-11-06 21:40:46 +00:00
37e0dd4e51 Update regionals/bioglow_mission_11.py 2026-11-06 21:31:47 +00:00
f141e9b517 Update regionals/bioglow_mission_1.py 2026-11-05 23:22:26 +00:00
a19ee01d48 Update regionals/bioglow_main.py 2026-11-05 23:22:10 +00:00
296f653670 Update regionals/bioglow_mission_8.py 2026-11-05 03:48:59 +00:00
c809f1ae9a Update regionals/bioglow_main.py 2026-11-05 03:32:02 +00:00
c0329e4f42 Update regionals/bioglow_missions_15_14_ant.py 2026-10-31 20:58:01 +00:00
e463010a45 Update regionals/robot.py 2026-10-31 20:57:16 +00:00
1e0fb5eae6 Update regionals/bioglow_mission_3_4_5_experimental.py 2026-10-31 20:56:41 +00:00
ae329f8b91 Update regionals/bioglow_mission_8.py 2026-10-31 20:56:18 +00:00
bfc3ef416e Delete .DS_Store 2026-10-31 19:37:02 +00:00
111dc0d5aa Update regionals/bioglow_mission_keystone.py 2026-10-31 19:35:31 +00:00
68ca2b744b Update regionals/bioglow_mission_keystone.py 2026-10-31 19:31:03 +00:00
4a266358b8 Update regionals/bioglow_mission_keystone.py 2026-10-31 19:28:02 +00:00
72f26eb741 Update regionals/bioglow_mission_11.py 2026-10-31 19:24:47 +00:00
718d928d78 Update regionals/bioglow_main.py 2026-10-31 19:23:41 +00:00
f1e8bdc0ea Add regionals/bioglow_mission_keystone.py 2026-10-31 19:19:22 +00:00
cea616f13e Added object oriented version of codes 2026-10-31 13:27:07 -05:00
63f7c48a64 Update hazmat_scrimmage/bioglow_missions_3_4_5.py 2026-10-30 21:44:27 +00:00
9f201d78a1 Update hazmat_scrimmage/bioglow_mission_2.py 2026-10-30 20:43:15 +00:00
795831b932 Update hazmat_scrimmage/bioglow_missions_15_14_ant 2026-10-29 15:44:31 +00:00
4a1ad1c33e Update hazmat_scrimmage/bioglow_missions_15_14_ant 2026-10-29 15:43:54 +00:00
5644ad15e9 Update hazmat_scrimmage/bioglow_helpers.py 2026-10-26 00:11:58 +00:00
3a296edc34 Update hazmat_scrimmage/bioglow_robot_settings.py 2026-10-26 00:10:48 +00:00
9e1caaabed Update hazmat_scrimmage/bioglow_missions_14_15.py 2026-10-23 23:28:00 +00:00
2b3ab21252 Update hazmat_scrimmage/bioglow_hazmat_scrimmage_main.py 2026-10-23 23:26:08 +00:00
894df75b42 Update hazmat_scrimmage/bioglow_missions_3_4_5.py 2026-10-23 21:48:04 +00:00
1735e2bc34 Add hazmat_scrimmage/bioglow_missions_6_7_13_14_experimental.py 2026-10-23 18:32:07 +00:00
d85a427371 Add hazmat_scrimmage/bioglow_missions_6_7_experimental.py 2026-10-23 18:30:34 +00:00
b81c7bab63 Update hazmat_scrimmage/bioglow_mission_8.py 2026-10-21 00:52:23 +00:00
f2edae8e3f Update hazmat_scrimmage/bioglow_mission_8.py 2026-10-21 00:02:46 +00:00
e1d1c612c9 Update hazmat_scrimmage/bioglow_hazmat_scrimmage_main.py 2026-10-20 23:16:19 +00:00
8374311800 Upload files to "hazmat_scrimmage" 2026-10-20 23:15:08 +00:00
1ddab4463e Upload files to "hazmat_scrimmage" 2026-10-20 23:14:43 +00:00
81842d5a75 Upload files to "hazmat_scrimmage" 2026-10-20 23:14:14 +00:00
c6c117daf7 Upload files to "hazmat_scrimmage" 2026-10-20 23:13:34 +00:00
29a16afbf8 Add hazmat_scrimmage/readme.md 2026-10-20 23:07:38 +00:00
88858f1d39 Add shout-fll-scrimmage/bioglow_mission_3_4_5_experimental.py
experimental
2026-10-20 22:05:29 +00:00
a11bd235a2 Add shout-fll-scrimmage/bioglow_mission_11_experimental.py
experimental for mission 11 with new attachment
2026-10-20 04:20:14 +00:00
eead961fa0 Added function to check stalled motors 2026-10-20 01:34:16 +00:00
c54feaad7d Added run until stalled 2026-10-20 01:30:38 +00:00
8d61cf2019 Update shout-fll-scrimmage/bioglow_mission_2.py 2026-10-19 04:18:09 +00:00
d65be19478 Add shout-fll-scrimmage/bioglow_mission_2.py 2026-10-17 02:43:04 +00:00
7d13418fea Add shout-fll-scrimmage/bioglow_mission_1.py 2026-10-17 02:42:16 +00:00
7bfbf49fcf Add shout-fll-scrimmage/bioglow_mission_8.py 2026-10-17 02:41:52 +00:00
1c03cd11ff Add shout-fll-scrimmage/bioglow_missions_14_15.py 2026-10-17 02:41:01 +00:00
9f4cfc193e Add shout-fll-scrimmage/bioglow_mission_12.py 2026-10-17 02:40:31 +00:00
d72cf75118 Add shout-fll-scrimmage/bioglow_mission_13.py 2026-10-17 02:40:02 +00:00
aa0949368c Add shout-fll-scrimmage/bioglow_missions_11_6_7.py 2026-10-17 02:39:33 +00:00
201de8e185 Update shout-fll-scrimmage/bioglow_shoutfll_scrimmage_main.py 2026-10-17 02:38:47 +00:00
038742e173 Update shout-fll-scrimmage/bioglow_missions_3_4_5.py 2026-10-17 01:25:02 +00:00
53517762ef Update shout-fll-scrimmage/bioglow_shoutfll_scrimmage_main.py 2026-10-17 01:22:55 +00:00
1c0099bb0a Added more comments 2026-10-12 22:36:24 -05:00
ece88cf3ad Added comments 2026-10-12 22:31:47 -05:00
8f30669980 Fixed bioglow robot settings import 2026-10-12 22:20:07 -05:00
73f96e6498 Rename all pynamics files 2026-10-12 22:14:40 -05:00
33344aafd0 restructure files for scrimmage 2026-10-12 22:11:02 -05:00
514bb91f7b cleanup folder structure 2026-10-12 11:38:27 -05:00
57 changed files with 3853 additions and 218 deletions

View File

@@ -209,140 +209,9 @@ async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Parthiv
async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Johannes
>>>>>>> 3cd1849 (Update discovery-phase/missions/Showcase_codes.py)
<<<<<<< HEAD
<<<<<<< HEAD
<<<<<<< HEAD
await set_speed(600,200,300,200)
await drive_base.straight(500)
await multitask(
drive_base.straight(-200),
right_arm.run_angle(400,-150)
)
await set_speed(600,500,100,200)
await drive_base.turn(45)
await drive_base.straight(346)
await drive_base.turn(-68)
await multitask(
left_arm.run_angle(600,240),
set_speed(300,200,300,200),
drive_base.straight(280)
)
await left_arm.run_angle(600,-100)
await drive_base.turn(22)
await drive_base.straight(-100)
await right_arm.run_angle(5000,45)
await set_speed(600,500,100,200)
await drive_base.straight(200)
await drive_base.arc(-1500,None,-1000)
async def Run2(): # Exploding Seeds - Johannes
await multitask(
right_arm.run_angle(400,-100),
left_arm.run_angle(300,100),
drive_base.arc(150,None,115)
)
await drive_base.straight(270)
await left_arm.run_angle(300,-100)
await drive_base.straight(-100)
await left_arm.run_angle(300,-100)
await drive_base.arc(-1000,None,-800)
async def Run2_1(): # Drone Survey, Experimental - Johannes
await multitask(
right_arm.run_angle(400,100),
set_speed(600,200,300,200),
drive_base.straight(300)
)
set_speed(600,500,300,200)
await drive_base.straight(470)
await drive_base.turn(-15)
await drive_base.straight(55)
await drive_base.straight(-280)
await drive_base.turn(15)
await right_arm.run_angle(400,-100)
await drive_base.straight(40)
await drive_base.turn(-15)
await drive_base.straight(50)
await drive_base.turn(-25)
async def Run3(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimental - Johannes
await drive_base.straight(98)
await drive_base.turn(-45)
await drive_base.straight(300)
await right_arm.run_angle(300,120)
await drive_base.straight(290)
await drive_base.turn(50)
await drive_base.straight(250)
await drive_base.turn(40)
set_speed(100,200,300,200)
<<<<<<< HEAD
await drive_base.straight(170)
<<<<<<< HEAD
await drive_base.straight(-145)
await drive_base.turn(80)
await drive_base.straight(230)
set_speed(600, 500, 300, 200)
await drive_base.straight(-138)
=======
await drive_base.straight(-135)
await drive_base.turn(80)
await drive_base.straight(230)
set_speed(600, 500, 300, 200)
await drive_base.straight(-145)
>>>>>>> 0e1e580 (Update discovery-phase/missions/Showcase_codes.py)
=======
await drive_base.straight(177)
await drive_base.straight(-145)
await drive_base.turn(85)
await drive_base.straight(232)
set_speed(600, 500, 300, 200)
await drive_base.straight(-138)
>>>>>>> 788745f (Update discovery-phase/missions/Showcase_codes.py)
await multitask(
drive_base.turn(-36),
right_arm.run_angle(200,-140)
)
set_speed(100,200,300,200)
await drive_base.straight(230)
<<<<<<< HEAD
<<<<<<< HEAD
await right_arm.run_angle(300,105)
=======
await right_arm.run_angle(300,160)
>>>>>>> 0e1e580 (Update discovery-phase/missions/Showcase_codes.py)
=======
await right_arm.run_angle(300,105)
>>>>>>> 788745f (Update discovery-phase/missions/Showcase_codes.py)
await wait(200)
await drive_base.turn(-90)
await drive_base.straight(-170)
set_speed(600,500,300,200)
await drive_base.straight(100)
await drive_base.arc(800,None,-900)
async def Run4(): #Flip the Rock, Lucky Leaves, Reaching Roots - Rishabh
#Run
=======
>>>>>>> c15d8f0 (Update discovery-phase/missions/Showcase_codes.py)
await drive_base.straight(550)#Works (600(Previous))
await drive_base.straight(-200)
=======
await drive_base.straight(415)
await drive_base.straight(-150)
>>>>>>> a9d3cd9 (Update discovery-phase/missions/Showcase_codes.py)
=======
await drive_base.straight(415)
await drive_base.straight(-150)
>>>>>>> dev
await drive_base.turn(45)
await drive_base.straight(600)
await drive_base.turn(-90)
@@ -360,12 +229,6 @@ async def Run4(): #Flip the Rock, Lucky Leaves, Reaching Roots - Rishabh
set_speed(700,500,300,200)
await drive_base.arc(-700,angle=-150)
<<<<<<< HEAD
<<<<<<< HEAD
async def Run11(): # Interchangable Mission - Parthiv
=======
=======
>>>>>>> dev
async def Run2(): # Exploding Seeds - Johannes
await multitask(
@@ -404,10 +267,6 @@ async def Run4(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimen
await drive_base.straight(110)
await drive_base.turn(-45)
<<<<<<< HEAD
<<<<<<< HEAD
<<<<<<< HEAD
=======
>>>>>>> dev
await drive_base.straight(280)
=======
await drive_base.straight(300)
@@ -434,30 +293,11 @@ async def Run4(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimen
await drive_base.turn(40)
set_speed(100,200,300,200)
await drive_base.straight(170)
<<<<<<< HEAD
=======
await drive_base.straight(300)
await right_arm.run_angle(300,120)
await drive_base.straight(290)
await drive_base.turn(50)
await drive_base.straight(250)
await drive_base.turn(40)
set_speed(100,200,300,200)
await drive_base.straight(170)
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
=======
>>>>>>> dev
await drive_base.straight(-145)
await drive_base.turn(80)
await drive_base.straight(230)
set_speed(600, 500, 300, 200)
await drive_base.straight(-138)
<<<<<<< HEAD
<<<<<<< HEAD
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
=======
=======
>>>>>>> dev
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
await multitask(
drive_base.turn(-36),
@@ -465,10 +305,6 @@ async def Run4(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimen
)
set_speed(100,200,300,200)
<<<<<<< HEAD
<<<<<<< HEAD
<<<<<<< HEAD
=======
>>>>>>> dev
await drive_base.straight(235)
await multitask(
right_arm.run_angle(300,100),
@@ -481,14 +317,6 @@ async def Run4(): # Window to the Past, Leaf Cutter, Humongous Fungus, Experimen
await right_arm.run_angle(300,105)
await wait(200)
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
<<<<<<< HEAD
=======
await drive_base.straight(230)
await right_arm.run_angle(300,105)
await wait(200)
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
=======
>>>>>>> dev
await drive_base.turn(-90)
await drive_base.straight(-170)
set_speed(600,500,300,200)
@@ -542,24 +370,10 @@ async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Parthiv
>>>>>>> 3cd1849 (Update discovery-phase/missions/Showcase_codes.py)
<<<<<<< HEAD
<<<<<<< HEAD
<<<<<<< HEAD
async def Run6(): # Interchangable Mission - Parthiv
<<<<<<< HEAD
>>>>>>> c15d8f0 (Update discovery-phase/missions/Showcase_codes.py)
=======
=======
=======
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
async def Run11(): # Interchangable Mission - Parthiv
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
>>>>>>> 0cd478f (resolving)
=======
async def Run6(): # Interchangable Mission - Parthiv
=======
async def Run11(): # Interchangable Mission - Parthiv
>>>>>>> 2b44156 (Update discovery-phase/missions/Showcase_codes.py)
>>>>>>> dev
await left_arm.run_angle(200,-80)
await drive_base.arc(-350,angle=90)

View 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)

View 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())

View 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

View 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'.")

View 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)

View 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()

View 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())

View 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)
)

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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)

View File

@@ -0,0 +1 @@
# this is is our code for hazmat scrimmage

134
regionals/arm.py Normal file
View 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()

View 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())

View 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
View 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())

View 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())

View 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())

View 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

View 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())

View 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())

View 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())

View 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
View 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
View 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),
)

View 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)

View 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())

View 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

View 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'.")

View 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)

View 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()

View File

@@ -0,0 +1,213 @@
########################################################
# 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(600, 500, 300, 200)
# Use this to change drive base movement
def set_speed(straight_speed, st_acc, turn_speed, turn_acc):
drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc)
async def multilift_up(rotation_angle):
await multitask(
right_arm.run_angle(400,rotation_angle),
left_arm.run_angle(400,-1*rotation_angle)
)
async def multilift_down(rotation_angle):
await multitask(
right_arm.run_angle(1000,-1*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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View 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())

View File

@@ -0,0 +1,30 @@
#####################################
# 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 - Parthiv
await drive_base.straight(415)
await drive_base.straight(-150)
await drive_base.turn(45)
await drive_base.straight(600)
await drive_base.turn(-90)
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 left_arm.run_angle(200,-300)
await drive_base.straight(-95)
await drive_base.turn(22)
await right_arm.run_angle(5000,100)
await drive_base.straight(90)
set_speed(700,500,300,200)
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())

View File

@@ -1,42 +1,33 @@
#SETTING UP
import umath
####################################################
# 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
# Initialize hub and devices
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36],[12,20,24]]) # FIXED: both gear trains in ONE list
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
lazer_ranger = UltrasonicSensor(Port.E)
color_sensor = ColorSensor(Port.F)
# DriveBase configuration
WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels)
AXLE_TRACK = 180 # mm (distance between wheels)
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, 450, 300, 200)
drive_base.settings(600, 500, 300, 200)
drive_base.use_gyro(True)
#Run
drive_base.straight(550)#Works (600(Previous))
drive_base.straight(-200)
drive_base.turn(45)
drive_base.straight(575)
drive_base.turn(-90)#-90 for Coach Cisco table
left_arm.run_angle(200,270)
drive_base.straight(300)
left_arm.run_angle(200,-265)
right_arm.run_angle(200, -200)
drive_base.straight(-105)#Use -105 for Coach Cisco table(-105 for school)
drive_base.turn(32)#was 24-Use 24 for Coach Cisco's table(32 for school)
right_arm.run_angle(1500, 180)
drive_base.turn(30)
drive_base.straight(-1000)

View 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_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
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())

View File

@@ -1,6 +0,0 @@
'''
This is where you write the main code to start all mission codes
Combine all call here
'''

View File

@@ -1 +0,0 @@
#Write Code here for your missions