Enhance Battery and Motor Diagnostics: Add standard deviation calculation and motor diagnostics functionality

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

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