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

Reviewed-on: #15

README.md

@@ -10,7 +10,7 @@ How to use this:
 
 Included utilities:
 
-- Diagnostics - a program that allows you to diagnose issues and test parts of your robot, such as battery, motor, and color sensor. Open each program in the ```diagnostics``` folder in Pybricks, connect your robot, switch to the ```FullDiagnostics.py``` file and press run.
+- Diagnostics - a program that allows you to diagnose issues and test parts of your robot, such as battery, motor, and color sensor. Open each program in the ```diagnostics``` folder in Pybricks, (you can select all of them at once) connect your robot, switch to the ```FullDiagnostics.py``` file and press run.
 - Color Sensor Tests (UPCOMING) - a program that identifies what color the sensor is detecting. If you'd like, you can use our color ranges in your own programs.
 
 Please set your window size to 90% on small screens for best results with the ASCII art.               

diagnostics/ColorSensorDiagnostics.py

@@ -4,7 +4,6 @@ from pybricks.tools import wait, StopWatch
 class ColorSensorDiagnostics:
     def __init__(self, hub, colorsensorclass):
         self.colorsensor = None
-        self.colorsensorclass = colorsensorclass
         self.PORT_MAP = {
             "A": Port.A,
             "B": Port.B,
@@ -13,6 +12,7 @@ class ColorSensorDiagnostics:
             "E": Port.E,
             "F": Port.F,
         }
+        self.colorsensorclass = colorsensorclass
     def initializeColorSensor(self):
         VALID_PORTS = {"A", "B", "C", "D", "E", "F"}
         while True:

diagnostics/FullDiagnostics.py

@@ -12,8 +12,8 @@ from HubDiagnostics import HubDiagnostics
 battery = BatteryDiagnostics(HUB)
 motor = MotorDiagnostics(HUB, Motor)
 colorsensor = ColorSensorDiagnostics(HUB, ColorSensor)
-hubdiags = HubDiagnostics(HUB)
 drivebase = DriveBaseDiagnostics(HUB, Motor, DriveBase)
+hubdiagnostics = HubDiagnostics(HUB)
 CLEARCONFIRM = input("Clear the console before proceeding? Y/N (default: yes): ")
 if(CLEARCONFIRM == "Y" or CLEARCONFIRM == "y" or CLEARCONFIRM == "yes" or CLEARCONFIRM == ""):
     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")
@@ -27,9 +27,8 @@ print("""
  █████           █████    █████  ▒▒█████ █████   █████ █████     █████ █████ ▒▒█████████ ▒▒█████████ 
 ▒▒▒▒▒           ▒▒▒▒▒    ▒▒▒▒▒    ▒▒▒▒▒ ▒▒▒▒▒   ▒▒▒▒▒ ▒▒▒▒▒     ▒▒▒▒▒ ▒▒▒▒▒   ▒▒▒▒▒▒▒▒▒   ▒▒▒▒▒▒▒▒▒  
                                                                                                      
-The free and open source diagnostics tool for the LEGO® Education SPIKE™ Prime robots, designed for FIRST Lego League.
+The free and open source diagnostics tool for LEGO® Education SPIKE™ Prime robots, designed for FIRST Lego League.
 Developed by Team 65266, Lego Dynamics.                                                
-Please set your window size to 90% on small screens for best results with the ASCII art.               
           """
           )
 while True:
@@ -38,8 +37,8 @@ while True:
     print("Enter 'b' for battery diagnostics")
     print("Enter 'm' for motor diagnostics")
     print("Enter 'cs' for color sensor diagnostics")
-    print("Enter 'h' for hub diagnostics")
     print("Enter 'db' for drive base diagnostics")
+    print("Enter 'h' for hub diagnostics")
     print("Enter 'q' to quit")
 
     choice = input("Your choice: ").strip().lower()
@@ -54,21 +53,21 @@ while True:
     elif choice == "m":
         print("------------------------MOTOR DIAGNOSTICS------------------------")
         motor.fullTest()
-        print("Motor diagnostics completed.")
-    elif choice == "h":
-        print("-------------------------HUB DIAGNOSTICS-------------------------")
-        hubdiags.printAll()
-        print("Hub diagnostics completed.")
+        print("[Motor Diagnostics] Motor diagnostics completed.")
+
     elif choice == "q":
-        print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!")
+        print("Diagnostics completed successfully. Exiting program.")
         break
-    elif choice == "db":
-        print("----------------------DRIVEBASE DIAGNOSTICS----------------------")
-        drivebase.printAll()
     elif choice == "cs":
         print("---------------------COLOR SENSOR DIAGNOSTICS---------------------")
         colorsensor.printAll()
-        print("Color sensor diagnostics completed.")
-        
+        print("[Color Sensor Diagnostics] Color sensor diagnostics completed.")
+    elif choice == "db":
+        print("----------------------DRIVE BASE DIAGNOSTICS----------------------")
+        drivebase.printAll()
+        print("[Drivebase Diagnostics] Drivebase diagnostics completed.")
+    elif choice == "h":
+        print("--------------------------HUB DIAGNOSTICS--------------------------")
+        hubdiagnostics.printAll(False)
     else:
         print("Invalid choice. Please enter 'b', 'm', or 'q'.")

diagnostics/HubDiagnostics.py

@@ -1,10 +1,8 @@
 from pybricks.tools import wait, StopWatch
-from pybricks.parameters import Port
 from pybricks import version
 import OtherFunctions as debug
-import usys
-
-
+from MicroPythonDiagnostics import MicroPythonDiagnostics
+from pybricks.parameters import Port
 class HubDiagnostics:
     def __init__(self, hub):
         self.hub = hub
@@ -16,20 +14,28 @@ class HubDiagnostics:
             "E": Port.E,
             "F": Port.F,
         }
-    def printAbout(self):
-        print("Pybricks version information:", version)
-        print("MicroPython information:", usys.implementation)
-        print("MicroPython version:", usys.version)
     def testLightSources(self, verbose):
         v = verbose
         self.hub.display.off()
         for x in range(5):
             for y in range(5):
-                debug.log(f"Turning on pixel at position {x}, {y}...", v)
+                debug.log(f"[Hub Diagnostics - Light Sources] Turning on pixel at position {x}, {y}...", v)
                 self.hub.display.pixel(x, y, brightness=100)
                 wait(100)
-                debug.log(f"Turning off pixel at position {x}, {y}...", v)
+                debug.log(f"[Hub Diagnostics - Light Sources] Turning off pixel at position {x}, {y}...", v)
                 self.hub.display.pixel(x, y, brightness=0)
-    def printAll(self):
-        self.printAbout()
-        self.testLightSources(False)
+
+    def printAll(self, verbose=True):
+        v = verbose
+        debug.log("[Hub Diagnostics] Starting hub diagnostics...", v)
+        while True:
+            choice = input("[Hub Diagnostics] Which hub diagnostic would you like to run?\n[Hub Diagnostics] Enter 'l' for light source test\n[Hub Diagnostics] Enter 'm' for MicroPython diagnostics\n[Hub Diagnostics] Enter 'q' to quit\n[Hub Diagnostics] Your choice: ").strip().lower()
+            if choice == "l":
+                debug.log("[Hub Diagnostics] Running light source test...", v)
+                self.testLightSources(v)
+            if choice == "m":
+                debug.log("[Hub Diagnostics] Running MicroPython diagnostics...", v)
+                MicroPythonDiagnostics.printAll()
+            if choice == "q":
+                print("[Hub Diagnostics] Hub diagnostics completed.")
+                return

diagnostics/MicroPythonDiagnostics.py

@@ -0,0 +1,11 @@
+import usys
+from pybricks import version
+class MicroPythonDiagnostics:
+    def __init__(self, hub):
+        pass
+    def printAll():
+        print("[Hub Diagnostics - MicroPython] Hub version information:", version)
+        print("[Hub Diagnostics - MicroPython] MicroPython version:", usys.version)
+        print("[Hub Diagnostics - MicroPython] Pybricks version information:", usys.version_info)
+        print("[Hub Diagnostics - MicroPython] MicroPython information:", usys.implementation)
+        

diagnostics/MotorDiagnostics.py

@@ -4,7 +4,6 @@ import umath
 class MotorDiagnostics:
   def __init__(self, hub, motorclass):
       self.testmotor = None
-      self.motorclass = motorclass
       self.port_map = {
         "A": Port.A,
         "B": Port.B,
@@ -13,7 +12,7 @@ class MotorDiagnostics:
         "E": Port.E,
         "F": Port.F,
       }
-
+      self.motorclass = motorclass
   def stdev(self, vals):
     DATA = vals
     if len(DATA) < 2:
@@ -37,7 +36,7 @@ class MotorDiagnostics:
     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
+    BASELINE = 10      # 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)

templates/color-sensor-start-logic.py

@@ -0,0 +1,62 @@
+from pybricks.pupdevices import ColorSensor
+from pybricks.parameters import Color, Port
+from pybricks.tools import run_task
+from pybricks.tools import wait
+from pybricks.hubs import PrimeHub
+hub = PrimeHub()
+color_sensor = ColorSensor(Port.F) # Change the port to your color sensor's port
+# Function to classify color based on HSV
+def detect_color(h, s, v, reflected):
+    if reflected > 4:
+        if h < 4 or h > 350:  # red
+            return "Red"
+        elif 3 < h < 40 and s > 70:  # orange
+            return "Orange"
+        elif 47 < h < 56:  # yellow
+            return "Yellow"
+        elif 70 < h < 160:  # green - your brick should approach from the top for accuracy
+            return "Green"
+        elif 195 < h < 198:  # light blue
+            return "Light_Blue"
+        elif 210 < h < 225:  # blue - your brick should approach from the top for accuracy
+            return "Blue"
+        elif 260 < h < 350:  # purple
+            return "Purple"
+       
+        else:
+            return "Unknown"
+    return "Unknown"
+async def main():
+    while True:
+        h, s, v = await color_sensor.hsv()
+        reflected = await color_sensor.reflection()
+        color = detect_color(h, s, v, reflected)
+        
+
+        if color == "Green":
+            print('Running Task 1')
+            # Run a function with await Function() here
+        elif color == "Red":
+            print('Running Task 2')
+            # Run a function with await Function() here
+        elif color == "Yellow":
+            print('Running Task 3')
+            # Run a function with await Function() here
+        elif color == "Blue": 
+            print('Running Task 4')
+            # Run a function with await Function() here
+        elif color == "Orange":
+            print('Running Task 5')
+            # Run a function with await Function() here
+        elif color == "Purple":
+            print('Running Task 6')
+            # Run a function with await Function() here
+        elif color == "Light_Blue":
+            print("Running Task 7")
+            # Run a function with await Function() here
+        else:
+            print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
+            #pass
+        await wait(10)
+# Run the main function
+run_task(main())

templates/xboxcontroller.py

@@ -0,0 +1,18 @@
+from pybricks.pupdevices import Motor
+from pybricks.parameters import Button, Direction, Port, Side, Stop
+from pybricks.tools import run_task, multitask
+from pybricks.tools import wait, StopWatch
+from pybricks.robotics import DriveBase
+from pybricks.iodevices import XboxController
+from pybricks.hubs import PrimeHub
+hub = PrimeHub()
+testmotor = Motor(Port.C)
+async def main():
+    while True:
+        if(Button.UP in buttons.pressed()):
+            testmotor.run(500)
+        else:
+            testmotor.stop()
+        await wait(10)
+# Run the main function
+run_task(main())