Add diagnostics/ColorSensorDiagnostics.py

README.md

@@ -1,10 +1,10 @@
-# Team 65266 Lego Dynamics - Pybricks Utils
+# Team 65266 Lego Dynamics - PYNAMICS - Pybricks Utilities
 
 A collection of Pybricks utilities to assist in your FLL robot programming with Python. Created by FLL team 65266, Lego Dynamics.
 
-<img src="https://codes.fll-65266.org/Arcmyx/pynamics-pybricks-utils/raw/branch/arcmyx-dev/pynamics-screenshot.png" alt="Pynamics screenshot" width="670">
+<img src="https://codes.fll-65266.org/Arcmyx/pynamics-pybricks-utils/raw/branch/main/pynamics-screenshot.png" alt="Pynamics screenshot" width="670">
 
-How to use this
+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.
 

diagnostics/ColorSensorDiagnostics.py

@@ -0,0 +1,49 @@
+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
+
+hub = PrimeHub()
+class ColorSensorDiagnostics:
+    def __init__(self):
+        self.colorsensor = None
+        self.port_map = {
+            "A": Port.A,
+            "B": Port.B,
+            "C": Port.C,
+            "D": Port.D,
+            "E": Port.E,
+            "F": Port.F,
+        }
+    def initializeColorSensor(self):
+        valid_ports = {"A", "B", "C", "D", "E", "F"}
+        while True:
+            colorinput = input(
+            "This will test your color sensor.\n"
+            "Enter the port for the color sensor you would like to test (A, B, C, D, E, or F): "
+            ).strip().upper()
+            if colorinput not in valid_ports:
+                print("Invalid port. Please enter A-F.")
+                continue
+            try:
+                if self.colorsensor is None:
+                    self.colorsensor = ColorSensor(self.port_map[colorinput])
+                    print(f"Color Sensor initialized on port {colorinput}.")
+                else:
+                    print(f"Reusing existing color sensor on port {colorinput}.")
+                break
+
+            except OSError as e:
+                if e.errno == 16:  # EBUSY
+                    print(f"Port {colorinput} is already in use. Reusing existing color sensor.")
+                    break
+                else:
+                    print(f"Error initializing color sensor on port {colorinput}: {e}")
+                    print("Make sure a color sensor is actually connected to this port.")
+                    self.colorsensor = None
+        self.colorsensor.detectable_colors(Color.RED, Color.YELLOW, Color.GREEN, Color.BLUE, Color.WHITE, Color.NONE)
+    def printOutput(self):
+        while True:
+            print("HSV output:", self.colorsensor.hsv)
+            print("Detected color:", self.colorsensor.color())

diagnostics/MotorDiagnostics.py

@@ -41,7 +41,7 @@ class MotorDiagnostics:
     # Stability: penalize deviation relative to desired
     stability = max(0, 100 - (stdev_speed / desired) * 100)
 
-    # Normalize load: map 10–20 as baseline (≈0%), 200 as max (≈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)