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/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/FullDiagnostics.py b/utils/FullDiagnostics.py index af8a6b0..af9737c 100644 --- a/utils/FullDiagnostics.py +++ b/utils/FullDiagnostics.py @@ -5,12 +5,32 @@ from pybricks.robotics import DriveBase from pybricks.tools import wait, StopWatch hub = PrimeHub() from BatteryDiagnostics import BatteryDiagnostics +from MotorDiagnostics import MotorDiagnostics 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") +motor = MotorDiagnostics() +print(""" + ███████████ █████ █████ ██████ █████ █████████ ██████ ██████ █████ █████████ █████████ +▒▒███▒▒▒▒▒███▒▒███ ▒▒███ ▒▒██████ ▒▒███ ███▒▒▒▒▒███ ▒▒██████ ██████ ▒▒███ ███▒▒▒▒▒███ ███▒▒▒▒▒███ + ▒███ ▒███ ▒▒███ ███ ▒███▒███ ▒███ ▒███ ▒███ ▒███▒█████▒███ ▒███ ███ ▒▒▒ ▒███ ▒▒▒ + ▒██████████ ▒▒█████ ▒███▒▒███▒███ ▒███████████ ▒███▒▒███ ▒███ ▒███ ▒███ ▒▒█████████ + ▒███▒▒▒▒▒▒ ▒▒███ ▒███ ▒▒██████ ▒███▒▒▒▒▒███ ▒███ ▒▒▒ ▒███ ▒███ ▒███ ▒▒▒▒▒▒▒▒███ + ▒███ ▒███ ▒███ ▒▒█████ ▒███ ▒███ ▒███ ▒███ ▒███ ▒▒███ ███ ███ ▒███ + █████ █████ █████ ▒▒█████ █████ █████ █████ █████ █████ ▒▒█████████ ▒▒█████████ +▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ ▒▒▒▒▒▒▒▒▒ + 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") @@ -20,12 +40,15 @@ 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() + print("Battery diagnostics completed.") 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