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

Reviewed-on: Arcmyx/pybricks-utils#5

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)

utils/ColorSensorTests-old.py

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

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!")

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