diff --git a/utils/MotorDiagnostics.py b/utils/MotorDiagnostics.py index e8cc945..8fdb3bc 100644 --- a/utils/MotorDiagnostics.py +++ b/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 - 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 @@ -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: - self.testmotor = Motor(self.port_map[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) @@ -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!!!") - \ No newline at end of file + self.testmotor.stop() \ No newline at end of file