Compare commits
40 Commits
dev
...
466657022a
| Author | SHA1 | Date | |
|---|---|---|---|
| 466657022a | |||
| f4cfa3e8f4 | |||
| 0c3b2e1053 | |||
| cc1643f9e8 | |||
| 47a5a8445b | |||
| e53e367751 | |||
| 13a24e17ad | |||
| bda38fab1c | |||
| 275c73382c | |||
| 72ffdf2172 | |||
| 1a94799e5a | |||
| 99080ce07e | |||
| 75013f1039 | |||
| f2a0d940e0 | |||
| 656591ee43 | |||
| 2b44156e6c | |||
| 635f79db04 | |||
| 58d9160a2c | |||
| 97e67ce81d | |||
| 8399fae12b | |||
| 6480331d5e | |||
| 8b0258273b | |||
| c2a1f0abb3 | |||
| 143380f0ec | |||
| 0f87f0e49a | |||
| 1cae9c258f | |||
| 9b262cc39d | |||
| fdbe815d51 | |||
| f79ac34715 | |||
| 03adeb476d | |||
| cf95327d1a | |||
| 2d197652a6 | |||
| 66acbde5df | |||
| 9021a0bf09 | |||
| 2b7088edaa | |||
| 9788fae8cd | |||
| ea09f1fdc2 | |||
| 81bd650958 | |||
| 4ab24a47c2 | |||
| bcf6b0524c |
@@ -1,33 +1,42 @@
|
||||
####################################################
|
||||
# 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.
|
||||
|
||||
#SETTING UP
|
||||
import umath
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.tools import run_task, multitask
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.hubs import PrimeHub
|
||||
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]]) # FIXED: both gear trains in ONE list
|
||||
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
|
||||
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
|
||||
lazer_ranger = UltrasonicSensor(Port.E)
|
||||
color_sensor = ColorSensor(Port.F)
|
||||
|
||||
# DriveBase configuration
|
||||
WHEEL_DIAMETER = 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.
|
||||
WHEEL_DIAMETER = 68.8 # mm (adjust for your wheels)
|
||||
AXLE_TRACK = 180 # mm (distance between wheels)
|
||||
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||
drive_base.settings(600, 500, 300, 200)
|
||||
drive_base.settings(600, 450, 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)
|
||||
|
||||
|
||||
@@ -209,9 +209,140 @@ 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)
|
||||
@@ -229,6 +360,12 @@ async def Run1(): # Flip The Rock, Lucky Leaves, Reaching Roots - Johannes
|
||||
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(
|
||||
@@ -267,6 +404,10 @@ 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)
|
||||
@@ -293,11 +434,30 @@ 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),
|
||||
@@ -305,6 +465,10 @@ 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),
|
||||
@@ -317,6 +481,14 @@ 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)
|
||||
@@ -370,10 +542,24 @@ 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)
|
||||
|
||||
@@ -1,49 +0,0 @@
|
||||
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)
|
||||
@@ -1,48 +0,0 @@
|
||||
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())
|
||||
@@ -1,165 +0,0 @@
|
||||
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
|
||||
@@ -1,74 +0,0 @@
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.tools import wait, StopWatch
|
||||
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'.")
|
||||
@@ -1,35 +0,0 @@
|
||||
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)
|
||||
@@ -1,136 +0,0 @@
|
||||
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()
|
||||
@@ -1,111 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,178 +0,0 @@
|
||||
########################################################
|
||||
# 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)
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -1,31 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,36 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,29 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,29 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,48 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,37 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,26 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,51 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,51 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,29 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,73 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,58 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,33 +0,0 @@
|
||||
####################################################
|
||||
# bioglow_robot_settings.py
|
||||
# This file contains all the common robot settings
|
||||
# Only change this when necessary
|
||||
#####################################################
|
||||
|
||||
#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge.
|
||||
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.tools import run_task, multitask
|
||||
from pybricks.tools import wait, StopWatch
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.hubs import PrimeHub
|
||||
import umath
|
||||
|
||||
# Initialize hub and devices
|
||||
hub = PrimeHub()
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
|
||||
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[20,28],[12,20]]) # FIXED: both gear trains in ONE list
|
||||
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[20,28],[12,20]]) #Added gear train list for gear ration
|
||||
lazer_ranger = UltrasonicSensor(Port.E)
|
||||
color_sensor = ColorSensor(Port.F)
|
||||
|
||||
# DriveBase configuration
|
||||
WHEEL_DIAMETER = 62.4 # mm (adjust for your wheels)
|
||||
AXLE_TRACK = 150 # mm (distance between wheels)
|
||||
|
||||
# The ONE drive base. Every other file imports it from here — never make another.
|
||||
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||
drive_base.settings(600, 500, 300, 200)
|
||||
drive_base.use_gyro(True)
|
||||
@@ -1 +0,0 @@
|
||||
# this is is our code for hazmat scrimmage
|
||||
134
regionals/arm.py
134
regionals/arm.py
@@ -1,134 +0,0 @@
|
||||
"""
|
||||
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()
|
||||
@@ -1,30 +0,0 @@
|
||||
"""
|
||||
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())
|
||||
@@ -1,165 +0,0 @@
|
||||
########################################################
|
||||
# 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=...).
|
||||
@@ -1,122 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,46 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,63 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
|
||||
@@ -1,80 +0,0 @@
|
||||
#####################################
|
||||
# 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
|
||||
@@ -1,69 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,92 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,42 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,54 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,317 +0,0 @@
|
||||
"""
|
||||
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()
|
||||
@@ -1,116 +0,0 @@
|
||||
"""
|
||||
robot.py — everything about OUR robot, in one place.
|
||||
(Replaces bioglow_robot_settings.py.)
|
||||
|
||||
Every mission starts with:
|
||||
import robot
|
||||
|
||||
Then:
|
||||
await robot.move(55) # forward 55 cm
|
||||
await robot.turn(-90) # left 90 degrees
|
||||
await robot.arc(-35, degrees=90) # curve left
|
||||
await robot.left_arm.move_by(100, speed=47) # arms (see arm.py)
|
||||
robot.color_sensor, robot.lazer_ranger, robot.hub
|
||||
|
||||
RULE: motors and sensors are created ONLY in this file.
|
||||
Creating Motor(Port.A) anywhere else gives a "port already in use" error.
|
||||
|
||||
Important: test all code with the battery at 100%, and make all updates
|
||||
with the robot at full charge.
|
||||
"""
|
||||
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.tools import run_task, multitask, wait, StopWatch
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.hubs import PrimeHub
|
||||
import umath
|
||||
|
||||
# ---------------------------------------------------------------- hub
|
||||
hub = PrimeHub()
|
||||
|
||||
# ---------------------------------------------------------------- wheels
|
||||
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
|
||||
right_motor = Motor(Port.B, Direction.CLOCKWISE)
|
||||
|
||||
WHEEL_DIAMETER = 62.4 # mm (measure your wheels)
|
||||
AXLE_TRACK = 150 # mm (distance between the two wheels)
|
||||
|
||||
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
|
||||
drive_base.use_gyro(True)
|
||||
|
||||
# ---------------------------------------------------------------- arms
|
||||
# Gear trains: both trains go in ONE list. 2.33:1 reduction, so top speed is ~430 deg/s.
|
||||
left_arm_motor = Motor(Port.C, Direction.CLOCKWISE, [[20, 28], [12, 20]])
|
||||
right_arm_motor = Motor(Port.D, Direction.CLOCKWISE, [[20, 28], [12, 20]])
|
||||
|
||||
# Each arm gets % speeds, named positions and homing (see arm.py).
|
||||
# Before using home() or move_to(), on the robot:
|
||||
# 1. set home_backward: which way each arm turns to reach its hard stop
|
||||
# 2. add positions, measured from that stop (run arm_calibrate.py),
|
||||
# for example positions={"down": 0, "up": 120}
|
||||
from arm import Arm
|
||||
left_arm = Arm(left_arm_motor, speed=50, positions={}, home_backward=True)
|
||||
right_arm = Arm(right_arm_motor, speed=50, positions={}, home_backward=False)
|
||||
|
||||
# ---------------------------------------------------------------- sensors
|
||||
lazer_ranger = UltrasonicSensor(Port.E) # mounted at the BACK of the robot
|
||||
color_sensor = ColorSensor(Port.F)
|
||||
|
||||
# ---------------------------------------------------------------- moves
|
||||
# movement.py does the work, so robot.move(55) means "drive 55 cm".
|
||||
#
|
||||
# NORMAL SPEEDS AND ACCELERATIONS. Used whenever a move doesn't give its own:
|
||||
# await robot.move(30) -> normal values
|
||||
# await robot.move(30, speed=40, acceleration=200) -> just for this move
|
||||
# Speeds are % of top speed: 100% straight = about 544 mm/s, 100% turning = about 416 deg/s.
|
||||
DEFAULT_SPEED = 80 # % for move() and arc() (about 435 mm/s)
|
||||
DEFAULT_TURN_SPEED = 72 # % for turn() (about 300 deg/s)
|
||||
DEFAULT_ACCELERATION = 500 # mm/s per second, move() and arc()
|
||||
DEFAULT_TURN_ACCELERATION = 200 # deg/s per second, turn()
|
||||
|
||||
import movement
|
||||
movement.initialize(drive_base, speed=DEFAULT_SPEED, turn_speed=DEFAULT_TURN_SPEED,
|
||||
acceleration=DEFAULT_ACCELERATION,
|
||||
turn_acceleration=DEFAULT_TURN_ACCELERATION)
|
||||
from movement import (move, turn, arc, move_for_seconds, move_until_stalled,
|
||||
drive, stop, brake, reset, distance_cm, angle,
|
||||
set_default_speed, reset_settings)
|
||||
|
||||
|
||||
async def settle_gyro(ms=250, max_seconds=2):
|
||||
"""Let the gyro settle before a run starts.
|
||||
|
||||
Waits ms milliseconds (default half a second), then keeps waiting until the
|
||||
hub says it's perfectly still, so the first turn starts from a steady heading.
|
||||
Gives up after max_seconds, so a run never gets stuck here.
|
||||
"""
|
||||
await wait(ms)
|
||||
timer = StopWatch()
|
||||
while timer.time() < max_seconds * 1000:
|
||||
still = hub.imu.stationary()
|
||||
ready = hub.imu.ready() if hasattr(hub.imu, "ready") else True
|
||||
if still and ready:
|
||||
return
|
||||
await wait(10)
|
||||
|
||||
|
||||
async def home_arms():
|
||||
"""Find both arms' starting positions. Check home_backward first!"""
|
||||
await multitask(left_arm.home(), right_arm.home())
|
||||
|
||||
|
||||
async def lift_up(degrees, speed=None):
|
||||
"""Raise both arms together (they face opposite ways). speed is in %."""
|
||||
await multitask(
|
||||
right_arm.move_by(degrees, speed),
|
||||
left_arm.move_by(-degrees, speed),
|
||||
)
|
||||
|
||||
|
||||
async def lift_down(degrees, speed=None):
|
||||
"""Lower both arms together. speed is in %."""
|
||||
await multitask(
|
||||
right_arm.move_by(-degrees, speed),
|
||||
left_arm.move_by(degrees, speed),
|
||||
)
|
||||
@@ -1,49 +0,0 @@
|
||||
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)
|
||||
@@ -1,48 +0,0 @@
|
||||
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())
|
||||
@@ -1,165 +0,0 @@
|
||||
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
|
||||
@@ -1,74 +0,0 @@
|
||||
from pybricks.hubs import PrimeHub
|
||||
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
|
||||
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
|
||||
from pybricks.robotics import DriveBase
|
||||
from pybricks.tools import wait, StopWatch
|
||||
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'.")
|
||||
@@ -1,35 +0,0 @@
|
||||
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)
|
||||
@@ -1,136 +0,0 @@
|
||||
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()
|
||||
@@ -1,213 +0,0 @@
|
||||
########################################################
|
||||
# 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())
|
||||
|
||||
@@ -1,31 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,36 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,29 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,29 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,58 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,37 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,22 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,51 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,25 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,30 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
@@ -1,111 +0,0 @@
|
||||
#####################################
|
||||
# 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())
|
||||
6
shout-fll-scrimmage/main.py
Normal file
6
shout-fll-scrimmage/main.py
Normal file
@@ -0,0 +1,6 @@
|
||||
'''
|
||||
|
||||
This is where you write the main code to start all mission codes
|
||||
Combine all call here
|
||||
|
||||
'''
|
||||
1
shout-fll-scrimmage/missions/mission_1_2_3.py
Normal file
1
shout-fll-scrimmage/missions/mission_1_2_3.py
Normal file
@@ -0,0 +1 @@
|
||||
#Write Code here for your missions
|
||||
Reference in New Issue
Block a user