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

Reviewed-on: Arcmyx/pybricks-utils#2

README.md

@@ -1,19 +1,11 @@
-# Team 65266 Lego Dynamics - PYNAMICS - Pybricks Utilities
+# Team 65266 Lego Dynamics - Pybricks Utils
 
 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/main/pynamics-screenshot.png" alt="Pynamics screenshot" width="670">
-
-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.
-
 Included utilities:
+- Diagnostics - a program that prints out useful information like battery life, etc.
+- Color Sensor Tests - a program that identifies what color the sensor is detecting. If you'd like, you can use our color ranges in your own programs.
 
-- 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.               
 This code is licensed under the Creative Commons Attribution 4.0 International License (CC BY 4.0). 
 
 Without the confusing legal speak, this means that you are free to:
@@ -23,4 +15,4 @@ Without the confusing legal speak, this means that you are free to:
 Under the following condition:
 - Attribution (BY) — You must give appropriate credit, provide a link to the license, and indicate if changes were made. You may do so in any reasonable manner, but not in any way that suggests the licensor endorses you or your use. Essentially, give us credit, link this repository, and don't take the credit for our work, because that's just sad.
 
-For the full legal details, please review the CC BY 4.0 Legal Code (read the [LICENSE](LICENSE) file here).
+For the full legal details, please review the CC BY 4.0 Legal Code.

diagnostics/battery_diagnostics.py

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

diagnostics/color_sensor_diagnostics.py

@@ -1,48 +0,0 @@
-from pybricks.parameters import Color, Port, Stop
-from pybricks.tools import wait, StopWatch
-
-class ColorSensorDiagnostics:
-    def __init__(self, hub, colorsensorclass):
-        self.colorsensor = None
-        self.PORT_MAP = {
-            "A": Port.A,
-            "B": Port.B,
-            "C": Port.C,
-            "D": Port.D,
-            "E": Port.E,
-            "F": Port.F,
-        }
-        self.colorsensorclass = colorsensorclass
-    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())

diagnostics/drive_base_diagnostics.py

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

diagnostics/full_diagnostics.py

@@ -1,73 +0,0 @@
-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 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)
-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")
-print("""
- ███████████  █████ █████ ██████   █████   █████████   ██████   ██████ █████   █████████   █████████ 
-▒▒███▒▒▒▒▒███▒▒███ ▒▒███ ▒▒██████ ▒▒███   ███▒▒▒▒▒███ ▒▒██████ ██████ ▒▒███   ███▒▒▒▒▒███ ███▒▒▒▒▒███
- ▒███    ▒███ ▒▒███ ███   ▒███▒███ ▒███  ▒███    ▒███  ▒███▒█████▒███  ▒███  ███     ▒▒▒ ▒███    ▒▒▒ 
- ▒██████████   ▒▒█████    ▒███▒▒███▒███  ▒███████████  ▒███▒▒███ ▒███  ▒███ ▒███         ▒▒█████████ 
- ▒███▒▒▒▒▒▒     ▒▒███     ▒███ ▒▒██████  ▒███▒▒▒▒▒███  ▒███ ▒▒▒  ▒███  ▒███ ▒███          ▒▒▒▒▒▒▒▒███
- ▒███            ▒███     ▒███  ▒▒█████  ▒███    ▒███  ▒███      ▒███  ▒███ ▒▒███     ███ ███    ▒███
- █████           █████    █████  ▒▒█████ █████   █████ █████     █████ █████ ▒▒█████████ ▒▒█████████ 
-▒▒▒▒▒           ▒▒▒▒▒    ▒▒▒▒▒    ▒▒▒▒▒ ▒▒▒▒▒   ▒▒▒▒▒ ▒▒▒▒▒     ▒▒▒▒▒ ▒▒▒▒▒   ▒▒▒▒▒▒▒▒▒   ▒▒▒▒▒▒▒▒▒  
-                                                                                                     
-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:
-    
-    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 'db' for drive base diagnostics")
-    print("Enter 'h' for hub 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] Motor diagnostics completed.")
-
-    elif choice == "q":
-        print("Diagnostics completed successfully. Exiting program.")
-        break
-    elif choice == "cs":
-        print("---------------------COLOR SENSOR DIAGNOSTICS---------------------")
-        colorsensor.printAll()
-        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

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

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

@@ -1,135 +0,0 @@
-from pybricks.parameters import Direction, Port, Stop
-from pybricks.tools import wait, StopWatch
-import umath
-class MotorDiagnostics:
-  def __init__(self, hub, motorclass):
-      self.testmotor = None
-      self.port_map = {
-        "A": Port.A,
-        "B": Port.B,
-        "C": Port.C,
-        "D": Port.D,
-        "E": Port.E,
-        "F": Port.F,
-      }
-      self.motorclass = motorclass
-  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)
-  def health_score(self, desired, avg_speed, stdev_speed, avg_load):
-    # Speed accuracy: penalize % error
-    ACCURACY = max(0, 100 - abs(avg_speed - desired) / desired * 100)
-
-    # Stability: penalize deviation relative to desired
-    STABILITY = max(0, 100 - (stdev_speed / desired) * 100)
-
-    # Normalize load: map 10 to 20 as baseline (around 0%), 200 as max (around 100%)
-    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)
-
-    LOAD_SCORE = max(0, 100 - LOAD_PCT)
-
-    # Final score: average of the three
-    return (ACCURACY + STABILITY + LOAD_SCORE) / 3
-
-  def initializeMotor(self):
-    VALID_PORTS = {"A", "B", "C", "D", "E", "F"}
-    while True:
-      motorinput = input(
-        "This test will run your motor at 3 speeds: 180, 540, and 1000 degrees per second.\n"
-        "Please make sure your motor is not under any load (for example, your hand) during the test.\n"
-        "If you want to test the wheel's load, note that this will affect the load measurements.\n"
-        "Enter the port for the motor you would like to test (A, B, C, D, E, or F): "
-      ).strip().upper()
-      if motorinput not in VALID_PORTS:
-        print("That is not a valid port. Please enter A-F.")
-        continue
-      try:
-        # Only create a new Motor if we don't already have one
-        if self.testmotor is None:
-            self.testmotor = self.motorclass(self.port_map[motorinput])
-            print(f"Motor initialized on port {motorinput}.")
-        else:
-            print(f"Reusing existing motor on port {motorinput}.")
-        break
-
-      except OSError as e:
-        if e.errno == 16:  # EBUSY
-          print(f"Port {motorinput} is already in use. Reusing existing motor.")
-          # Do not overwrite self.testmotor here — keep the existing reference
-          break
-        else:
-          print(f"Error initializing motor on port {motorinput}: {e}")
-          print("Make sure a motor is actually connected to this port.")
-          self.testmotor = None
-
-  def testSpeed(self, speed):
-    self.testmotor.reset_angle(0)
-    
-    motorspeeds = []
-    motorloads = []
-    TARGET_ANGLE = speed * 3
-    print("\n", speed, "DEGREES PER SECOND TEST")
-    self.testmotor.run_angle(speed, TARGET_ANGLE, Stop.HOLD, False)
-    stopwatchmotor = StopWatch()
-    while stopwatchmotor.time() < 3000:
-        wait(20)
-        motorspeeds.append(self.testmotor.speed())
-        motorloads.append(self.testmotor.load())
-    
-    MAX_SPEED, MAX_ACCEL, MAX_TORQUE = self.testmotor.control.limits()
-
-
-    print("Desired motor speed: ", str(speed))
-    if motorspeeds:
-      avg = sum(motorspeeds) / len(motorspeeds)
-      print("Average motor speed:", avg)
-      print("Motor speed deviation:", str(self.stdev(motorspeeds)))
-    else:
-      print("No speed samples collected.")
-      avg = 0
-    if motorloads:
-      avgload = sum(motorloads) / len(motorloads)
-
-      print("Average motor load:", avgload)
-      print("Motor load deviation:", str(self.stdev(motorloads)))
-    else:
-      print("No load samples collected.")
-      avgload = 0
-    SCORE = self.health_score(speed, avg, self.stdev(motorspeeds), avgload)
-    print("Health score for this test:", str(SCORE) + "%")
-    return SCORE
-  def fullTest(self):
-    self.initializeMotor()
-    print("Load measurements are in mNm. Speed measurements are in degrees per second.")
-    MAX_SPEED, MAX_ACCEL, MAX_TORQUE = self.testmotor.control.limits()
-    print("Maximum motor speed:", MAX_SPEED)
-    test180 = self.testSpeed(180)
-    test540 = self.testSpeed(540)
-    test1000 = self.testSpeed(1000)
-    print("\n FINAL MOTOR STATISTICS")
-    final = (test180 + test540 + test1000) / 3
-    print("Final motor health score:", str(final) + "%")
-    if final < 65:
-      print("Your motor is in need of attention. Make sure to clean it regularly and charge the Prime Hub.")
-    elif final < 85:
-      print("Your motor is in OK condition. Make sure to clean it regularly and charge the Prime Hub.")
-    elif final < 95:
-      print("Your motor is in great condition!")
-    else:
-      print("Your motor is in AMAZING condition!!!")
-    self.testmotor.stop()

diagnostics/other_functions.py

@@ -1,3 +0,0 @@
-def log(string, verbose):
-    if(verbose):
-        print("[LOG (verbose)]", string)

templates/color_sensor_start_logic.py

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

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

utils/BatteryDiagnostics.py

@@ -0,0 +1,33 @@
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.tools import run_task, multitask
+from pybricks.tools import wait, StopWatch
+from pybricks.robotics import DriveBase
+from pybricks.hubs import PrimeHub
+
+# Initialize hub and devices
+hub = PrimeHub()
+left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
+right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
+left_arm = Motor(Port.C, Direction.CLOCKWISE) # Specify default direction
+right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
+lazer_ranger = UltrasonicSensor(Port.E)
+color_sensor = ColorSensor(Port.F)
+
+# DriveBase configuration
+WHEEL_DIAMETER = 68.8  # mm (adjust for your wheels)
+AXLE_TRACK = 180  # mm (distance between wheels)
+drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
+drive_base.settings(600, 500, 300, 200)
+drive_base.use_gyro(True)
+
+WALL_DISTANCE = 200  # mm
+
+if hub.battery.voltage() > 7800:
+    print(f"Battery voltage is sufficient: {hub.battery.voltage()}")
+elif hub.battery.voltage < 7800 :
+    print(f"Charging needed: {hub.battery.voltage()}")
+
+print(hub.battery.current())
+print(hub.imu.heading())
+print(hub.system.info())

utils/ColorSensorTests-old.py

@@ -0,0 +1,69 @@
+from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
+from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
+from pybricks.tools import run_task, multitask
+from pybricks.tools import wait, StopWatch
+from pybricks.robotics import DriveBase
+from pybricks.hubs import PrimeHub
+
+# Initialize hub and devices
+hub = PrimeHub()
+
+color_sensor = ColorSensor(Port.F)
+
+# Color Settings
+# https://docs.pybricks.com/en/latest/parameters/color.html
+print("Default Detected Colors:", color_sensor.detectable_colors())
+
+# Custom color Hue, Saturation, Brightness value for Lego bricks
+Color.MAGENTA = Color(315,100,60)
+Color.BLUE = Color(240,100,100)
+Color.CYAN = Color(180,100,100)
+Color.RED = Color(350, 100, 100)
+LEGO_BRICKS_COLOR = [
+    Color.BLUE, 
+    Color.GREEN, 
+    Color.WHITE,
+    Color.RED, 
+    Color.YELLOW, 
+    Color.MAGENTA, 
+    Color.NONE
+]
+magenta_counter = 0
+stable_color = None
+real_color = None
+#Update Detectable colors
+color_sensor.detectable_colors(LEGO_BRICKS_COLOR)
+print(f'Yellow:{Color.YELLOW} : {Color.YELLOW.h}, {Color.YELLOW.s}, {Color.YELLOW.v}')
+print("Updated Detected Colors:", color_sensor.detectable_colors())
+async def main():  
+    while True:
+        global magenta_counter, stable_color, real_color
+        color_reflected_percent = await color_sensor.reflection()
+        print("Reflection: ", color_reflected_percent)
+        if color_reflected_percent > 15:
+            color_detected = await color_sensor.color()
+
+            if color_detected == Color.MAGENTA:
+                magenta_counter += 1
+            else:
+                magenta_counter = 0
+                stable_color = color_detected
+
+            # Only accept magenta if it's been stable for a while - usually triggers before other colors so we gotta do this :|
+            if magenta_counter > 10:
+                stable_color = Color.MAGENTA
+                if stable_color != Color.MAGENTA:
+                    stable_color = await color_sensor.color()
+
+            real_color = stable_color
+            #if(color_detected != Color.NONE):
+            #    return
+            
+            print("Magenta counter: ", magenta_counter)
+            if real_color is not None:
+                print(f'Detected color: {real_color} : {real_color.h}, {real_color.s}, {real_color.v}')
+            else:
+                print("No valid color detected yet.")
+        await wait(50)
+
+run_task(main())

utils/FullDiagnostics.py

@@ -0,0 +1,35 @@
+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
+battery = BatteryDiagnostics()
+clearConfirmation = input("Do you want to clear the console before proceeding? Y/N (default: yes): ")
+if(clearConfirmation == "Y" or clearConfirmation == "y" or clearConfirmation == "yes" or clearConfirmation == ""):
+    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")
+
+while True:
+    print("\nWhat diagnostic do you want to perform?")
+    print("Enter 'b' for Battery diagnostics")
+    print("Enter 'm' for Motor diagnostics")
+    print("Enter 'q' to Quit")
+
+    choice = input("Your choice: ").strip().lower()
+
+    if choice == "b":
+        print("-----------------------BATTERY DIAGNOSTICS-----------------------")
+        battery.printAll()
+
+    elif choice == "m":
+        print("------------------------MOTOR DIAGNOSTICS------------------------")
+        # motor.printAll()  # call your motor diagnostics here
+        print("Motor diagnostics would run here.")
+
+    elif choice == "q":
+        print("Diagnostics completed successfully. Exiting with code 0. Good luck in the robot game!")
+        break
+
+    else:
+        print("Invalid choice. Please enter 'b', 'm', or 'q'.")