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

Compare commits

base: Arcmyx:dev
Branches Tags
Arcmyx:dev Arcmyx:main Arcmyx:arcmyx-dev
Arcmyx:1.0.0
..
compare: Arcmyx:f3460290e5d3a67b378609acc13303be6c4765e7
Branches Tags
Arcmyx:main Arcmyx:dev Arcmyx:arcmyx-dev
Arcmyx:1.0.0

2 Commits
dev ... f3460290e5

Author SHA1 Message Date
Atharv Nagavarapu
f3460290e5 Merge pull request 'Update utils/BatteryDiagnostics.py' (#3) from dev into main 
Reviewed-on: Arcmyx/pybricks-utils#3
 2025-12-07 21:49:05 +00:00
Atharv Nagavarapu
345da15b22 Merge pull request 'dev' (#2) from dev into main 
Reviewed-on: Arcmyx/pybricks-utils#2
 2025-12-07 21:31:02 +00:00
21 changed files with 126 additions and 1948 deletions
Expand all files Collapse all files

19
README.md
View File

@@ -1,22 +1,11 @@
# Team 65266 Lego Dynamics - PYNAMICS - Pybricks Utilities
# Team 65266 Lego Dynamics - Pybricks Utils
A collection of Pybricks utilities to assist in your FLL robot programming with Python. Created by FLL team 65266, Lego Dynamics.
<img src="https://codes.fll-65266.org/Arcmyx/pynamics-pybricks-utils/raw/branch/main/pynamics-screenshot.png" alt="Pynamics screenshot" width="670">
How to use this:
- Download the repository by clicking on the "Code" tab, clicking the "< > Code" button, then downloading as a ZIP. Additionally, you can also use ```git clone https://codes.fll-65266.org/Arcmyx/pynamics.git```. Unzip the archive and open code.pybricks.com. Then choose which folder you'd like to use, and open each file in Pybricks by using the import button. For example, to use the diagnostics tool, simply open each program in the ```diagnostics``` folder into Pybricks. Then, follow the instructions for each utility.
- This method is not recommended due to the high probability of error. The team is currently working on a custom web Pynamics IDE that automatically fetches the latest compiled bytecode, sends it to your robot, and has an output viewer with custom formatting and integration with the program through (in the future) custom Pynamics ANSI escape codes.
Included utilities:
- Diagnostics - a program that prints out useful information like battery life, etc.
- Color Sensor Tests - a program that identifies what color the sensor is detecting. If you'd like, you can use our color ranges in your own programs.
- 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, (you can select all of them at once) connect your robot, switch to the ```FullDiagnostics.py``` file and press run. The program might take a bit to compile, since there are thousands of lines of code being imported from the other files (partly the reason why the Pynamics IDE will be an improvement, since the Pynamics team will distribute the pre-compiled bytecode)
- 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:
@@ -26,4 +15,4 @@ Without the confusing legal speak, this means that you are free to:
Under the following condition:
- Attribution (BY) — You must give appropriate credit, provide a link to the license, and indicate if changes were made. You may do so in any reasonable manner, but not in any way that suggests the licensor endorses you or your use. Essentially, give us credit, link this repository, and don't take the credit for our work, because that's just sad.
For the full legal details, please review the CC BY 4.0 Legal Code (read the [LICENSE](LICENSE) file here).
For the full legal details, please review the CC BY 4.0 Legal Code.

48
diagnostics/color_sensor_diagnostics.py
View File

@@ -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.PORT_MAP = {
"A": Port.A,
"B": Port.B,
"C": Port.C,
"D": Port.D,
"E": Port.E,
"F": Port.F,
}
self.colorsensorclass = colorsensorclass
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/drive_base_diagnostics.py
View File

@@ -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

73
diagnostics/full_diagnostics.py
View File

@@ -1,73 +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 battery_diagnostics import BatteryDiagnostics
from motor_diagnostics import MotorDiagnostics
from color_sensor_diagnostics import ColorSensorDiagnostics
from drive_base_diagnostics import DriveBaseDiagnostics
from hub_diagnostics import HubDiagnostics
battery = BatteryDiagnostics(HUB)
motor = MotorDiagnostics(HUB, Motor)
colorsensor = ColorSensorDiagnostics(HUB, ColorSensor)
drivebase = DriveBaseDiagnostics(HUB, Motor, DriveBase)
hubdiagnostics = HubDiagnostics(HUB)
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 LEGO® Education SPIKE™ Prime robots, designed for FIRST Lego League.
Developed by Team 65266, Lego Dynamics.
"""
)
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 'db' for drive base diagnostics")
print("Enter 'h' for hub 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] Motor diagnostics completed.")
elif choice == "q":
print("Diagnostics completed successfully. Exiting program.")
break
elif choice == "cs":
print("---------------------COLOR SENSOR DIAGNOSTICS---------------------")
colorsensor.printAll()
print("[Color Sensor Diagnostics] Color sensor diagnostics completed.")
elif choice == "db":
print("----------------------DRIVE BASE DIAGNOSTICS----------------------")
drivebase.printAll()
print("[Drivebase Diagnostics] Drivebase diagnostics completed.")
elif choice == "h":
print("--------------------------HUB DIAGNOSTICS--------------------------")
hubdiagnostics.printAll(False)
else:
print("Invalid choice. Please enter 'b', 'm', or 'q'.")

48
diagnostics/hub_diagnostics.py
View File

@@ -1,48 +0,0 @@
from pybricks.tools import wait, StopWatch
from pybricks import version
import other_functions as debug
from micropython_diagnostics import MicroPythonDiagnostics
from pybricks.parameters import Port, Color
from os_diagnostics import OSDiagnostics
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 testLightSources(self, verbose):
v = verbose
self.hub.display.off()
for x in range(5):
for y in range(5):
debug.log(f"[Hub Diagnostics - Light Sources] Turning on pixel at position {x}, {y}...", v)
self.hub.display.pixel(x, y, brightness=100)
wait(100)
debug.log(f"[Hub Diagnostics - Light Sources] Turning off pixel at position {x}, {y}...", v)
self.hub.display.pixel(x, y, brightness=0)
self.hub.light.on(Color.RED)
def printAll(self, verbose=True):
v = verbose
debug.log("[Hub Diagnostics] Starting hub diagnostics...", v)
while True:
choice = input("[Hub Diagnostics] Which hub diagnostic would you like to run?\n[Hub Diagnostics] Enter 'l' for light source test\n[Hub Diagnostics] Enter 'm' for MicroPython diagnostics\n[Hub Diagnostics] Enter 'o' for operating system diagnostics\n[Hub Diagnostics] Enter 'q' to quit\n[Hub Diagnostics] Your choice: ").strip().lower()
if choice == "l":
debug.log("[Hub Diagnostics] Running light source test...", v)
self.testLightSources(v)
if choice == "m":
debug.log("[Hub Diagnostics] Running MicroPython diagnostics...", v)
MicroPythonDiagnostics.printAll()
if choice == "o"
debug.log("[Hub Diagnostics] Running OS diagnostics...", v)
diag = OSDiagnostics(hub=PrimeHub(), motorclass=Motor)
diag.printAll()
if choice == "q":
print("[Hub Diagnostics] Hub diagnostics completed.")
return

128
diagnostics/micropython_diagnostics.py
View File

@@ -1,128 +0,0 @@
import micropython
import gc
from pybricks import version
class MicroPythonDiagnostics:
def __init__(self, hub):
self.successfultests = 0
self.failedtests = {}
def testgcmanual(self):
gc.disable()
print(f"Initial free: {gc.mem_free()} bytes")
large_data = [i for i in range(10000)]
print(f"After allocation: {gc.mem_free()} bytes")
gc.collect()
aftergcstillref = gc.mem_free()
print(f"After gc.collect (data still referenced): {aftergcstillref} bytes")
large_data = None
print("Reference to data removed.")
aftergcnoref = gc.mem_free()
gc.collect()
print(f"After gc.collect (data dead): {aftergcnoref} bytes")
if(aftergcnoref < aftergcstillref):
print("Completed Test 4/5: Manual garbage collection - SUCCESSFUL")
self.successfultests += 1
else:
print("Completed Test 4/5: Manual garbage collection - FAILED")
self.failedtests["Manual garbage collection"] = "Heap not cleared"
def testgcauto(self):
input("Disabling garbage collection. The amount of used memory should quickly increase. Press Enter to begin:")
gc.disable()
gc.threshold(5000)
total_mem = 255616
bytes_per_hash = 3000
print("Memory Monitor: [# = Used] [. = Free]")
print("-" * (total_mem // bytes_per_hash))
for i in range(500):
_ = bytearray(300)
if i % 25 == 0:
used = gc.mem_alloc()
free = gc.mem_free()
hashes = used // bytes_per_hash
dots = free // bytes_per_hash
print(f"{i:03d}: [{'#' * hashes}{'.' * dots}] {free} bytes free")
final_disabled_free = gc.mem_free()
input("Enabling garbage collection. The amount of used memory should stay relatively low. Press Enter to begin:")
gc.enable()
gc.threshold(5000)
total_mem = 255616
bytes_per_hash = 3000
print("Memory Monitor: [# = Used] [. = Free]")
print("-" * (total_mem // bytes_per_hash))
for i in range(500):
_ = bytearray(300)
if i % 25 == 0:
used = gc.mem_alloc()
free = gc.mem_free()
hashes = used // bytes_per_hash
dots = free // bytes_per_hash
print(f"{i:03d}: [{'#' * hashes}{'.' * dots}] {free} bytes free")
final_enabled_free = gc.mem_free()
if final_enabled_free > final_disabled_free:
print("Completed Test 5/5: Automatic garbage collection - SUCCESSFUL")
print(f"Difference: {final_enabled_free - final_disabled_free} bytes saved.")
self.successfultests += 1
else:
print("Completed Test 5/5: Automatic garbage collection - FAILED")
self.failedtests["Automatic garbage collection"] = "No GC difference"
def performMemoryDiagnostics(self):
input("Press Enter to retrieve memory information:")
print("[Hub Diagnostics - MicroPython - Memory] Memory information (retrieved from the MicroPython environment):")
micropython.mem_info(1)
input("After you're done reading the results, press Enter to run heap diagnostics:")
print("[Hub Diagnostics - MicroPython - Memory] Testing heap lock and unlock.")
print("[Hub Diagnostics - MicroPython - Memory] Allocating memory while heap is unlocked:")
try:
x = [1, 2, 3, 4, 5]
print("[Hub Diagnostics - MicroPython - Memory] Completed Test 1/5: Normal memory allocation - SUCCESS")
print("There was no MemoryError raised. The value of the new variable x is", x)
self.successfultests += 1
except MemoryError:
print("[Hub Diagnostics - MicroPython - Memory] Completed Test 1/5: Normal memory allocation - FAILED")
self.failedtests["Normal memory allocation"] = "MemoryError"
print("[Hub Diagnostics - MicroPython - Memory] Locking the heap:")
micropython.heap_lock()
print("[Hub Diagnostics - MicroPython - Memory] Heap was locked. Attempting to allocate memory (this should fail):")
try:
y = [10, 20, 30, 40, 50]
print("[Hub Diagnostics - MicroPython - Memory] Completed Test 2/5: Heap lock - FAILED")
self.failedtests["Heap lock"] = "No heap lock"
except MemoryError:
print("[Hub Diagnostics - MicroPython - Memory] Completed Test 2/5: Heap lock - SUCCESS")
self.successfultests += 1
print("[Hub Diagnostics - MicroPython - Memory] Unlocking the heap:")
micropython.heap_unlock()
print("[Hub Diagnostics - MicroPython - Memory] Heap was unlocked. Attempting to allocate memory (this should succeed):")
try:
z = [100, 200, 300, 400, 500]
print("[Hub Diagnostics - MicroPython - Memory] Completed Test 3/5: Heap unlock - FAILED")
print("The value of the new variable z is", z)
self.successfultests += 1
except MemoryError:
print("[Hub Diagnostics - MicroPython - Memory] Completed Test 3/5: Heap unlock - FAILED")
self.failedtests["Heap unlock"] = "No heap unlock"
def printAll(self):
self.performMemoryDiagnostics()
input("After you're done reading the results, press Enter to run manual garbage collection test:")
self.testgcmanual()
input("After you're done reading the results, press Enter to run automatic garbage collection test:")
self.testgcauto()
print(f"\n=== Results: {self.successfultests}/5 tests passed ===")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
#test = MicroPythonDiagnostics(hub=PrimeHub())
#test.printAll()

135
diagnostics/motor_diagnostics.py
View File

@@ -1,135 +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.port_map = {
"A": Port.A,
"B": Port.B,
"C": Port.C,
"D": Port.D,
"E": Port.E,
"F": Port.F,
}
self.motorclass = motorclass
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 = 10 # 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()

933
diagnostics/os_diagnostics.py
View File

@@ -1,933 +0,0 @@
from pybricks.parameters import Port
from uerrno import EAGAIN, EBUSY, ECANCELED, EINVAL, EIO, ENODEV, EOPNOTSUPP, EPERM, ETIMEDOUT
import uio
import ujson
import umath
import uselect
import ustruct
import usys
import urandom
from urandom import random
from pybricks.tools import wait, multitask, run_task
import pybricks as pybricksforvers
class FakeUART:
def __init__(self, port, baudrate, timeout):
self.timeout = timeout
self._force_error = None
print("Warning: No physical UART detected. Using simulator.")
def set_error(self, errno):
self._force_error = errno
def read(self, length=1):
if self._force_error is not None:
err = self._force_error
self._force_error = None
raise OSError(err)
if self.timeout is not None:
wait(self.timeout)
raise OSError(ETIMEDOUT)
else:
while True:
wait(1000)
def write(self, data):
if self._force_error is not None:
err = self._force_error
self._force_error = None
raise OSError(err)
def UARTDevice(port, baudrate=9600, timeout=None):
return FakeUART(port, baudrate, timeout)
class OSDiagnostics:
def __init__(self, hub, motorclass):
self.hub = hub
self.motorclass = motorclass
self.successfultests = 0
self.failedtests = {}
def testUErrno(self):
uerrnotestobject = UerrnoTest(self.hub, self.motorclass)
uerrnotestobject.testeagain()
uerrnotestobject.testebusy()
uerrnotestobject.testecanceled()
uerrnotestobject.testeinval()
uerrnotestobject.testeio()
uerrnotestobject.testenodev()
uerrnotestobject.testeopnotsupp()
uerrnotestobject.testeperm()
uerrnotestobject.testetimedout()
uerrnotestobject.print_results()
self.successfultests += uerrnotestobject.successfultests
self.failedtests.update(uerrnotestobject.failedtests)
def testUIO(self):
uiotestobject = UIOTest(self.hub, self.motorclass)
uiotestobject.print_results()
self.successfultests += uiotestobject.successfultests
self.failedtests.update(uiotestobject.failedtests)
def testUJSON(self):
ujsontestobject = UJSONTest(self.hub, self.motorclass)
ujsontestobject.print_results()
self.successfultests += ujsontestobject.successfultests
self.failedtests.update(ujsontestobject.failedtests)
def testUMath(self):
umathtestobject = UMathTest(self.hub, self.motorclass)
umathtestobject.print_results()
self.successfultests += umathtestobject.successfultests
self.failedtests.update(umathtestobject.failedtests)
def testURandom(self):
urandtestobject = URandomTest(self.hub, self.motorclass)
urandtestobject.print_results()
self.successfultests += urandtestobject.successfultests
self.failedtests.update(urandtestobject.failedtests)
def testUSelect(self):
uselecttestobject = USelectTest(self.hub, self.motorclass)
uselecttestobject.print_results()
self.successfultests += uselecttestobject.successfultests
self.failedtests.update(uselecttestobject.failedtests)
def testUStruct(self):
ustructtestobject = UStructTest(self.hub, self.motorclass)
ustructtestobject.print_results()
self.successfultests += ustructtestobject.successfultests
self.failedtests.update(ustructtestobject.failedtests)
def testUSys(self):
usystestobject = USysTest(self.hub, self.motorclass)
usystestobject.print_results()
self.successfultests += usystestobject.successfultests
self.failedtests.update(usystestobject.failedtests)
def printAll(self):
self.testUErrno()
self.testUIO()
self.testUJSON()
self.testUMath()
self.testURandom()
self.testUSelect()
self.testUStruct()
self.testUSys()
print(f"\n=== Results: {self.successfultests}/62 tests passed ===")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
else:
print("No tests failed. Great job!")
class UerrnoTest:
def __init__(self, hub, motorclass):
self.hub = hub
self.motorclass = motorclass
self.successfultests = 0
self.failedtests = {}
def testeagain(self):
print("Starting Test 1/9: EAGAIN - Try Again Error")
uart = UARTDevice(Port.A, baudrate=9600, timeout=1000)
uart.set_error(EAGAIN)
try:
uart.read(1)
print("No error raised.\nCompleted Test 1/9: EAGAIN - FAILED")
self.failedtests["EAGAIN"] = "No error raised"
except OSError as ex:
if ex.errno == EAGAIN:
print("EAGAIN can be thrown and caught.\nCompleted Test 1/9: EAGAIN - SUCCESSFUL")
self.successfultests += 1
elif ex.errno == EIO:
print("An unspecified error occurred (EIO).\nCompleted Test 1/9: EAGAIN - FAILED")
self.failedtests["EAGAIN"] = "EIO - Unspecified Error"
else:
print(f"Another error occurred with code: {ex.errno}.\nCompleted Test 1/9: EAGAIN - FAILED")
self.failedtests["EAGAIN"] = ex.errno
def testebusy(self):
# No reliable hardware trigger; use FakeUART
print("Starting Test 2/9: EBUSY - Device Busy Error")
uart = UARTDevice(Port.A, baudrate=9600, timeout=1000)
uart.set_error(EBUSY)
try:
uart.read(1)
print("No error raised.\nCompleted Test 2/9: EBUSY - FAILED")
self.failedtests["EBUSY"] = "No error raised"
except OSError as ex:
if ex.errno == EBUSY:
print("EBUSY can be thrown and caught.\nCompleted Test 2/9: EBUSY - SUCCESSFUL")
self.successfultests += 1
elif ex.errno == EIO:
print("An unspecified error occurred (EIO).\nCompleted Test 2/9: EBUSY - FAILED")
self.failedtests["EBUSY"] = "EIO - Unspecified Error"
else:
print(f"Another error occurred with code: {ex.errno}.\nCompleted Test 2/9: EBUSY - FAILED")
self.failedtests["EBUSY"] = ex.errno
def testecanceled(self):
# No reliable hardware trigger; use FakeUART
print("Starting Test 3/9: ECANCELED - Operation Canceled Error")
uart = UARTDevice(Port.A, baudrate=9600, timeout=1000)
uart.set_error(ECANCELED)
try:
uart.read(1)
print("No error raised.\nCompleted Test 3/9: ECANCELED - FAILED")
self.failedtests["ECANCELED"] = "No error raised"
except OSError as ex:
if ex.errno == ECANCELED:
print("ECANCELED can be thrown and caught.\nCompleted Test 3/9: ECANCELED - SUCCESSFUL")
self.successfultests += 1
elif ex.errno == EIO:
print("An unspecified error occurred (EIO).\nCompleted Test 3/9: ECANCELED - FAILED")
self.failedtests["ECANCELED"] = "EIO - Unspecified Error"
else:
print(f"Another error occurred with code: {ex.errno}.\nCompleted Test 3/9: ECANCELED - FAILED")
self.failedtests["ECANCELED"] = ex.errno
def testeinval(self):
# Triggered by passing an out-of-range value to motor.control.limits()
print("Starting Test 4/9: EINVAL - Invalid Argument Error")
try:
usys.stderr.flush()
except (OSError) as ex:
if ex.errno == EINVAL:
print("EINVAL can be thrown and caught.\nCompleted Test 4/9: EINVAL - SUCCESSFUL")
self.successfultests += 1
elif errno_val == EIO:
print("An unspecified error occurred (EIO).\nCompleted Test 4/9: EINVAL - FAILED")
self.failedtests["EINVAL"] = "EIO - Unspecified Error"
else:
print(f"Another error occurred with code: {ex}.\nCompleted Test 4/9: EINVAL - FAILED")
self.failedtests["EINVAL"] = str(ex)
def testeio(self):
# No reliable scriptable trigger (requires physical unplug); use FakeUART
print("Starting Test 5/9: EIO - I/O Error")
uart = UARTDevice(Port.A, baudrate=9600, timeout=1000)
uart.set_error(EIO)
try:
uart.read(1)
print("No error raised.\nCompleted Test 5/9: EIO - FAILED")
self.failedtests["EIO"] = "No error raised"
except OSError as ex:
if ex.errno == EIO:
print("EIO can be thrown and caught.\nCompleted Test 5/9: EIO - SUCCESSFUL")
self.successfultests += 1
else:
print(f"Another error occurred with code: {ex.errno}.\nCompleted Test 5/9: EIO - FAILED")
self.failedtests["EIO"] = ex.errno
def testenodev(self):
# Triggered by initializing a motor on an empty port
print("Starting Test 6/9: ENODEV - Device Not Found Error")
input("Make sure Port A doesn't have anything plugged in, then press Enter.")
try:
my_motor = self.motorclass(Port.A)
print("OS detected a motor when there was nothing. You may have allowed missing motors. This is useful for debugging, but not recommended for production as it can cause issues with device control.")
self.failedtests["ENODEV"] = "No error raised"
except OSError as ex:
if ex.errno == ENODEV:
print("There is no motor on this port. ENODEV can be thrown and caught.\nCompleted Test 6/9: ENODEV - SUCCESSFUL")
self.successfultests += 1
elif ex.errno == EIO:
print("An unspecified error occurred (EIO).\nCompleted Test 6/9: ENODEV - FAILED")
self.failedtests["ENODEV"] = "EIO - Unspecified Error"
else:
print(f"Another error occurred with code: {ex.errno}.\nCompleted Test 6/9: ENODEV - FAILED")
self.failedtests["ENODEV"] = ex.errno
def testeopnotsupp(self):
# No reliable scriptable trigger without specific hardware; use FakeUART
print("Starting Test 7/9: EOPNOTSUPP - Operation Not Supported Error")
uart = UARTDevice(Port.A, baudrate=9600, timeout=1000)
uart.set_error(EOPNOTSUPP)
try:
uart.read(1)
print("No error raised.\nCompleted Test 7/9: EOPNOTSUPP - FAILED")
self.failedtests["EOPNOTSUPP"] = "No error raised"
except OSError as ex:
if ex.errno == EOPNOTSUPP:
print("EOPNOTSUPP can be thrown and caught.\nCompleted Test 7/9: EOPNOTSUPP - SUCCESSFUL")
self.successfultests += 1
elif ex.errno == EIO:
print("An unspecified error occurred (EIO).\nCompleted Test 7/9: EOPNOTSUPP - FAILED")
self.failedtests["EOPNOTSUPP"] = "EIO - Unspecified Error"
else:
print(f"Another error occurred with code: {ex.errno}.\nCompleted Test 7/9: EOPNOTSUPP - FAILED")
self.failedtests["EOPNOTSUPP"] = ex.errno
def testeperm(self):
print("Starting Test 8/9: EPERM - Operation Not Permitted Error")
uart = UARTDevice(Port.A, baudrate=9600, timeout=1000)
uart.set_error(EPERM)
try:
uart.read(1)
print("No error raised.\nCompleted Test 8/9: EPERM - FAILED")
self.failedtests["EPERM"] = "No error raised"
except OSError as ex:
if ex.errno == EPERM:
print("EPERM can be thrown and caught.\nCompleted Test 8/9: EPERM - SUCCESSFUL")
self.successfultests += 1
elif ex.errno == EIO:
print("An unspecified error occurred (EIO).\nCompleted Test 8/9: EPERM - FAILED")
self.failedtests["EPERM"] = "EIO - Unspecified Error"
else:
print(f"Another error occurred with code: {ex.errno}.\nCompleted Test 8/9: EPERM - FAILED")
self.failedtests["EPERM"] = ex.errno
def testetimedout(self):
# Triggered by FakeUART (or real UART) timing out on read
print("Starting Test 9/9: ETIMEDOUT - Timed Out Error")
uart = UARTDevice(Port.A, baudrate=9600, timeout=1000)
try:
data = uart.read(10)
print("No error raised.\nCompleted Test 9/9: ETIMEDOUT - FAILED")
self.failedtests["ETIMEDOUT"] = "No error raised"
except OSError as ex:
if ex.errno == ETIMEDOUT:
print("Timed out with synthetic UART device. ETIMEDOUT can be thrown and caught.\nCompleted Test 9/9: ETIMEDOUT - SUCCESSFUL")
self.successfultests += 1
elif ex.errno == EIO:
print("An unspecified error occurred (EIO).\nCompleted Test 9/9: ETIMEDOUT - FAILED")
self.failedtests["ETIMEDOUT"] = "EIO - Unspecified Error"
else:
print(f"Another error occurred with code: {ex.errno}.\nCompleted Test 9/9: ETIMEDOUT - FAILED")
self.failedtests["ETIMEDOUT"] = ex.errno
def print_results(self):
print(f"\n=== Results: {self.successfultests}/9 tests passed ===")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
class UIOTest(OSDiagnostics):
def __init__(self, hub, motorclass):
self.hub = hub
self.motorclass = motorclass
self.successfultests = 0
self.failedtests = {}
# uio contains BytesIO, StringIO, and FileIO, but due to SPIKE Prime's lack of a filesystem, the test will omit FileIO and only include BytesIO and StringIO
def testbytesio(self):
try:
buffer = uio.BytesIO()
buffer.write(b'Hello, ')
buffer.write(b'Pybricks byte stream!')
current_content = buffer.getvalue()
print(f"Buffer content (via getvalue()): {current_content}")
print(f"Current cursor position: {buffer.tell()}")
# TODO: After testing that BytesIO actually works on this system, add checks to make sure that the outputs match what they should.
buffer.seek(0)
read_data = buffer.read()
print(f"Read data (via read()): {read_data}")
print(f"Current cursor position after reading: {buffer.tell()}")
buffer.close()
print("Buffer was closed successfully.")
print("Completed Test 1/2: BytesIO - SUCCESSFUL")
self.successfultests += 1
except Exception as ex:
print("An unexpected error occured.")
print("Completed Test 1/2: BytesIO - FAILED")
self.failedtests["BytesIO"] = ex.errno
def teststringio(self):
try:
buffer = uio.StringIO()
buffer.write('Hello, ')
buffer.write('Pybricks string stream!')
current_content = buffer.getvalue()
print(f"Buffer content (via getvalue()): {current_content}")
print(f"Current cursor position: {buffer.tell()}")
# TODO: After testing that StringIO actually works on this system, add checks to make sure that the outputs match what they should.
buffer.seek(0)
read_data = buffer.read()
print(f"Read data (via read()): {read_data}")
print(f"Current cursor position after reading: {buffer.tell()}")
buffer.close()
print("Buffer was closed successfully.")
print("Completed Test 2/2: StringIO - SUCCESSFUL")
self.successfultests += 1
except Exception as ex:
print("An unexpected error occured.")
print("Completed Test 2/2: StringIO - FAILED")
self.failedtests["StringIO"] = ex.errno
def print_results(self):
self.testbytesio()
self.teststringio()
print(f"\n=== Results: {self.successfultests}/2 tests passed ===")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
class UJSONTest:
def __init__(self, hub, motorclass):
self.hub = hub
self.motorclass = motorclass
self.successfultests = 0
self.failedtests = {}
# Tests ujson.dumps() and ujson.loads()
def testdumpsloads(self):
try:
original = {"robot": "Pybricks", "speed": 500, "active": True}
json_str = ujson.dumps(original)
print(f"Serialized (via dumps()): {json_str}")
restored = ujson.loads(json_str)
print(f"Deserialized (via loads()): {restored}")
# TODO: After confirming dumps/loads works on this system, add equality tests
print("Completed Test 1/3: dumps/loads - SUCCESSFUL")
self.successfultests += 1
except Exception as ex:
print("An unexpected error occurred.")
print("Completed Test 1/3: dumps/loads - FAILED")
self.failedtests["dumps/loads"] = getattr(ex, "errno", str(ex))
# Tests ujson.loads() raising ValueError on malformed input
def testloadsinvalid(self):
try:
ujson.loads("{not valid json}")
print("No error raised.")
print("Completed Test 2/3: loads invalid - FAILED")
self.failedtests["loads_invalid"] = "No error raised"
except ValueError:
print("ValueError raised on malformed JSON, as expected.")
print("Completed Test 2/3: loads invalid - SUCCESSFUL")
self.successfultests += 1
except Exception as ex:
print("An unexpected error occurred.")
print("Completed Test 2/3: loads invalid - FAILED")
self.failedtests["loads_invalid"] = getattr(ex, "errno", str(ex))
# Tests ujson.dump() and ujson.load() using a uio.StringIO stream
def testdumpload(self):
try:
original = {"hub": "SPIKE Prime", "port": "A", "value": 42}
stream = uio.StringIO()
ujson.dump(original, stream)
print(f"Serialized to stream (via dump()): {stream.getvalue()}")
stream.seek(0)
restored = ujson.load(stream)
print(f"Deserialized from stream (via load()): {restored}")
stream.close()
# TODO: After confirming dump/load works on this system, add
# equality assertions to verify round-trip fidelity.
print("Completed Test 3/3: dump/load (stream) - SUCCESSFUL")
self.successfultests += 1
except Exception as ex:
print("An unexpected error occurred.")
print("Completed Test 3/3: dump/load (stream) - FAILED")
self.failedtests["dump/load"] = getattr(ex, "errno", str(ex))
def print_results(self):
self.testdumpsloads()
self.testloadsinvalid()
self.testdumpload()
print(f"\n=== Results: {self.successfultests}/3 tests passed ===")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
class UMathTest:
def __init__(self, hub, motorclass):
self.hub = hub
self.motorclass = motorclass
self.successfultests = 0
self.failedtests = {}
def test_math(self):
EPSILON = 0.0001
if(umath.ceil(87.21) == 88):
self.successfultests += 1
else:
self.failedtests["ceilpos"] = "Failed"
if(umath.floor(14.61) == 14):
self.successfultests += 1
else:
self.failedtests["floorpos"] = "Failed"
if(umath.ceil(-87.21) == -87):
self.successfultests += 1
else:
self.failedtests["ceilneg"] = "Failed"
if(umath.floor(-14.61) == -15):
self.successfultests += 1
else:
self.failedtests["floorneg"] = "Failed"
if(umath.trunc(33.22) == 33):
self.successfultests += 1
else:
self.failedtests["truncpos"] = "Failed"
if(umath.trunc(-33.22) == -33):
self.successfultests += 1
else:
self.failedtests["truncneg"] = "Failed"
if(umath.fmod(6040, 3) == 1):
self.successfultests += 1
else:
self.failedtests["fmod"] = "Failed"
if(umath.fabs(88273) == 88273):
self.successfultests += 1
else:
self.failedtests["fabspos"] = "Failed"
if(umath.fabs(-27482) == 27482):
self.successfultests += 1
else:
self.failedtests["fabsneg"] = "Failed"
if(umath.fabs(-2742.233) == 2742.233):
self.successfultests += 1
else:
self.failedtests["fabsflt"] = "Failed"
if(umath.copysign(3928, -182) == -3928):
self.successfultests += 1
else:
self.failedtests["copysign"] = "Failed"
if(umath.exp(umath.log(1)) == 1):
self.successfultests += 2
else:
self.failedtests["eexp"] = "Failed"
self.failedtests["ln"] = "Failed"
if(abs(umath.e - 2.718282) < EPSILON):
self.successfultests += 1
else:
self.failedtests["e"] = "Failed"
if(umath.pow(19, 7) == 893871739):
self.successfultests += 1
else:
self.failedtests["pow"] = "Failed"
if(umath.sqrt(242064) == 492):
self.successfultests += 1
else:
self.failedtests["sqrt"] = "Failed"
if(abs(umath.pi - 3.141593) < EPSILON):
self.successfultests += 1
else:
self.failedtests["pi"] = "Failed"
if abs(umath.degrees(umath.pi * 3) - 540) < EPSILON:
self.successfultests += 1
else:
self.failedtests["degrees"] = "Failed"
if abs(umath.radians(270) - umath.pi * 1.5) < EPSILON:
self.successfultests += 1
else:
self.failedtests["radians"] = "Failed"
if(abs(umath.sin(umath.pi)) < EPSILON):
self.successfultests += 1
else:
self.failedtests["sin"] = "Failed"
if(abs(umath.asin(1) - umath.pi * 0.5) < EPSILON):
self.successfultests += 1
else:
self.failedtests["asin"] = "Failed"
if(abs(umath.cos(umath.pi) + 1) < EPSILON):
self.successfultests += 1
else:
self.failedtests["cos"] = "Failed"
if(abs(umath.acos(1)) < EPSILON):
self.successfultests += 1
else:
self.failedtests["acos"] = "Failed"
if(abs(umath.tan(umath.pi)) < EPSILON):
self.successfultests += 1
else:
self.failedtests["tan"] = "Failed"
if(abs(umath.atan(1) - umath.pi * 0.25) < EPSILON):
self.successfultests += 1
else:
self.failedtests["atan"] = "Failed"
if(abs(umath.atan2(1, -1) - umath.pi * 0.75) < EPSILON):
self.successfultests += 1
else:
self.failedtests["atan2"] = "Failed"
infinitenum = float('inf')
finitenum = 123456
if(umath.isfinite(finitenum) == True):
self.successfultests += 1
else:
self.failedtests["isfinitefinite"] = "Failed"
if(umath.isfinite(infinitenum) == False):
self.successfultests += 1
else:
self.failedtests["isfiniteinfinite"] = "Failed"
if(umath.isinf(finitenum) == False):
self.successfultests += 1
else:
self.failedtests["isinfinitefinite"] = "Failed"
if(umath.isinf(infinitenum) == True):
self.successfultests += 1
else:
self.failedtests["isinfiniteinfinite"] = "Failed"
nannum = float("nan")
if(umath.isnan(nannum) == True):
self.successfultests += 1
else:
self.failedtests["isnannan"] = "Failed"
if(umath.isnan(finitenum) == False):
self.successfultests += 1
else:
self.failedtests["isnannotnan"] = "Failed"
frac, integer = umath.modf(87.21)
if abs(integer - 87.0) < 0.01 and abs(frac - 0.21) < 0.01:
self.successfultests += 1
else:
self.failedtests["modf"] = "Failed"
mantissa, exponent = umath.frexp(64)
if mantissa == 0.5 and exponent == 7:
self.successfultests += 1
else:
self.failedtests["frexp"] = "Failed"
result = umath.ldexp(0.5, 7)
if result == 64:
self.successfultests += 1
else:
self.failedtests["ldexp"] = "Failed"
def print_results(self):
self.test_math()
print(f"\n=== Results: {self.successfultests}/35 tests passed ===")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
class URandomTest:
def __init__(self, hub, motorclass):
self.hub = hub
self.motorclass = motorclass
self.successfultests = 0
self.failedtests = {}
def testrandom(self):
NUM_SAMPLES = 6553
NUM_BUCKETS = 10
CHART_WIDTH = 50
SAMPLE_PEEK = 20
samples = [random() for _ in range(NUM_SAMPLES)]
bucket_counts = [0] * NUM_BUCKETS
for x in samples:
i = int(x * NUM_BUCKETS)
if i == NUM_BUCKETS:
i -= 1
bucket_counts[i] += 1
mean = sum(samples) / NUM_SAMPLES
ideal = NUM_SAMPLES / NUM_BUCKETS
max_count = max(bucket_counts)
print("=" * 66)
print(" random() distribution test n=" + str(NUM_SAMPLES))
print("=" * 66)
print("")
print(" Range Count Bar")
print(" -------------- ----- " + "-" * CHART_WIDTH)
for i in range(NUM_BUCKETS):
lo = i / NUM_BUCKETS
hi = (i + 1) / NUM_BUCKETS
count = bucket_counts[i]
bar = int(round(count / max_count * CHART_WIDTH))
dev = abs(count - ideal) / ideal
if dev <= 0.10:
marker = "#"
elif dev <= 0.20:
marker = "+"
else:
marker = "."
lo_str = "{:.2f}".format(lo)
hi_str = "{:.2f}".format(hi)
print(" " + "{:.2f}".format(lo) + " - " + "{:.2f}".format(hi) + " " + str(count) + " " + marker * bar)
print("")
print(" Samples : " + str(NUM_SAMPLES))
print(" Mean : " + "{:.6f}".format(mean) + " (ideal 0.500000)")
print(" Min : " + "{:.6f}".format(min(samples)))
print(" Max : " + "{:.6f}".format(max(samples)))
print(" Legend : # within 10% + within 20% . beyond 20%")
print("")
print(" First " + str(SAMPLE_PEEK) + " raw values:")
row = " "
for idx, val in enumerate(samples[:SAMPLE_PEEK], 1):
row += "{:.4f} ".format(val)
if idx % 5 == 0:
print(row)
row = " "
if row.strip():
print(row)
print("")
if(abs(0.5 - mean) < 0.06):
print("Random Distribution Test: SUCCESSFUL")
self.successfultests += 1
else:
print("Random Distribution Test: FAILED")
self.failedtests["randomdistribution"] = "Too much error"
def test_other_rands(self):
N = 1000
choicelist = ["apple", "banana", "grape", "orange"]
randint_vals = [urandom.randint(1, 100) for _ in range(N)]
randint_mean = sum(randint_vals) / N
print("Randint mean (ideally 50.5): {:.4f}".format(randint_mean))
if abs(50.5 - randint_mean) < 3.0:
print("Randint Test: SUCCESSFUL")
self.successfultests += 1
else:
print("Randint Test: FAILED")
self.failedtests["randint"] = "Too much error"
getrandbits_vals = [urandom.getrandbits(7) for _ in range(N)]
getrandbits_mean = sum(getrandbits_vals) / N
print("Getrandbits(7) mean (ideally 63.5): {:.4f}".format(getrandbits_mean))
if abs(63.5 - getrandbits_mean) < 4:
print("Getrandbits Test: SUCCESSFUL")
self.successfultests += 1
else:
print("Getrandbits Test: FAILED")
self.failedtests["getrandbits"] = "Too much error"
randrange_vals = [urandom.randrange(1, 1001, 4) for _ in range(N)]
randrange_mean = sum(randrange_vals) / N
print("Randrange(1,1001,4) mean (ideally 501.0): {:.4f}".format(randrange_mean))
if abs(501.0 - randrange_mean) < 20.0:
print("Randrange Test: SUCCESSFUL")
self.successfultests += 1
else:
print("Randrange Test: FAILED")
self.failedtests["randrange"] = "Too much error"
uniform_vals = [urandom.uniform(1, 10) for _ in range(N)]
uniform_mean = sum(uniform_vals) / N
print("Uniform(1,10) mean (ideally 5.5): {:.4f}".format(uniform_mean))
if abs(5.5 - uniform_mean) < 0.3:
print("Uniform Test: SUCCESSFUL")
self.successfultests += 1
else:
print("Uniform Test: FAILED")
self.failedtests["uniform"] = "Too much error"
choice_counts = {}
for item in choicelist:
choice_counts[item] = 0
for _ in range(N):
choice_counts[urandom.choice(choicelist)] += 1
ideal_pct = 100.0 / len(choicelist)
print("Choice distribution (ideally {:.1f}% each):".format(ideal_pct))
choice_ok = True
for item in choicelist:
pct = choice_counts[item] / N * 100
print(" " + item + ": {:.1f}%".format(pct))
if abs(pct - ideal_pct) > 5.0:
choice_ok = False
if choice_ok:
print("Choice Test: SUCCESSFUL")
self.successfultests += 1
else:
print("Choice Test: FAILED")
self.failedtests["choice"] = "Too much error"
def print_results(self):
self.testrandom()
self.test_other_rands()
print(f"\n=== Results: {self.successfultests}/6 tests passed ===")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
class USelectTest:
def __init__(self, hub, motorclass):
self.hub = hub
self.motorclass = motorclass
self.successfultests = 0
self.failedtests = {}
def print_results(self):
# Register the standard input so we can read keyboard presses.
keyboard = uselect.poll()
keyboard.register(usys.stdin)
print("Type a few keys, make sure you get back what character you typed, then press Escape.")
while True:
# Check if a key has been pressed.
if keyboard.poll(0):
# Read the key and print it.
key = usys.stdin.read(1)
if key == '\x1b':
print("Escape key pressed. Exiting...")
break
else:
print("Pressed:", key)
result = input("Input Y if the results were accurate:")
if(result == "Y" or result == "y"):
print(f"\n=== Results: 1/1 tests passed ===")
self.successfultests += 1
else:
self.failedtests["input"] = "Unsatisfied"
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
class UStructTest:
def __init__(self, hub, motorclass):
self.hub = hub
self.motorclass = motorclass
self.successfultests = 0
self.failedtests = {}
def print_results(self):
packed = ustruct.pack('ii', 42, 100)
print(f'Packed bytes using pack(): {packed}')
unpacked = ustruct.unpack('ii', packed)
print(f'Unpacked values using unpack(): {unpacked}')
print(unpacked == (42, 100))
if(unpacked == (42, 100)):
print("Completed Test 1/2: pack - SUCCESSFUL")
self.successfultests += 1
else:
print("Completed Test 1/2: pack - FAILED")
self.failedtests["pack"] = "Failed"
format_string = 'hhl'
size = ustruct.calcsize(format_string)
buffer = bytearray(size)
ustruct.pack_into(format_string, buffer, 0, 5, 10, 15)
print("Packed buffer using pack_into():", buffer)
unpackedfrom = ustruct.unpack_from(format_string, buffer, 0)
print("Unpacked buffer using unpack_from():", unpackedfrom)
if(unpackedfrom == (5, 10, 15)):
print("Completed Test 2/2: pack_into - SUCCESSFUL")
self.successfultests += 1
else:
print("Completed Test 2/2: pack_into - FAILED")
self.failedtests["pack_into"] = "Failed"
print(f"\n=== Results: {self.successfultests}/2 tests passed ===")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
class USysTest:
def __init__(self, hub, motorclass):
self.hub = hub
self.motorclass = motorclass
self.successfultests = 0
self.failedtests = {}
def printVersionDiagnostics(self):
try:
print("Hub version information:", pybricksforvers.version)
print("MicroPython version:", usys.version)
print("Pybricks version information:", usys.version_info)
print("MicroPython information:", usys.implementation)
self.successfultests += 1
print("Completed Test 1/4: versioninfo - SUCCESSFUL")
except Exception as ex:
self.failedtests["versioninfo"] = ex.errno
print("Completed Test 4/4: versioninfo - FAILED")
def teststdin(self):
# Register the standard input so we can read keyboard presses.
keyboard = uselect.poll()
keyboard.register(usys.stdin)
print("Type a few keys, make sure you get back what character you typed, then press Escape.")
while True:
# Check if a key has been pressed.
if keyboard.poll(0):
# Read the key and print it.
key = usys.stdin.read(1)
if key == '\x1b':
print("Escape key pressed. Exiting...")
break
else:
print("Pressed:", key)
result = input("Input Y if the results were accurate:")
if(result == "Y" or result == "y"):
print("Completed Test 1/4: stdin - SUCCESSFUL")
self.successfultests += 1
else:
self.failedtests["stdin"] = "Unsatisfied"
print("Completed Test 1/4: stdin - FAILED")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
def teststdout(self):
usys.stdout.flush()
try:
usys.stdout.buffer.write(b"stdout worked!\n")
print("Completed Test 2/4: stdout - SUCCESSFUL")
self.successfultests += 1
except Exception as ex:
print("Completed Test 2/4: stdout - FAILED")
self.failedtests["stdout"] = ex.errno
def teststderr(self):
usys.stdout.flush()
try:
usys.stderr.buffer.write(b"stderr worked!\n")
print("Completed Test 3/4: stderr - SUCCESSFUL")
self.successfultests += 1
except Exception as ex:
print("Completed Test 3/4: stderr - FAILED")
self.failedtests["stderr"] = ex.errno
def print_results(self):
self.teststdin()
self.teststdout()
self.teststderr()
self.printVersionDiagnostics()
print(f"\n=== Results: {self.successfultests}/4 tests passed ===")
if self.failedtests:
print("Failed tests:")
for key, value in self.failedtests.items():
print(f" {key}: {value}")
#diag = OSDiagnostics(hub=PrimeHub(), motorclass=Motor)
#diag.testAll()

3
diagnostics/other_functions.py
View File

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

1
experiments/game.py
View File

@@ -1 +0,0 @@
# will use the light matrix and btns

159
experiments/trajectory_drive.py
View File

@@ -1,159 +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, Axis
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, run_task, multitask
import umath
hub = PrimeHub()
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B, Direction.CLOCKWISE)
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]], [[12,20,24]])
right_arm = Motor(Port.D, Direction.CLOCKWISE, [[12,36],[12,20,24]])
lazer_ranger = UltrasonicSensor(Port.E)
color_sensor = ColorSensor(Port.F)
WHEEL_DIAMETER = 68.8
AXLE_TRACK = 180
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
drive_base.settings(600, 500, 300, 200)
drive_base.use_gyro(False)
current = {"x": 0, "y": 0, "a": 0}
target = {"x": 0, "y": 200, "a": 0}
done = False
def addtoposition(a, r):
current["x"] += r * umath.sin(umath.radians(a))
current["y"] += r * umath.cos(umath.radians(a))
current["a"] = a
async def track_heading():
while True:
current["a"] = hub.imu.heading()
await wait(20)
async def printStats():
last_dist = 0
while True:
if done:
break
current_dist = drive_base.distance()
instant_dist = current_dist - last_dist
last_dist = current_dist
current["x"] += instant_dist * umath.sin(umath.radians(current["a"]))
current["y"] += instant_dist * umath.cos(umath.radians(current["a"]))
dx = target["x"] - current["x"]
dy = target["y"] - current["y"]
remaining = umath.sqrt(dx*dx + dy*dy)
desired_a = 90 - umath.degrees(umath.atan2(dy, dx))
print("x:", current["x"], "y:", current["y"], "desired_a:", desired_a, "current_a:", current["a"], "remaining:", remaining)
await wait(50)
async def pid_turn(target_angle):
KP = 3
KI = 0.01
KD = 1.5
SETTLE_THRESHOLD = 0.5
SETTLE_TIME = 500
integral = 0
last_error = 0
settled_timer = StopWatch()
settled = False
while True:
error = target_angle - current["a"]
while error > 180: error -= 360
while error < -180: error += 360
integral += error
integral = max(-100, min(100, integral))
derivative = error - last_error
last_error = error
turn_rate = KP * error + KI * integral + KD * derivative
turn_rate = max(-200, min(200, turn_rate))
drive_base.drive(0, turn_rate)
if abs(error) < SETTLE_THRESHOLD:
if not settled:
settled = True
settled_timer.reset()
elif settled_timer.time() > SETTLE_TIME:
drive_base.stop()
break
else:
settled = False
await wait(20)
async def driveTrajectory():
global done
ARRIVE_THRESHOLD = 30 # give stage 2 more room to work
FINAL_THRESHOLD = 7
SPEED = 400
SLOW_SPEED = 30
TURN_GAIN = 3.0
# stage 1
while True:
dx = target["x"] - current["x"]
dy = target["y"] - current["y"]
remaining = umath.sqrt(dx*dx + dy*dy)
if remaining < ARRIVE_THRESHOLD:
drive_base.stop()
break
desired_a = 90 - umath.degrees(umath.atan2(dy, dx))
error = desired_a - current["a"]
while error > 180: error -= 360
while error < -180: error += 360
turn_rate = max(-200, min(200, error * TURN_GAIN))
# in stage 1, scale speed down as it gets closer
speed = max(150, SPEED * min(1, remaining / 300))
drive_base.drive(speed, turn_rate)
await wait(20)
print("Stage 1 done")
# stage 2
while True:
dx = target["x"] - current["x"]
dy = target["y"] - current["y"]
remaining = umath.sqrt(dx*dx + dy*dy)
if remaining < FINAL_THRESHOLD:
drive_base.stop()
break
a_rad = umath.radians(current["a"])
forward_x = umath.sin(a_rad)
forward_y = umath.cos(a_rad)
signed_dist = dx * forward_x + dy * forward_y
speed = SLOW_SPEED if signed_dist > 0 else -SLOW_SPEED
drive_base.drive(speed, 0)
await wait(20)
drive_base.stop()
print("Stage 2 done")
# stage 3 — PID angle correction
await pid_turn(target["a"])
done = True
print("Done! x:", current["x"], "y:", current["y"], "a:", current["a"])
async def main():
drive_base.reset()
await multitask(driveTrajectory(), track_heading(), printStats())
hub.imu.reset_heading(0)
while True:
if hub.imu.ready():
break
run_task(main())
print("done")

BIN
pynamics-screenshot.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 253 KiB

62
templates/color_sensor_start_logic.py
View File

@@ -1,62 +0,0 @@
from pybricks.pupdevices import ColorSensor
from pybricks.parameters import Color, Port
from pybricks.tools import run_task
from pybricks.tools import wait
from pybricks.hubs import PrimeHub
hub = PrimeHub()
color_sensor = ColorSensor(Port.F) # Change the port to your color sensor's port
# Function to classify color based on HSV
def detect_color(h, s, v, reflected):
if reflected > 4:
if h < 4 or h > 350: # red
return "Red"
elif 3 < h < 40 and s > 70: # orange
return "Orange"
elif 47 < h < 56: # yellow
return "Yellow"
elif 70 < h < 160: # green - your brick should approach from the top for accuracy
return "Green"
elif 195 < h < 198: # light blue
return "Light_Blue"
elif 210 < h < 225: # blue - your brick should approach from the top for accuracy
return "Blue"
elif 260 < h < 350: # purple
return "Purple"
else:
return "Unknown"
return "Unknown"
async def main():
while True:
h, s, v = await color_sensor.hsv()
reflected = await color_sensor.reflection()
color = detect_color(h, s, v, reflected)
if color == "Green":
print('Running Task 1')
# Run a function with await Function() here
elif color == "Red":
print('Running Task 2')
# Run a function with await Function() here
elif color == "Yellow":
print('Running Task 3')
# Run a function with await Function() here
elif color == "Blue":
print('Running Task 4')
# Run a function with await Function() here
elif color == "Orange":
print('Running Task 5')
# Run a function with await Function() here
elif color == "Purple":
print('Running Task 6')
# Run a function with await Function() here
elif color == "Light_Blue":
print("Running Task 7")
# Run a function with await Function() here
else:
print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
#pass
await wait(10)
# Run the main function
run_task(main())

44
templates/logger.py
View File

@@ -1,44 +0,0 @@
from pybricks.tools import StopWatch
class Logger:
def __init__(self, verboseness=7):
self.time = StopWatch()
self.time.pause()
self.verboseness = verboseness
self.lvldict = {
0: "FATAL",
1: "ALERT",
2: "CRIT",
3: "ERR",
4: "WARNING",
5: "NOTICE",
6: "INFO",
7: "DEBUG"
}
def start(self):
self.time.reset()
self.time.resume()
def log(self, message, level, origin):
if level <= self.verboseness:
ms = self.time.time()
timestamp = "{:02d}:{:02d}.{:03d}".format(
(ms // 60000) % 60,
(ms // 1000) % 60,
ms % 1000
)
label = self.lvldict.get(level, "UNKNOWN")
padding = " " * (7 - len(label))
print("[{}] {}{} [{}] {}".format(timestamp, label, padding, origin, message))
def fatal(self, message, origin): self.log(message, 0, origin)
def alert(self, message, origin): self.log(message, 1, origin)
def crit(self, message, origin): self.log(message, 2, origin)
def err(self, message, origin): self.log(message, 3, origin)
def warning(self, message, origin): self.log(message, 4, origin)
def notice(self, message, origin): self.log(message, 5, origin)
def info(self, message, origin): self.log(message, 6, origin)
def debug(self, message, origin): self.log(message, 7, origin)
def crash(self, message, origin):
self.log(message, 0, origin)
raise FatalLoggerError("[FATAL] [{}] {}".format(origin, message))

9
templates/xbox_controller.py
View File

@@ -1,9 +0,0 @@
from pybricks.iodevices import XboxController
from pybricks.parameters import Direction, Port
from pybricks.tools import wait
xbox = XboxController()
while True:
print("Xbox left joystick x-position:", xbox.joystick_left()[0])
wait(50)

74
tests/pynamics-logger.py
View File

@@ -1,74 +0,0 @@
# Event codes:
#0: notify
#1: hfnotify
#2: prgm_start
#3: prgm_end
#4: prgm_crash
#5: snsr_data
#6: mtr_data
#7: perf_smpl
#8: get_time
#9: breakpoint
class PynamicsLogger:
def __init__(self):
self.verboseness = 7
self.lvldict = {
0: "FATAL",
1: "ALERT",
2: "CRIT",
3: "ERR",
4: "WARNING",
5: "NOTICE",
6: "INFO",
7: "DEBUG"
}
self.time = StopWatch()
self.time.pause()
def start():
self.time.reset()
self.time.resume()
def log(self, message, level, origin):
if level <= self.verboseness:
ms = self.time.time()
timestamp = "{:02d}:{:02d}.{:03d}".format(
(ms // 60000) % 60,
(ms // 1000) % 60,
ms % 1000
)
label = self.lvldict[level]
padding = " " * (7 - len(label))
print(f"[{timestamp}] [{label}]{padding} [{origin}] {message}")
def sendCommand(self, eventnum, level, msg, origin):
print(f"\x1b[?PYN;{str(eventnum)};{lvldict[level]};{msg}~")
def notify(level, msg, origin):
self.sendCommand(self, 0, level, msg, origin)
def notifyfatal(self, message, origin): notify(message, 0, origin)
def notifyalert(self, message, origin): notify(message, 1, origin)
def notifycrit(self, message, origin): notify(message, 2, origin)
def notifyerr(self, message, origin): notify(message, 3, origin)
def notifywarning(self, message, origin): notify(message, 4, origin)
def notifynotice(self, message, origin): notify(message, 5, origin)
def notifyinfo(self, message, origin): notify(message, 6, origin)
def notifydebug(self, message, origin): notify(message, 7, origin)
def notifycrash(self, message, origin):
sendCommand(4, 0, "uhhhh so the program kinda crashed sorry", origin)
raise FatalLoggerError("[FATAL] [{}] {}".format(origin, message))
def breakpoint(self, message, origin):
sendCommand(3, 8, "currently everything about the robot")
def testAll(self):
self.start()
notifyfatal("hey you should probably know that your RAM is corrupted and you cant replace it because of the shortage. byeeeee", "test prgm")
notifyalert(0, 1, "UNRECOVERABLE PYTHON ERROR!!!!! Exiting gracefully... JUST KIDDING WHAT DID YOU THINK IT WAS GONNA DO", "test prgm")
notifycrit(0, 2, "so the program is running but it just started looping on the motor frying bit of your program.", "test prgm")
notifyerr(0, 3, "syntax error, you failure!!!! use an error checker", "test prgm")
notifywarning(0, 4, "wanted to just say that your motor is kinda being slow", "test prgm")
notifynotice(0, 5, "so your motor is like 1% slower than it should be but really no one cares", "test prgm")
notifyinfo(0, 6, "hey everything in the program is going well just in case nothing in your life is", "test prgm")
notifydebug(0, 7, "nobody cares about me but you should know that", "test prgm")
if __name__ == "__main__":
pynlogger = PynamicsLogger()
pynlogger.testAll()

25
tests/test-allow-missing-uart.py
View File

@@ -1,25 +0,0 @@
from pybricks.iodevices import UARTDevice as _UARTDevice
from pybricks.tools import wait
from uerrno import ETIMEDOUT
class FakeUART:
def __init__(self, port, baudrate, timeout):
self.timeout = timeout
print("Warning: No physical UART detected. Using simulator.")
def read(self, length=1):
if self.timeout is not None:
wait(self.timeout)
raise OSError(ETIMEDOUT)
else:
while True:
wait(1000)
def write(self, data):
pass
def UARTDevice(port, baudrate=9600, timeout=None):
try:
return _UARTDevice(port, baudrate, timeout)
except OSError:
return FakeUART(port, baudrate, timeout)

10
tests/testgc.py
View File

@@ -1,10 +0,0 @@
input("gc")
try:
import gc
except Exception as ex:
print(ex.errno)
input("ugc")
try:
import ugc
except Exception as ex:
print(ex.errno)

34
diagnostics/battery_diagnostics.py → utils/BatteryDiagnostics.py
View File

@@ -1,18 +1,24 @@
from pybricks.tools import wait
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()
class BatteryDiagnostics:
def __init__(self, hub):
def __init__(self):
self.voltage = 0
self.current = 0
self.hub = hub
def printVoltage(self):
self.voltage = self.hub.battery.voltage()
self.voltage = 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()
self.current = hub.battery.current()
print("Battery current:", self.current)
def printAll(self):
timeelapsed = 0
@@ -29,21 +35,17 @@ class BatteryDiagnostics:
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
data = vals
# 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)
# Calculate the standard deviation (square root of the variance)
STD_DEV_MANUAL = umath.sqrt(VARIANCE)
variance = sum([(x - mean) ** 2 for x in data]) / (len(data) - 1)
return (STD_DEV_MANUAL)
# Calculate the standard deviation (square root of the variance)
std_dev_manual = umath.sqrt(variance)
return (std_dev_manual)

69
utils/ColorSensorTests-old.py Normal file
View File

@@ -0,0 +1,69 @@
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.tools import run_task, multitask
from pybricks.tools import wait, StopWatch
from pybricks.robotics import DriveBase
from pybricks.hubs import PrimeHub
# Initialize hub and devices
hub = PrimeHub()
color_sensor = ColorSensor(Port.F)
# Color Settings
# https://docs.pybricks.com/en/latest/parameters/color.html
print("Default Detected Colors:", color_sensor.detectable_colors())
# Custom color Hue, Saturation, Brightness value for Lego bricks
Color.MAGENTA = Color(315,100,60)
Color.BLUE = Color(240,100,100)
Color.CYAN = Color(180,100,100)
Color.RED = Color(350, 100, 100)
LEGO_BRICKS_COLOR = [
Color.BLUE,
Color.GREEN,
Color.WHITE,
Color.RED,
Color.YELLOW,
Color.MAGENTA,
Color.NONE
]
magenta_counter = 0
stable_color = None
real_color = None
#Update Detectable colors
color_sensor.detectable_colors(LEGO_BRICKS_COLOR)
print(f'Yellow:{Color.YELLOW} : {Color.YELLOW.h}, {Color.YELLOW.s}, {Color.YELLOW.v}')
print("Updated Detected Colors:", color_sensor.detectable_colors())
async def main():
while True:
global magenta_counter, stable_color, real_color
color_reflected_percent = await color_sensor.reflection()
print("Reflection: ", color_reflected_percent)
if color_reflected_percent > 15:
color_detected = await color_sensor.color()
if color_detected == Color.MAGENTA:
magenta_counter += 1
else:
magenta_counter = 0
stable_color = color_detected
# Only accept magenta if it's been stable for a while - usually triggers before other colors so we gotta do this :|
if magenta_counter > 10:
stable_color = Color.MAGENTA
if stable_color != Color.MAGENTA:
stable_color = await color_sensor.color()
real_color = stable_color
#if(color_detected != Color.NONE):
# return
print("Magenta counter: ", magenta_counter)
if real_color is not None:
print(f'Detected color: {real_color} : {real_color.h}, {real_color.s}, {real_color.v}')
else:
print("No valid color detected yet.")
await wait(50)
run_task(main())

35
utils/FullDiagnostics.py Normal file
View File

@@ -0,0 +1,35 @@
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
battery = BatteryDiagnostics()
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 == ""):
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")
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")
choice = input("Your choice: ").strip().lower()
if choice == "b":
print("-----------------------BATTERY DIAGNOSTICS-----------------------")
battery.printAll()
elif choice == "m":
print("------------------------MOTOR DIAGNOSTICS------------------------")
# motor.printAll() # call your motor diagnostics here
print("Motor diagnostics would run here.")
elif choice == "q":
print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!")
break
else:
print("Invalid choice. Please enter 'b', 'm', or 'q'.")