diff --git a/BatteryDiagnostics.py b/BatteryDiagnostics.py new file mode 100644 index 0000000..682793f --- /dev/null +++ b/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) \ No newline at end of file diff --git a/ColorSensorDiagnostics.py b/ColorSensorDiagnostics.py new file mode 100644 index 0000000..44fae16 --- /dev/null +++ b/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()) \ No newline at end of file diff --git a/DriveBaseDiagnostics.py b/DriveBaseDiagnostics.py new file mode 100644 index 0000000..45e6e38 --- /dev/null +++ b/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 diff --git a/FullDiagnostics.py b/FullDiagnostics.py new file mode 100644 index 0000000..1e3f57a --- /dev/null +++ b/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'.") \ No newline at end of file diff --git a/HubDiagnostics.py b/HubDiagnostics.py new file mode 100644 index 0000000..9bdf436 --- /dev/null +++ b/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)