BatteryDiagnostics.py

@@ -0,0 +1,49 @@
+from pybricks.tools import wait
+import umath
+class BatteryDiagnostics:
+    def __init__(self, hub):
+        self.voltage = 0
+        self.current = 0
+        self.hub = hub
+    def printVoltage(self):
+        self.voltage = self.hub.battery.voltage()
+        if self.voltage > 7800:
+            print(f"Battery voltage is sufficient: {self.voltage}")
+        elif self.voltage < 7800 :
+            print(f"Charging needed: {self.voltage}")
+    def printCurrent(self):
+        self.current = self.hub.battery.current()
+        print("Battery current:", self.current)
+    def printAll(self):
+        timeelapsed = 0
+        voltageList = []
+        currentList = []
+        while True:
+            print("\n\n\n\n\n")
+            self.printVoltage()
+            voltageList.append(self.voltage)
+            self.printCurrent()
+            currentList.append(self.current)
+            wait(50)
+            timeelapsed += 50
+            
+            if(timeelapsed >= 3000):
+                break
+        print("--------------FINAL RESULTS OF BATTERY DIAGNOSTICS---------------")
+        print("Voltage deviation:", self.stdev(voltageList))
+        print("Current deviation:", self.stdev(currentList))
+    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)

ColorSensorDiagnostics.py

@@ -0,0 +1,48 @@
+from pybricks.parameters import Color, Port, Stop
+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,
+            "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 = self.colorsensorclass(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 printAll(self):
+        self.initializeColorSensor()
+        stopwatch = StopWatch()
+        while stopwatch.time() < 5000:
+            print("HSV output:", self.colorsensor.hsv())
+            print("Detected color:", self.colorsensor.color())

DriveBaseDiagnostics.py

@@ -0,0 +1,165 @@
+from pybricks.parameters import Direction, Port, Side, Stop
+from pybricks.robotics import DriveBase
+from pybricks.tools import wait, StopWatch
+
+from usys import stdin
+from uselect import poll
+
+class DriveBaseDiagnostics:
+    def __init__(self, hub, motorclass, dbclass):
+        self.hub = hub
+        self.motorclass = motorclass
+        self.dbclass = dbclass
+        self.drive_base = None
+        self.PORT_MAP = {
+            "A": Port.A,
+            "B": Port.B,
+            "C": Port.C,
+            "D": Port.D,
+            "E": Port.E,
+            "F": Port.F,
+            }
+    def initializeDriveBase(self):
+
+        print("DriveBase setup:")
+        print("1 = Load from savefile (paste JSON)")
+        print("2 = Use defaults")
+        print("3 = Enter values manually")
+
+        choice = input("Choose an option: ")
+
+        # Default values
+        WHEEL_DIAMETER = 68.8
+        AXLE_TRACK = 180
+        DRIVE_SETTINGS = (600, 2000, 300, 2000)
+
+        # Motor ports (None until set)
+        leftmotorport = Port.A
+        rightmotorport = Port.B
+
+        # -----------------------------
+        # OPTION 1: LOAD SAVEFILE
+        # -----------------------------
+        if choice == "1":
+            print("Paste JSON:")
+            raw = input("> ")
+
+            # --- wheel ---
+            if "\"wheel\"" in raw:
+                part = raw.split("\"wheel\"")[1]
+                part = part.split(":")[1]
+                part = part.split(",")[0]
+                WHEEL_DIAMETER = float(part)
+
+            # --- axle ---
+            if "\"axle\"" in raw:
+                part = raw.split("\"axle\"")[1]
+                part = part.split(":")[1]
+                part = part.split(",")[0]
+                AXLE_TRACK = float(part)
+
+            # --- settings ---
+            if "\"settings\"" in raw:
+                part = raw.split("\"settings\"")[1]
+                part = part.split("[")[1]
+                part = part.split("]")[0]
+                nums = part.split(",")
+                DRIVE_SETTINGS = (int(nums[0]), int(nums[1]), int(nums[2]), int(nums[3]))
+
+                            # --- left motor port ---
+                if "\"left_port\"" in raw:
+                    part = raw.split("\"left_port\"")[1]
+                    part = part.split("\"")[1]   # first quoted value
+                    leftmotorport = part
+
+                # --- right motor port ---
+                if "\"right_port\"" in raw:
+                    part = raw.split("\"right_port\"")[1]
+                    part = part.split("\"")[1]
+                    rightmotorport = part
+
+
+            print("Loaded config.")
+
+        # -----------------------------
+        # OPTION 3: MANUAL ENTRY
+        # -----------------------------
+        elif choice == "3":
+            WHEEL_DIAMETER = float(input("Wheel diameter: "))
+            AXLE_TRACK = float(input("Axle track: "))
+
+            print("Enter drive settings:")
+            s1 = int(input("Straight speed: "))
+            s2 = int(input("Straight accel: "))
+            s3 = int(input("Turn rate: "))
+            s4 = int(input("Turn accel: "))
+            DRIVE_SETTINGS = (s1, s2, s3, s4)
+
+            # Ask for motor ports HERE (manual only)
+            leftmotorport = input("Left motor port: ")
+            rightmotorport = input("Right motor port: ")
+
+        # -----------------------------
+        # OPTION 2: DEFAULTS
+        # -----------------------------
+        else:
+            pass
+
+        # -----------------------------
+        # CREATE MOTORS
+        # -----------------------------
+        left_motor = self.motorclass(leftmotorport, Direction.COUNTERCLOCKWISE)
+        right_motor = self.motorclass(rightmotorport, Direction.CLOCKWISE)
+
+        # -----------------------------
+        # CREATE DRIVEBASE
+        # -----------------------------
+        self.drive_base = self.dbclass(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
+        self.drive_base.settings(*DRIVE_SETTINGS)
+        self.drive_base.use_gyro(True)
+
+        print("DriveBase initialized.")
+        return DRIVE_SETTINGS
+
+
+    
+    def printAll(self):
+        self.driveRobot()
+    def driveRobot(self):
+        drivesettings = self.initializeDriveBase()
+        print(drivesettings)
+        keyboard = poll()
+        keyboard.register(stdin)
+
+        last_key_time = StopWatch()
+        last_key_time.reset()
+
+        while True:
+            key = None
+
+            # Check for keypress immediately
+            if keyboard.poll(0):
+                key = stdin.read(1).upper()
+                last_key_time.reset()
+
+            # Process key
+            if key:
+                if key == "W":
+                    self.drive_base.drive(drivesettings[0], 0)
+                elif key == "A":
+                    self.drive_base.drive(drivesettings[0], -180)
+                elif key == "S":
+                    self.drive_base.drive(-drivesettings[0], 0)
+                elif key == "D":
+                    self.drive_base.drive(drivesettings[0], 180)
+                elif key == "X":
+                    break
+
+            # Auto-stop if no key for 120 ms
+            if last_key_time.time() > 120:
+                self.drive_base.stop()
+
+            # Tiny manual delay to avoid 100% CPU
+            # (much faster than wait())
+            for _ in range(200):
+                pass

FullDiagnostics.py

@@ -0,0 +1,74 @@
+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()
+from BatteryDiagnostics import BatteryDiagnostics
+from MotorDiagnostics import MotorDiagnostics
+from ColorSensorDiagnostics import ColorSensorDiagnostics
+from DriveBaseDiagnostics import DriveBaseDiagnostics
+from HubDiagnostics import HubDiagnostics
+battery = BatteryDiagnostics(HUB)
+motor = MotorDiagnostics(HUB, Motor)
+colorsensor = ColorSensorDiagnostics(HUB, ColorSensor)
+hubdiags = HubDiagnostics(HUB)
+drivebase = DriveBaseDiagnostics(HUB, Motor, DriveBase)
+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")
+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.               
+          """
+          )
+while True:
+    
+    print("\nWhich diagnostic do you want to perform?")
+    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 'q' to quit")
+
+    choice = input("Your choice: ").strip().lower()
+
+    if choice == "b":
+        print("-----------------------BATTERY DIAGNOSTICS-----------------------")
+        print("This test will check the battery voltage and current. It will measure these over a period of 3 seconds and provide average and deviation values. Your voltage should be above 7800 mV for optimal performance.")
+        input("Press Enter to begin the battery diagnostics.")
+        battery.printAll()
+        print("Battery diagnostics completed.")
+
+    elif choice == "m":
+        print("------------------------MOTOR DIAGNOSTICS------------------------")
+        motor.fullTest()
+        print("Motor diagnostics completed.")
+    elif choice == "h":
+        print("-------------------------HUB DIAGNOSTICS-------------------------")
+        hubdiags.printAll()
+        print("Hub diagnostics completed.")
+    elif choice == "q":
+        print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!")
+        break
+    elif choice == "db":
+        print("----------------------DRIVEBASE DIAGNOSTICS----------------------")
+        drivebase.printAll()
+    elif choice == "cs":
+        print("---------------------COLOR SENSOR DIAGNOSTICS---------------------")
+        colorsensor.printAll()
+        print("Color sensor diagnostics completed.")
+        
+    else:
+        print("Invalid choice. Please enter 'b', 'm', or 'q'.")

HubDiagnostics.py

@@ -0,0 +1,35 @@
+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)