From e4e9ef988bcac113bc24c2ab478cf4ec143975e2 Mon Sep 17 00:00:00 2001 From: alkadienePhoton Date: Sun, 7 Dec 2025 16:57:28 -0600 Subject: [PATCH 1/6] Enhance Battery and Motor Diagnostics: Add standard deviation calculation and motor diagnostics functionality --- utils/BatteryDiagnostics.py | 9 ++-- utils/FullDiagnostics.py | 8 ++- utils/MotorDiagnostics.py | 101 ++++++++++++++++++++++++++++++++++++ 3 files changed, 113 insertions(+), 5 deletions(-) create mode 100644 utils/MotorDiagnostics.py diff --git a/utils/BatteryDiagnostics.py b/utils/BatteryDiagnostics.py index db04b4a..cd55e36 100644 --- a/utils/BatteryDiagnostics.py +++ b/utils/BatteryDiagnostics.py @@ -39,13 +39,16 @@ class BatteryDiagnostics: 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]) / (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) + + return (std_dev_manual) \ No newline at end of file diff --git a/utils/FullDiagnostics.py b/utils/FullDiagnostics.py index af8a6b0..3c4b079 100644 --- a/utils/FullDiagnostics.py +++ b/utils/FullDiagnostics.py @@ -5,7 +5,9 @@ from pybricks.robotics import DriveBase from pybricks.tools import wait, StopWatch 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 == ""): 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") @@ -20,12 +22,14 @@ while True: 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.") + input("Press Enter to begin the battery diagnostics.") battery.printAll() elif choice == "m": print("------------------------MOTOR DIAGNOSTICS------------------------") - # motor.printAll() # call your motor diagnostics here - print("Motor diagnostics would run here.") + motor.fullTest() + print("Motor diagnostics completed.") elif choice == "q": print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!") diff --git a/utils/MotorDiagnostics.py b/utils/MotorDiagnostics.py new file mode 100644 index 0000000..f4aad24 --- /dev/null +++ b/utils/MotorDiagnostics.py @@ -0,0 +1,101 @@ +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 +import umath + +hub = PrimeHub() +class MotorDiagnostics: + def __init__(self): + self.testmotor = None + 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) + max_speed, max_accel, max_torque = self.testmotor.control.limits() + # Load: penalize load percentage directly + load_pct = (avg_load / max_torque) * 100 if max_torque else 0 + 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 in valid_ports: + self.testmotor = Motor(Port[motorinput]) + print(f"Motor initialized on port {motorinput}.") + break # exit the loop once a valid port is entered + else: + print("Invalid port. Please enter A, B, C, D, or E, or F.") + + def testSpeed(self, speed): + self.testmotor.reset_angle(0) + + motorspeeds = [] + motorloads = [] + target_angle = speed * 3 + + self.testmotor.run_angle(speed, target_angle, Stop.HOLD, False) + while abs(self.testmotor.speed()) > 5: + wait(20) + motorspeeds.append(self.testmotor.speed()) + motorloads.append(self.testmotor.load()) + + max_speed, max_accel, max_torque = self.testmotor.control.limits() + print("Maximum motor speed (deg/s):", max_speed) + + print("Desired motor speed (degrees per second): ", str(speed)) + if motorspeeds: + avg = sum(motorspeeds) / len(motorspeeds) + print("Average motor speed (degrees per second):", avg) + print("Motor speed deviation (this measures how much your motor deviates from the average speed):", str(self.stdev(motorspeeds))) + else: + print("No speed samples collected.") + avg = 0 + if motorloads: + avgload = sum(motorloads) / len(motorloads) + print("Average motor load (mNm):", avgload) + print("Motor load deviation (this measures how much your motor load deviates from the average load):", 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:", score) + return score + def fullTest(self): + self.initializeMotor() + test180 = self.testSpeed(180) + test540 = self.testSpeed(540) + test1000 = self.testSpeed(1000) + final = (test180 + test540 + test1000) / 3 + print("Final motor health score:", final) + # print final health scores here as a combination of the 3 speeds, include speeds individual scores and total score \ No newline at end of file From 086bd8841d51951000d6babc1da8594fde3b00bf Mon Sep 17 00:00:00 2001 From: alkadienePhoton Date: Sun, 7 Dec 2025 17:01:18 -0600 Subject: [PATCH 2/6] Fixed up UI --- utils/FullDiagnostics.py | 1 + 1 file changed, 1 insertion(+) diff --git a/utils/FullDiagnostics.py b/utils/FullDiagnostics.py index 3c4b079..e203ee4 100644 --- a/utils/FullDiagnostics.py +++ b/utils/FullDiagnostics.py @@ -25,6 +25,7 @@ while True: 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.") input("Press Enter to begin the battery diagnostics.") battery.printAll() + print("Battery diagnostics completed.") elif choice == "m": print("------------------------MOTOR DIAGNOSTICS------------------------") From 969f245de7ff5685dcccf08be8cebf339f61af7e Mon Sep 17 00:00:00 2001 From: alkadienePhoton Date: Sun, 7 Dec 2025 17:16:55 -0600 Subject: [PATCH 3/6] Added ASCII art --- utils/FullDiagnostics.py | 14 ++++++++++++++ utils/ascii.py | 14 ++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 utils/ascii.py diff --git a/utils/FullDiagnostics.py b/utils/FullDiagnostics.py index e203ee4..6026e4e 100644 --- a/utils/FullDiagnostics.py +++ b/utils/FullDiagnostics.py @@ -13,6 +13,20 @@ if(clearConfirmation == "Y" or clearConfirmation == "y" 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(""" + ███████████ █████ █████ ██████ █████ █████████ ██████ ██████ █████ █████████ █████████ +▒▒███▒▒▒▒▒███▒▒███ ▒▒███ ▒▒██████ ▒▒███ ███▒▒▒▒▒███ ▒▒██████ ██████ ▒▒███ ███▒▒▒▒▒███ ███▒▒▒▒▒███ + ▒███ ▒███ ▒▒███ ███ ▒███▒███ ▒███ ▒███ ▒███ ▒███▒█████▒███ ▒███ ███ ▒▒▒ ▒███ ▒▒▒ + ▒██████████ ▒▒█████ ▒███▒▒███▒███ ▒███████████ ▒███▒▒███ ▒███ ▒███ ▒███ ▒▒█████████ + ▒███▒▒▒▒▒▒ ▒▒███ ▒███ ▒▒██████ ▒███▒▒▒▒▒███ ▒███ ▒▒▒ ▒███ ▒███ ▒███ ▒▒▒▒▒▒▒▒███ + ▒███ ▒███ ▒███ ▒▒█████ ▒███ ▒███ ▒███ ▒███ ▒███ ▒▒███ ███ ███ ▒███ + █████ █████ █████ ▒▒█████ █████ █████ █████ █████ █████ ▒▒█████████ ▒▒█████████ +▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ + + + + """ + ) print("\nWhat diagnostic do you want to perform?") print("Enter 'b' for Battery diagnostics") print("Enter 'm' for Motor diagnostics") diff --git a/utils/ascii.py b/utils/ascii.py new file mode 100644 index 0000000..84115d7 --- /dev/null +++ b/utils/ascii.py @@ -0,0 +1,14 @@ +print(""" + ███████████ █████ █████ ██████ █████ █████████ ██████ ██████ █████ █████████ █████████ +▒▒███▒▒▒▒▒███▒▒███ ▒▒███ ▒▒██████ ▒▒███ ███▒▒▒▒▒███ ▒▒██████ ██████ ▒▒███ ███▒▒▒▒▒███ ███▒▒▒▒▒███ + ▒███ ▒███ ▒▒███ ███ ▒███▒███ ▒███ ▒███ ▒███ ▒███▒█████▒███ ▒███ ███ ▒▒▒ ▒███ ▒▒▒ + ▒██████████ ▒▒█████ ▒███▒▒███▒███ ▒███████████ ▒███▒▒███ ▒███ ▒███ ▒███ ▒▒█████████ + ▒███▒▒▒▒▒▒ ▒▒███ ▒███ ▒▒██████ ▒███▒▒▒▒▒███ ▒███ ▒▒▒ ▒███ ▒███ ▒███ ▒▒▒▒▒▒▒▒███ + ▒███ ▒███ ▒███ ▒▒█████ ▒███ ▒███ ▒███ ▒███ ▒███ ▒▒███ ███ ███ ▒███ + █████ █████ █████ ▒▒█████ █████ █████ █████ █████ █████ ▒▒█████████ ▒▒█████████ +▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ + + + + """ + ) \ No newline at end of file From d92d401e2aa392d3ab53f43840396d03b3d8c17b Mon Sep 17 00:00:00 2001 From: alkadienePhoton Date: Sun, 7 Dec 2025 17:25:17 -0600 Subject: [PATCH 4/6] Deleted useless stuff --- utils/ColorSensorTests-old.py | 69 ----------------------------------- utils/ascii.py | 14 ------- 2 files changed, 83 deletions(-) delete mode 100644 utils/ColorSensorTests-old.py delete mode 100644 utils/ascii.py diff --git a/utils/ColorSensorTests-old.py b/utils/ColorSensorTests-old.py deleted file mode 100644 index 3d5a8d9..0000000 --- a/utils/ColorSensorTests-old.py +++ /dev/null @@ -1,69 +0,0 @@ -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()) \ No newline at end of file diff --git a/utils/ascii.py b/utils/ascii.py deleted file mode 100644 index 84115d7..0000000 --- a/utils/ascii.py +++ /dev/null @@ -1,14 +0,0 @@ -print(""" - ███████████ █████ █████ ██████ █████ █████████ ██████ ██████ █████ █████████ █████████ -▒▒███▒▒▒▒▒███▒▒███ ▒▒███ ▒▒██████ ▒▒███ ███▒▒▒▒▒███ ▒▒██████ ██████ ▒▒███ ███▒▒▒▒▒███ ███▒▒▒▒▒███ - ▒███ ▒███ ▒▒███ ███ ▒███▒███ ▒███ ▒███ ▒███ ▒███▒█████▒███ ▒███ ███ ▒▒▒ ▒███ ▒▒▒ - ▒██████████ ▒▒█████ ▒███▒▒███▒███ ▒███████████ ▒███▒▒███ ▒███ ▒███ ▒███ ▒▒█████████ - ▒███▒▒▒▒▒▒ ▒▒███ ▒███ ▒▒██████ ▒███▒▒▒▒▒███ ▒███ ▒▒▒ ▒███ ▒███ ▒███ ▒▒▒▒▒▒▒▒███ - ▒███ ▒███ ▒███ ▒▒█████ ▒███ ▒███ ▒███ ▒███ ▒███ ▒▒███ ███ ███ ▒███ - █████ █████ █████ ▒▒█████ █████ █████ █████ █████ █████ ▒▒█████████ ▒▒█████████ -▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ - - - - """ - ) \ No newline at end of file From bcf128310d70d9c2413b60e8800b8a3169dcf968 Mon Sep 17 00:00:00 2001 From: alkadienePhoton Date: Sun, 7 Dec 2025 17:37:22 -0600 Subject: [PATCH 5/6] Add ASCII art and project description and credits to FullDiagnostics.py --- utils/FullDiagnostics.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/utils/FullDiagnostics.py b/utils/FullDiagnostics.py index 6026e4e..9d63d63 100644 --- a/utils/FullDiagnostics.py +++ b/utils/FullDiagnostics.py @@ -23,8 +23,8 @@ while True: █████ █████ █████ ▒▒█████ █████ █████ █████ █████ █████ ▒▒█████████ ▒▒█████████ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ - - +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. """ ) print("\nWhat diagnostic do you want to perform?") From 28375e87a8ec6f07b335218f26e324e754dc4808 Mon Sep 17 00:00:00 2001 From: Atharv Nagavarapu <30nagava@elmbrookstudents.org> Date: Mon, 8 Dec 2025 13:48:04 +0000 Subject: [PATCH 6/6] Update utils/FullDiagnostics.py --- utils/FullDiagnostics.py | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/utils/FullDiagnostics.py b/utils/FullDiagnostics.py index 9d63d63..af9737c 100644 --- a/utils/FullDiagnostics.py +++ b/utils/FullDiagnostics.py @@ -8,12 +8,7 @@ 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 == ""): - 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(""" +print(""" ███████████ █████ █████ ██████ █████ █████████ ██████ ██████ █████ █████████ █████████ ▒▒███▒▒▒▒▒███▒▒███ ▒▒███ ▒▒██████ ▒▒███ ███▒▒▒▒▒███ ▒▒██████ ██████ ▒▒███ ███▒▒▒▒▒███ ███▒▒▒▒▒███ ▒███ ▒███ ▒▒███ ███ ▒███▒███ ▒███ ▒███ ▒███ ▒███▒█████▒███ ▒███ ███ ▒▒▒ ▒███ ▒▒▒ @@ -22,11 +17,20 @@ while True: ▒███ ▒███ ▒███ ▒▒█████ ▒███ ▒███ ▒███ ▒███ ▒███ ▒▒███ ███ ███ ▒███ █████ █████ █████ ▒▒█████ █████ █████ █████ █████ █████ ▒▒█████████ ▒▒█████████ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ - -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. + + The free & open-source diagnostics tool for LEGO® Education SPIKE™ Prime + Designed for FIRST LEGO League teams + Developed by Team 65266 — Lego Dynamics + """ ) +clearConfirmation = input("Do you want to clear the console before proceeding? Y/N (default: yes): ") +yeses = ["Y", "YES", "Yes", "yes", ""] +if(clearConfirmation in yeses): + 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")