Arcmyx/pynamics
2
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

Merge pull request 'dev' (#12) from dev into main

Browse Source 
Reviewed-on: #12
This commit is contained in:
Atharv Nagavarapu
2025-12-19 22:45:12 +00:00
parent 60384d4367 78e31482d0
commit 98a1e9c466
8 changed files with 331 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
README.md
View File

@@ -13,6 +13,7 @@ Included utilities:
- Diagnostics - a program that allows you to diagnose issues and test parts of your robot, such as battery, motor, and color sensor. Open each program in the ```diagnostics``` folder in Pybricks, connect your robot, switch to the ```FullDiagnostics.py``` file and press run.
- Color Sensor Tests (UPCOMING) - a program that identifies what color the sensor is detecting. If you'd like, you can use our color ranges in your own programs.
Please set your window size to 90% on small screens for best results with the ASCII art.
This code is licensed under the Creative Commons Attribution 4.0 International License (CC BY 4.0).
Without the confusing legal speak, this means that you are free to:

28
diagnostics/BatteryDiagnostics.py
View File

@@ -1,24 +1,18 @@
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
from pybricks.tools import wait
import umath
# Initialize hub and devices
hub = PrimeHub()
class BatteryDiagnostics:
def __init__(self):
def __init__(self, hub):
self.voltage = 0
self.current = 0
self.hub = hub
def printVoltage(self):
self.voltage = hub.battery.voltage()
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 = hub.battery.current()
self.current = self.hub.battery.current()
print("Battery current:", self.current)
def printAll(self):
timeelapsed = 0
@@ -39,17 +33,17 @@ class BatteryDiagnostics:
print("Voltage deviation:", self.stdev(voltageList))
print("Current deviation:", self.stdev(currentList))
def stdev(self, vals):
data = vals
if len(data) < 2:
DATA = vals
if len(DATA) < 2:
return 0
# Calculate the mean
mean = sum(data) / len(data)
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)
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)
STD_DEV_MANUAL = umath.sqrt(VARIANCE)
return (std_dev_manual)
return (STD_DEV_MANUAL)

48
diagnostics/ColorSensorDiagnostics.py Normal file
View File

@@ -0,0 +1,48 @@
from pybricks.parameters import Color, Port, Stop
from pybricks.tools import wait, StopWatch
class ColorSensorDiagnostics:
def __init__(self, hub, colorsensorclass):
self.colorsensor = None
self.colorsensorclass = colorsensorclass
self.PORT_MAP = {
"A": Port.A,
"B": Port.B,
"C": Port.C,
"D": Port.D,
"E": Port.E,
"F": Port.F,
}
def initializeColorSensor(self):
VALID_PORTS = {"A", "B", "C", "D", "E", "F"}
while True:
colorinput = input(
"This will test your color sensor.\n"
"Enter the port for the color sensor you would like to test (A, B, C, D, E, or F): "
).strip().upper()
if colorinput not in VALID_PORTS:
print("Invalid port. Please enter A-F.")
continue
try:
if self.colorsensor is None:
self.colorsensor = self.colorsensorclass(self.PORT_MAP[colorinput])
print(f"Color Sensor initialized on port {colorinput}.")
else:
print(f"Reusing existing color sensor on port {colorinput}.")
break
except OSError as e:
if e.errno == 16: # EBUSY
print(f"Port {colorinput} is already in use. Reusing existing color sensor.")
break
else:
print(f"Error initializing color sensor on port {colorinput}: {e}")
print("Make sure a color sensor is actually connected to this port.")
self.colorsensor = None
self.colorsensor.detectable_colors([Color.RED, Color.YELLOW, Color.GREEN, Color.BLUE, Color.WHITE, Color.NONE])
def printAll(self):
self.initializeColorSensor()
stopwatch = StopWatch()
while stopwatch.time() < 5000:
print("HSV output:", self.colorsensor.hsv())
print("Detected color:", self.colorsensor.color())

165
diagnostics/DriveBaseDiagnostics.py Normal file
View File

@@ -0,0 +1,165 @@
from pybricks.parameters import Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
from usys import stdin
from uselect import poll
class DriveBaseDiagnostics:
def __init__(self, hub, motorclass, dbclass):
self.hub = hub
self.motorclass = motorclass
self.dbclass = dbclass
self.drive_base = None
self.PORT_MAP = {
"A": Port.A,
"B": Port.B,
"C": Port.C,
"D": Port.D,
"E": Port.E,
"F": Port.F,
}
def initializeDriveBase(self):
print("DriveBase setup:")
print("1 = Load from savefile (paste JSON)")
print("2 = Use defaults")
print("3 = Enter values manually")
choice = input("Choose an option: ")
# Default values
WHEEL_DIAMETER = 68.8
AXLE_TRACK = 180
DRIVE_SETTINGS = (600, 2000, 300, 2000)
# Motor ports (None until set)
leftmotorport = Port.A
rightmotorport = Port.B
# -----------------------------
# OPTION 1: LOAD SAVEFILE
# -----------------------------
if choice == "1":
print("Paste JSON:")
raw = input("> ")
# --- wheel ---
if "\"wheel\"" in raw:
part = raw.split("\"wheel\"")[1]
part = part.split(":")[1]
part = part.split(",")[0]
WHEEL_DIAMETER = float(part)
# --- axle ---
if "\"axle\"" in raw:
part = raw.split("\"axle\"")[1]
part = part.split(":")[1]
part = part.split(",")[0]
AXLE_TRACK = float(part)
# --- settings ---
if "\"settings\"" in raw:
part = raw.split("\"settings\"")[1]
part = part.split("[")[1]
part = part.split("]")[0]
nums = part.split(",")
DRIVE_SETTINGS = (int(nums[0]), int(nums[1]), int(nums[2]), int(nums[3]))
# --- left motor port ---
if "\"left_port\"" in raw:
part = raw.split("\"left_port\"")[1]
part = part.split("\"")[1] # first quoted value
leftmotorport = part
# --- right motor port ---
if "\"right_port\"" in raw:
part = raw.split("\"right_port\"")[1]
part = part.split("\"")[1]
rightmotorport = part
print("Loaded config.")
# -----------------------------
# OPTION 3: MANUAL ENTRY
# -----------------------------
elif choice == "3":
WHEEL_DIAMETER = float(input("Wheel diameter: "))
AXLE_TRACK = float(input("Axle track: "))
print("Enter drive settings:")
s1 = int(input("Straight speed: "))
s2 = int(input("Straight accel: "))
s3 = int(input("Turn rate: "))
s4 = int(input("Turn accel: "))
DRIVE_SETTINGS = (s1, s2, s3, s4)
# Ask for motor ports HERE (manual only)
leftmotorport = input("Left motor port: ")
rightmotorport = input("Right motor port: ")
# -----------------------------
# OPTION 2: DEFAULTS
# -----------------------------
else:
pass
# -----------------------------
# CREATE MOTORS
# -----------------------------
left_motor = self.motorclass(leftmotorport, Direction.COUNTERCLOCKWISE)
right_motor = self.motorclass(rightmotorport, Direction.CLOCKWISE)
# -----------------------------
# CREATE DRIVEBASE
# -----------------------------
self.drive_base = self.dbclass(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
self.drive_base.settings(*DRIVE_SETTINGS)
self.drive_base.use_gyro(True)
print("DriveBase initialized.")
return DRIVE_SETTINGS
def printAll(self):
self.driveRobot()
def driveRobot(self):
drivesettings = self.initializeDriveBase()
print(drivesettings)
keyboard = poll()
keyboard.register(stdin)
last_key_time = StopWatch()
last_key_time.reset()
while True:
key = None
# Check for keypress immediately
if keyboard.poll(0):
key = stdin.read(1).upper()
last_key_time.reset()
# Process key
if key:
if key == "W":
self.drive_base.drive(drivesettings[0], 0)
elif key == "A":
self.drive_base.drive(drivesettings[0], -180)
elif key == "S":
self.drive_base.drive(-drivesettings[0], 0)
elif key == "D":
self.drive_base.drive(drivesettings[0], 180)
elif key == "X":
break
# Auto-stop if no key for 120 ms
if last_key_time.time() > 120:
self.drive_base.stop()
# Tiny manual delay to avoid 100% CPU
# (much faster than wait())
for _ in range(200):
pass

46
diagnostics/FullDiagnostics.py
View File

@@ -3,13 +3,19 @@ from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSenso
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
hub = PrimeHub()
HUB = PrimeHub()
from BatteryDiagnostics import BatteryDiagnostics
from MotorDiagnostics import MotorDiagnostics
battery = BatteryDiagnostics()
motor = MotorDiagnostics()
clearConfirmation = input("Do you want to clear the console before proceeding? Y/N (default: yes): ")
if(clearConfirmation == "Y" or clearConfirmation == "y" or clearConfirmation == "yes" or clearConfirmation == ""):
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("""
███████████ █████ █████ ██████ █████ █████████ ██████ ██████ █████ █████████ █████████
@@ -22,21 +28,25 @@ 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.
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("\nWhat diagnostic do you want to perform?")
print("Enter 'b' for Battery diagnostics")
print("Enter 'm' for Motor diagnostics")
print("Enter 'q' to Quit")
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 the voltage and current over a period of 3 seconds and provide average values and deviation values. Your voltage should be above 7800 mV for optimal performance.")
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.")
@@ -45,10 +55,20 @@ while True:
print("------------------------MOTOR DIAGNOSTICS------------------------")
motor.fullTest()
print("Motor diagnostics completed.")
elif choice == "h":
print("-------------------------HUB DIAGNOSTICS-------------------------")
hubdiags.printAll()
print("Hub diagnostics completed.")
elif choice == "q":
print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!")
break
elif choice == "db":
print("----------------------DRIVEBASE DIAGNOSTICS----------------------")
drivebase.printAll()
elif choice == "cs":
print("---------------------COLOR SENSOR DIAGNOSTICS---------------------")
colorsensor.printAll()
print("Color sensor diagnostics completed.")
else:
print("Invalid choice. Please enter 'b', 'm', or 'q'.")

35
diagnostics/HubDiagnostics.py Normal file
View File

@@ -0,0 +1,35 @@
from pybricks.tools import wait, StopWatch
from pybricks.parameters import Port
from pybricks import version
import OtherFunctions as debug
import usys
class HubDiagnostics:
def __init__(self, hub):
self.hub = hub
self.port_map = {
"A": Port.A,
"B": Port.B,
"C": Port.C,
"D": Port.D,
"E": Port.E,
"F": Port.F,
}
def printAbout(self):
print("Pybricks version information:", version)
print("MicroPython information:", usys.implementation)
print("MicroPython version:", usys.version)
def testLightSources(self, verbose):
v = verbose
self.hub.display.off()
for x in range(5):
for y in range(5):
debug.log(f"Turning on pixel at position {x}, {y}...", v)
self.hub.display.pixel(x, y, brightness=100)
wait(100)
debug.log(f"Turning off pixel at position {x}, {y}...", v)
self.hub.display.pixel(x, y, brightness=0)
def printAll(self):
self.printAbout()
self.testLightSources(False)

76
diagnostics/MotorDiagnostics.py
View File

@@ -1,14 +1,10 @@
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.parameters import Direction, Port, Stop
from pybricks.tools import wait, StopWatch
import umath
hub = PrimeHub()
class MotorDiagnostics:
def __init__(self):
def __init__(self, hub, motorclass):
self.testmotor = None
self.motorclass = motorclass
self.port_map = {
"A": Port.A,
"B": Port.B,
@@ -19,44 +15,40 @@ class MotorDiagnostics:
}
def stdev(self, vals):
data = vals
if len(data) < 2:
DATA = vals
if len(DATA) < 2:
return 0
# Calculate the mean
mean = sum(data) / len(data)
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)
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)
STD_DEV_MANUAL = umath.sqrt(VARIANCE)
return (std_dev_manual)
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)
ACCURACY = max(0, 100 - abs(avg_speed - desired) / desired * 100)
# Stability: penalize deviation relative to desired
stability = max(0, 100 - (stdev_speed / desired) * 100)
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)
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)
LOAD_SCORE = max(0, 100 - LOAD_PCT)
# Final score: average of the three
return (accuracy + stability + load_score) / 3
return (ACCURACY + STABILITY + LOAD_SCORE) / 3
# Final score: average of the three
return (accuracy + stability + load_score) / 3
def initializeMotor(self):
valid_ports = {"A", "B", "C", "D", "E", "F"}
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"
@@ -64,11 +56,13 @@ class MotorDiagnostics:
"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 = Motor(self.port_map[motorinput])
self.testmotor = self.motorclass(self.port_map[motorinput])
print(f"Motor initialized on port {motorinput}.")
else:
print(f"Reusing existing motor on port {motorinput}.")
@@ -89,16 +83,16 @@ class MotorDiagnostics:
motorspeeds = []
motorloads = []
target_angle = speed * 3
TARGET_ANGLE = speed * 3
print("\n", speed, "DEGREES PER SECOND TEST")
self.testmotor.run_angle(speed, target_angle, Stop.HOLD, False)
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()
MAX_SPEED, MAX_ACCEL, MAX_TORQUE = self.testmotor.control.limits()
print("Desired motor speed: ", str(speed))
@@ -117,25 +111,25 @@ class MotorDiagnostics:
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
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)
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 < 80:
if final < 65:
print("Your motor is in need of attention. Make sure to clean it regularly and charge the Prime Hub.")
elif final < 90:
elif final < 85:
print("Your motor is in OK condition. Make sure to clean it regularly and charge the Prime Hub.")
elif final < 97:
elif final < 95:
print("Your motor is in great condition!")
else:
print("Your motor is in AMAZING condition!!!")

3
diagnostics/OtherFunctions.py Normal file
View File

@@ -0,0 +1,3 @@
def log(string, verbose):
if(verbose):
print("[LOG (verbose)]", string)