Update diagnostics/color_sensor_diagnostics.py

diagnostics/HubDiagnostics.py

@@ -1,35 +0,0 @@
-from pybricks.tools import wait, StopWatch
-from pybricks.parameters import Port
-from pybricks import version
-import OtherFunctions as debug
-import usys
-
-
-class HubDiagnostics:
-    def __init__(self, hub):
-        self.hub = hub
-        self.port_map = {
-            "A": Port.A,
-            "B": Port.B,
-            "C": Port.C,
-            "D": Port.D,
-            "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)
-                self.hub.display.pixel(x, y, brightness=100)
-                wait(100)
-                debug.log(f"Turning off pixel at position {x}, {y}...", v)
-                self.hub.display.pixel(x, y, brightness=0)
-    def printAll(self):
-        self.printAbout()
-        self.testLightSources(False)

diagnostics/color_sensor_diagnostics.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/full_diagnostics.py

@@ -4,16 +4,16 @@ from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
 from pybricks.robotics import DriveBase
 from pybricks.tools import wait, StopWatch
 HUB = PrimeHub()
-from BatteryDiagnostics import BatteryDiagnostics
-from MotorDiagnostics import MotorDiagnostics
-from ColorSensorDiagnostics import ColorSensorDiagnostics
-from DriveBaseDiagnostics import DriveBaseDiagnostics
-from HubDiagnostics import HubDiagnostics
+from battery_diagnostics import BatteryDiagnostics
+from motor_diagnostics import MotorDiagnostics
+from color_sensor_diagnostics import ColorSensorDiagnostics
+from drive_base_diagnostics import DriveBaseDiagnostics
+from hub_diagnostics 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.
-Developed by Team 65266, Lego Dynamics.  
-Please set your window size to 90% on small screens for best results with the ASCII art.               
+The free and open source diagnostics tool for LEGO® Education SPIKE™ Prime robots, designed for FIRST Lego League.
+Developed by Team 65266, Lego Dynamics.                                                
           """
           )
 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/hub_diagnostics.py

@@ -0,0 +1,41 @@
+from pybricks.tools import wait, StopWatch
+from pybricks import version
+import other_functions as debug
+from micropython_diagnostics import MicroPythonDiagnostics
+from pybricks.parameters import Port
+class HubDiagnostics:
+    def __init__(self, hub):
+        self.hub = hub
+        self.port_map = {
+            "A": Port.A,
+            "B": Port.B,
+            "C": Port.C,
+            "D": Port.D,
+            "E": Port.E,
+            "F": Port.F,
+        }
+    def testLightSources(self, verbose):
+        v = verbose
+        self.hub.display.off()
+        for x in range(5):
+            for y in range(5):
+                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"[Hub Diagnostics - Light Sources] Turning off pixel at position {x}, {y}...", v)
+                self.hub.display.pixel(x, y, brightness=0)
+
+    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/micropython_diagnostics.py

@@ -0,0 +1,42 @@
+import usys
+import micropython
+from pybricks import version
+class MicroPythonDiagnostics:
+    def __init__(self, hub):
+        pass
+    def printVersionDiagnostics():
+        print("[Hub Diagnostics - MicroPython - Version] Hub version information:", version)
+        print("[Hub Diagnostics - MicroPython - Version] MicroPython version:", usys.version)
+        print("[Hub Diagnostics - MicroPython - Version] Pybricks version information:", usys.version_info)
+        print("[Hub Diagnostics - MicroPython - Version] MicroPython information:", usys.implementation)
+    def performMemoryDiagnostics():
+        print("[Hub Diagnostics - MicroPython - Memory] Memory information (retrieved from the MicroPython environment):")
+        micropython.mem_info(1)
+        print("[Hub Diagnostics - MicroPython - Memory] Testing heap lock and unlock.")
+        print("[Hub Diagnostics - MicroPython - Memory] Allocating memory while heap is unlocked:")
+        try:
+            x = 5000
+            print("[Hub Diagnostics - MicroPython - Memory] [SUCCESS] There was no MemoryError raised. The value of the new variable x is", x)
+        except MemoryError:
+            print("[Hub Diagnostics - MicroPython - Memory] [FAIL] Allocation failed. Your memory may be faulty.")
+        print("[Hub Diagnostics - MicroPython - Memory] Locking the heap:")
+        micropython.heap_lock()
+        print("[Hub Diagnostics - MicroPython - Memory] Heap was locked. Attempting to allocate memory (this should fail):")
+        try:
+            y = 10000
+            print("[Hub Diagnostics - MicroPython - Memory] [FAIL] There was no MemoryError raised. Heap lock failed.")
+        except MemoryError:
+            print("[Hub Diagnostics - MicroPython - Memory] [SUCCESS] Allocation failed. Test successful. The heap was successfully locked.")
+            # Attempt to add gc to this for memory diagnostics, in addition test machine.soft_reset() and add that first to reset the heap.
+        print("[Hub Diagnostics - MicroPython - Memory] Unlocking the heap:")
+        micropython.heap_unlock()
+        print("[Hub Diagnostics - MicroPython - Memory] Heap was unlocked. Attempting to allocate memory (this should succeed):")
+        try:
+            z = 17000
+            print("[Hub Diagnostics - MicroPython - Memory] [SUCCESS] There was no MemoryError raised. The value of the new variable y is", x)
+        except MemoryError:
+            print("[Hub Diagnostics - MicroPython - Memory] [FAIL] Allocation failed. The heap failed to unlock.")
+    def printAll():
+        printVersionDiagnostics()
+        performMemoryDiagnostics()
+        

diagnostics/motor_diagnostics.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/xbox_controller.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())