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

Reviewed-on: #9

README.md

@@ -2,7 +2,9 @@
 
 A collection of Pybricks utilities to assist in your FLL robot programming with Python. Created by FLL team 65266, Lego Dynamics.
 
-How to use this:
+<img src="https://codes.fll-65266.org/Arcmyx/pynamics-pybricks-utils/raw/branch/arcmyx-dev/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/pybricks-utils.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.
 

utils/BatteryDiagnostics.py

@@ -35,6 +35,7 @@ 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):

utils/FullDiagnostics.py

@@ -8,6 +8,9 @@ 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")
 print("""
  ███████████  █████ █████ ██████   █████   █████████   ██████   ██████ █████   █████████   █████████ 
 ▒▒███▒▒▒▒▒███▒▒███ ▒▒███ ▒▒██████ ▒▒███   ███▒▒▒▒▒███ ▒▒██████ ██████ ▒▒███   ███▒▒▒▒▒███ ███▒▒▒▒▒███
@@ -17,20 +20,13 @@ print("""
  ▒███            ▒███     ▒███  ▒▒█████  ▒███    ▒███  ▒███      ▒███  ▒███ ▒▒███     ███ ███    ▒███
  █████           █████    █████  ▒▒█████ █████   █████ █████     █████ █████ ▒▒█████████ ▒▒█████████ 
 ▒▒▒▒▒           ▒▒▒▒▒    ▒▒▒▒▒    ▒▒▒▒▒ ▒▒▒▒▒   ▒▒▒▒▒ ▒▒▒▒▒     ▒▒▒▒▒ ▒▒▒▒▒   ▒▒▒▒▒▒▒▒▒   ▒▒▒▒▒▒▒▒▒  
-
- The free & open-source diagnostics tool for LEGO® Education SPIKE™ Prime
- Designed for FIRST LEGO League teams
- Developed by Team 65266 — Lego Dynamics
-                                      
+                                                                                                     
+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.                                                
           """
           )
-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")

utils/MotorDiagnostics.py

@@ -9,6 +9,15 @@ hub = PrimeHub()
 class MotorDiagnostics:
   def __init__(self):
       self.testmotor = None
+      self.port_map = {
+        "A": Port.A,
+        "B": Port.B,
+        "C": Port.C,
+        "D": Port.D,
+        "E": Port.E,
+        "F": Port.F,
+      }
+
   def stdev(self, vals):
     data = vals
     if len(data) < 2:
@@ -31,11 +40,18 @@ class MotorDiagnostics:
 
     # 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
+
+    # Normalize load: map 10–20 as baseline (≈0%), 200 as max (≈100%)
+    baseline = 15      # 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
+
 
     # Final score: average of the three
     return (accuracy + stability + load_score) / 3
@@ -49,12 +65,24 @@ class MotorDiagnostics:
         "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.")
+      try:
+        # Only create a new Motor if we don't already have one
+        if self.testmotor is None:
+            self.testmotor = Motor(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)
@@ -62,40 +90,53 @@ class MotorDiagnostics:
     motorspeeds = []
     motorloads = []
     target_angle = speed * 3
-
+    print("\n", speed, "DEGREES PER SECOND TEST")
     self.testmotor.run_angle(speed, target_angle, Stop.HOLD, False)
-    while abs(self.testmotor.speed()) > 5:
+    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("Maximum motor speed (deg/s):", max_speed)
 
-    print("Desired motor speed (degrees per second): ", str(speed))
+
+    print("Desired motor speed: ", 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)))
+      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 (mNm):", avgload)
-      print("Motor load deviation (this measures how much your motor load deviates from the average load):", str(self.stdev(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:", score)
+    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:", final)
-    # print final health scores here as a combination of the 3 speeds, include speeds individual scores and total score
+    print("Final motor health score:", str(final) + "%")
+    if final < 80:
+      print("Your motor is in need of attention. Make sure to clean it regularly and charge the Prime Hub.")
+    elif final < 90:
+      print("Your motor is in OK condition. Make sure to clean it regularly and charge the Prime Hub.")
+    elif final < 97:
+      print("Your motor is in great condition!")
+    else:
+      print("Your motor is in AMAZING condition!!!")
+    self.testmotor.stop()