diff --git a/diagnostics/MotorDiagnostics.py b/diagnostics/MotorDiagnostics.py
new file mode 100644
index 0000000..8cc879e
--- /dev/null
+++ b/diagnostics/MotorDiagnostics.py
@@ -0,0 +1,135 @@
+from pybricks.parameters import Direction, Port, Stop
+from pybricks.tools import wait, StopWatch
+import umath
+class MotorDiagnostics:
+  def __init__(self, hub, motorclass):
+      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:
+        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)
+
+    # Normalize load: map 10 to 20 as baseline (around 0%), 200 as max (around 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
+
+  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 not in VALID_PORTS:
+        print("That is not a valid port. Please enter A-F.")
+        continue
+      try:
+        # Only create a new Motor if we don't already have one
+        if self.testmotor is None:
+            self.testmotor = self.motorclass(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)
+    
+    motorspeeds = []
+    motorloads = []
+    TARGET_ANGLE = speed * 3
+    print("\n", speed, "DEGREES PER SECOND TEST")
+    self.testmotor.run_angle(speed, TARGET_ANGLE, Stop.HOLD, False)
+    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("Desired motor speed: ", str(speed))
+    if motorspeeds:
+      avg = sum(motorspeeds) / len(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:", 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:", 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:", str(final) + "%")
+    if final < 65:
+      print("Your motor is in need of attention. Make sure to clean it regularly and charge the Prime Hub.")
+    elif final < 85:
+      print("Your motor is in OK condition. Make sure to clean it regularly and charge the Prime Hub.")
+    elif final < 95:
+      print("Your motor is in great condition!")
+    else:
+      print("Your motor is in AMAZING condition!!!")
+    self.testmotor.stop()
\ No newline at end of file
diff --git a/diagnostics/OtherFunctions.py b/diagnostics/OtherFunctions.py
new file mode 100644
index 0000000..c3290a5
--- /dev/null
+++ b/diagnostics/OtherFunctions.py
@@ -0,0 +1,2 @@
+def vprint(string, verbose):
+    print("[LOG (verbose)]", string)
\ No newline at end of file