utils/MotorDiagnostics.py

@@ -40,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
+    # Normalize load: map 10–20 as baseline (≈0%), 200 as max (≈100%)
-    load_pct = (avg_load / max_torque) * 100 if max_torque else 0
+    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
@@ -58,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:
+      try:
-        self.testmotor = Motor(self.port_map[motorinput])
+        # Only create a new Motor if we don't already have one
-        print(f"Motor initialized on port {motorinput}.")
+        if self.testmotor is None:
-        break  # exit the loop once a valid port is entered
+            self.testmotor = Motor(self.port_map[motorinput])
-      else:
+            print(f"Motor initialized on port {motorinput}.")
-        print("Invalid port. Please enter A, B, C, D, or E, or F.")
+        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)
@@ -109,7 +128,7 @@ class MotorDiagnostics:
     test180 = self.testSpeed(180)
     test540 = self.testSpeed(540)
     test1000 = self.testSpeed(1000)
-    print("FINAL MOTOR STATISTICS")
+    print("\n FINAL MOTOR STATISTICS")
     final = (test180 + test540 + test1000) / 3
     print("Final motor health score:", str(final) + "%")
     if final < 80:
@@ -120,4 +139,4 @@ class MotorDiagnostics:
       print("Your motor is in great condition!")
     else:
       print("Your motor is in AMAZING condition!!!")
-    
+    self.testmotor.stop()