From 33344aafd0c42e5e2a0ee6efe927dade6c1e5a71 Mon Sep 17 00:00:00 2001 From: nifrali Date: Mon, 12 Oct 2026 22:11:02 -0500 Subject: [PATCH 1/2] restructure files for scrimmage --- shout-fll-scrimmage/BatteryDiagnostics.py | 49 +++++ shout-fll-scrimmage/ColorSensorDiagnostics.py | 48 +++++ shout-fll-scrimmage/DriveBaseDiagnostics.py | 165 ++++++++++++++++ shout-fll-scrimmage/FullDiagnostics.py | 74 ++++++++ shout-fll-scrimmage/HubDiagnostics.py | 35 ++++ shout-fll-scrimmage/MotorDiagnostics.py | 136 ++++++++++++++ shout-fll-scrimmage/bioglow_helpers.py | 176 ++++++++++++++++++ shout-fll-scrimmage/bioglow_missions_3_4_5.py | 27 +++ shout-fll-scrimmage/bioglow_robot_settings.py | 27 +++ .../bioglow_shoutfll_scrimmage_main.py | 86 +++++++++ shout-fll-scrimmage/main.py | 6 - shout-fll-scrimmage/missions/mission_1_2_3.py | 1 - 12 files changed, 823 insertions(+), 7 deletions(-) create mode 100644 shout-fll-scrimmage/BatteryDiagnostics.py create mode 100644 shout-fll-scrimmage/ColorSensorDiagnostics.py create mode 100644 shout-fll-scrimmage/DriveBaseDiagnostics.py create mode 100644 shout-fll-scrimmage/FullDiagnostics.py create mode 100644 shout-fll-scrimmage/HubDiagnostics.py create mode 100644 shout-fll-scrimmage/MotorDiagnostics.py create mode 100644 shout-fll-scrimmage/bioglow_helpers.py create mode 100644 shout-fll-scrimmage/bioglow_missions_3_4_5.py create mode 100644 shout-fll-scrimmage/bioglow_robot_settings.py create mode 100644 shout-fll-scrimmage/bioglow_shoutfll_scrimmage_main.py delete mode 100644 shout-fll-scrimmage/main.py delete mode 100644 shout-fll-scrimmage/missions/mission_1_2_3.py diff --git a/shout-fll-scrimmage/BatteryDiagnostics.py b/shout-fll-scrimmage/BatteryDiagnostics.py new file mode 100644 index 0000000..682793f --- /dev/null +++ b/shout-fll-scrimmage/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/shout-fll-scrimmage/ColorSensorDiagnostics.py b/shout-fll-scrimmage/ColorSensorDiagnostics.py new file mode 100644 index 0000000..44fae16 --- /dev/null +++ b/shout-fll-scrimmage/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/shout-fll-scrimmage/DriveBaseDiagnostics.py b/shout-fll-scrimmage/DriveBaseDiagnostics.py new file mode 100644 index 0000000..45e6e38 --- /dev/null +++ b/shout-fll-scrimmage/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/shout-fll-scrimmage/FullDiagnostics.py b/shout-fll-scrimmage/FullDiagnostics.py new file mode 100644 index 0000000..1e3f57a --- /dev/null +++ b/shout-fll-scrimmage/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/shout-fll-scrimmage/HubDiagnostics.py b/shout-fll-scrimmage/HubDiagnostics.py new file mode 100644 index 0000000..9bdf436 --- /dev/null +++ b/shout-fll-scrimmage/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) diff --git a/shout-fll-scrimmage/MotorDiagnostics.py b/shout-fll-scrimmage/MotorDiagnostics.py new file mode 100644 index 0000000..b8e762b --- /dev/null +++ b/shout-fll-scrimmage/MotorDiagnostics.py @@ -0,0 +1,136 @@ +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.motorclass = motorclass + self.port_map = { + "A": Port.A, + "B": Port.B, + "C": Port.C, + "D": Port.D, + "E": Port.E, + "F": Port.F, + } + + 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 = 15 # 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() \ No newline at end of file diff --git a/shout-fll-scrimmage/bioglow_helpers.py b/shout-fll-scrimmage/bioglow_helpers.py new file mode 100644 index 0000000..e3dcc0a --- /dev/null +++ b/shout-fll-scrimmage/bioglow_helpers.py @@ -0,0 +1,176 @@ +""" +PID Straight Drive — heading-corrected forward / backward movement. + +Improvements over baseline: + • Derivative on measurement (not error) — no derivative kick on setpoint change + • Fixed-interval dt — loop sleeps a constant 20 ms, so /dt is dropped from the + derivative term (dividing by 0.02 was silently multiplying KD by 50) + • prev_error seeded from first real heading — eliminates first-tick spike + • Per-motor speed clamping — prevents one side pivoting at low speeds + • Deceleration ramp — smooth stop in the last RAMP_MM millimetres + • Dual-motor exit + timeout — survives stall on either wheel + • Integral freeze near target — stops windup right where it matters most +""" + +from bioglow_settings import * # hub, motors, sensors, drive_base, wait, umath, ... + +LOOP_MS = 20 # fixed control loop period +RAMP_MM = 50 # begin decelerating this far from target +MIN_SPEED = 80 # floor speed during ramp (avoid stall) +TIMEOUT_MS = 8_000 # safety abort +# Tuning sequence + +KP = 4.8 # Wide chassis = stable platform, can afford higher P gain + # without oscillation. Pushes harder through corrections. + +KI = 0.012 # Slightly lower than default — large wheels cover distance + # fast so the integrator has less time to build up per run. + # Raise to 0.012 if you see consistent sideways drift at end. + +KD = 7.5 # Higher than default. Big wheels = more rotational momentum, + # needs stronger damping to prevent overshoot on corrections. + +def _mm_to_deg(mm: float) -> float: + return (abs(mm) / (umath.pi * WHEEL_DIAMETER)) * 360.0 + + +def _motor_avg_angle(m1, m2) -> float: + return (abs(m1.angle()) + abs(m2.angle())) / 2.0 + + +async def pid_straight(distance_mm: float, speed: int = 600): + """ + Drive straight for distance_mm at speed (deg/s). + Positive = forward, negative = reverse. + """ + direction = 1 if distance_mm >= 0 else -1 + degrees_need = _mm_to_deg(distance_mm) + target_hdg = hub.imu.heading() + + left_motor.reset_angle(0) + right_motor.reset_angle(0) + + # ── seed integral and prev_error from the real opening heading ── + first_error = target_hdg - hub.imu.heading() # 0.0 on a perfect start, + integral = 0.0 # non-zero if robot is tilted + prev_heading = hub.imu.heading() # for derivative-on-measurement + + watchdog = StopWatch() + + while True: + traveled = _motor_avg_angle(left_motor, right_motor) + + # ── exit: distance reached or timeout ── + if traveled >= degrees_need: + break + if watchdog.time() > TIMEOUT_MS: + break + + # ── heading error with wrap ── + heading = hub.imu.heading() + error = target_hdg - heading + if error > 180: error -= 360 + if error < -180: error += 360 + + # ── derivative on measurement — no kick when target changes ── + d_heading = heading - prev_heading # raw delta (deg / loop) + if d_heading > 180: d_heading -= 360 + if d_heading < -180: d_heading += 360 + prev_heading = heading + + # ── integral with freeze near target ── + near_target = (degrees_need - traveled) < _mm_to_deg(RAMP_MM / 2) + if not near_target: + integral = max(-50.0, min(50.0, integral + error)) # dt=const → no *dt + + # ── PID output ── + # derivative term: -KD * d_heading (measurement, not error difference) + correction = KP * error + KI * integral - KD * d_heading + correction = max(-200.0, min(200.0, correction)) + + # ── ramp speed near end ── + remaining = degrees_need - traveled + ramp_degrees = _mm_to_deg(RAMP_MM) + if remaining < ramp_degrees and ramp_degrees > 0: + ramp_factor = max(float(MIN_SPEED) / speed, + remaining / ramp_degrees) + run_speed = int(speed * ramp_factor) + else: + run_speed = speed + + # ── per-motor clamping — prevents pivot at low speeds ── + l_cmd = max(-speed, min(speed, direction * run_speed + int(correction))) + r_cmd = max(-speed, min(speed, direction * run_speed - int(correction))) + + left_motor.run(l_cmd) + right_motor.run(r_cmd) + await wait(LOOP_MS) + + left_motor.brake() + right_motor.brake() + + +""" +Debugging helps +""" +DEBUG = 1 # Enable when you want to show logs +# Example conversion function (adjust min/max values as needed for your hub) +async def get_battery_percentage(voltage_mV:float): + max_voltage = 8400.0 # max battery level https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb87f4ba8db36994a/5f8801b918967612e58a69a6/techspecs_techniclargehubrechargeablebattery.pdf?locale=en-us + min_voltage = 5000.0 # min battery level + percentage = ((float(voltage_mV) - min_voltage) / float(max_voltage - min_voltage) )* 100 + return max(0, min(100, percentage)) # Ensure percentage is between 0 and 100 + +async def wait_button_release(): + """Wait for all buttons to be released""" + while hub.buttons.pressed(): + await wait(500) + await wait(1000) # Debounce delay + +WALL_DISTANCE = 300 # mm +async def drive_forward(): + """Drive forward continuously using DriveBase.""" + drive_base.drive(1000,0) + +async def drive_backward(): + """Drive backward continuously using DriveBase.""" + drive_base.drive(-400, 0) # FIXED: was +400, which drove forward + + +async def monitor_distance(stop_mm: int = WALL_DISTANCE): + """Monitor ultrasonic sensor and stop when wall is detected.""" + while True: + distance = await lazer_ranger.distance() + print('Distancing...', distance) + + if distance is None: + await wait(50) + continue + if distance < stop_mm: + drive_base.stop() + print(f"Wall detected at {distance}mm!") + break + + # Small delay to prevent overwhelming the sensor + await wait(50) + +# Use this to set default +def set_default_speed(): + drive_base.settings(600, 500, 300, 200) + +# Use this to change drive base movement +def set_speed(straight_speed, st_acc, turn_speed, turn_acc): + drive_base.settings(straight_speed, st_acc, turn_speed, turn_acc) + +async def multilift_up(rotation_angle): + await multitask( + right_arm.run_angle(400,rotation_angle), + left_arm.run_angle(400,-1*rotation_angle) + ) + +async def multilift_down(rotation_angle): + await multitask( + right_arm.run_angle(1000,-1*rotation_angle), + left_arm.run_angle(1000,rotation_angle) + ) + diff --git a/shout-fll-scrimmage/bioglow_missions_3_4_5.py b/shout-fll-scrimmage/bioglow_missions_3_4_5.py new file mode 100644 index 0000000..a65d9a7 --- /dev/null +++ b/shout-fll-scrimmage/bioglow_missions_3_4_5.py @@ -0,0 +1,27 @@ +##################################### +# Solution for BioGlow Missions 3,4,5 +##################################### + +import bioglow_helpers as helper +from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings + +async def solve_missions(): # Flip The Rock, Lucky Leaves, Reaching Roots - Rishabh + + await drive_base.straight(550)#Works (600(Previous)) + await drive_base.straight(-200) + await drive_base.turn(45) + await drive_base.straight(575) + await drive_base.turn(-90)#-90 for Coach Cisco table + await left_arm.run_angle(200,270) + await drive_base.straight(300) + await left_arm.run_angle(200,-265) + await right_arm.run_angle(200, -200) + await drive_base.straight(-105)#Use -105 for Coach Cisco table(-105 for school) + await drive_base.turn(32)#was 24-Use 24 for Coach Cisco's table(32 for school) + await right_arm.run_angle(1500, 180) + await drive_base.turn(30) + await drive_base.straight(-1000) + + +if __name__ == "__main__": # This run this mission stand-alone when not called from bioglow_main.py + run_task(solve_missions()) \ No newline at end of file diff --git a/shout-fll-scrimmage/bioglow_robot_settings.py b/shout-fll-scrimmage/bioglow_robot_settings.py new file mode 100644 index 0000000..caee48f --- /dev/null +++ b/shout-fll-scrimmage/bioglow_robot_settings.py @@ -0,0 +1,27 @@ +#Important Notice: All codes should be tested while the robot's battery is at 100%, and all updates must be made when the robot is at full charge. + +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 +import umath + +# 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, [[12,36],[12,20,24]]) # FIXED: both gear trains in ONE list +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 = 62.4 # mm (adjust for your wheels) +AXLE_TRACK = 150 # mm (distance between wheels) + +# The ONE drive base. Every other file imports it from here — never make another. +drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK) +drive_base.settings(600, 500, 300, 200) +drive_base.use_gyro(True) diff --git a/shout-fll-scrimmage/bioglow_shoutfll_scrimmage_main.py b/shout-fll-scrimmage/bioglow_shoutfll_scrimmage_main.py new file mode 100644 index 0000000..13d7782 --- /dev/null +++ b/shout-fll-scrimmage/bioglow_shoutfll_scrimmage_main.py @@ -0,0 +1,86 @@ +import bioglow_helpers as helper +import bioglow_settings as robot +import bioglow_mission_3_4_5 as Run_1 + +# 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 < 24 and s > 80: # orange + return "Orange" + elif 30 < h < 60 and v > 50: # yellow + return "Yellow" + elif 70 < h < 160: # green - do it vertically not horizontally for accuracy + return "Green" + elif 195 < h < 198: # light blue + return "Light_Blue" + elif 210 < h < 225: # blue - do it vertically not horizontally for accuracy + return "Blue" + elif 260 < h < 350: # purple + return "Purple" + elif s < 15 and v > 70: # white + return "White" + else: + return "Unknown" + return "Unknown" + + +async def main(): + while True: + pressed = robot.hub.buttons.pressed() + h, s, v = await robot.color_sensor.hsv() + reflected = await robot.color_sensor.reflection() + color = detect_color(h, s, v, reflected) + if helper.DEBUG : + #print(color_sensor.color()) + #print(h,s,v) + #print(color) + print(f"button pressed: {pressed}") + + + if color == "Green": + print('Running Mission 1, Detect Green') + await Run_1.solve_missions() + elif color == "Red": + print('Running Mission 2, Detect Red') + pass # TODO: Run2 not written yet (it crashed with NameError) + elif color == "Yellow": + print('Running Mission 3, Detect Yellow') + pass # TODO: Run3 not written yet (it crashed with NameError) + elif color == "Blue": + print('Running Mission 4, Detect Blue') + pass # TODO: Run4 not written yet (it crashed with NameError) + elif color == "Orange": + print('Running Mission 5, Detect Orange') + pass # TODO: Run5 not written yet (it crashed with NameError) + elif color == "Purple": + print('Running Mission 6, Detect Purple') + pass # TODO: Run6 not written yet (it crashed with NameError) + elif color == "Light_Blue": + print('Running Mission 7, Detect Cyan') + pass # TODO: Run7 not written yet (it crashed with NameError) + elif color == "White": + print('Detect White') + else: + print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})") + #pass + + # Show battery % for debugging + if robot.Button.BLUETOOTH in pressed: # using bluetooth button here since away from color sensor + # Get the battery voltage in millivolts (mV) + battery_voltage_mV = robot.hub.battery.voltage() + # Use the function with your voltage reading + percentage = await helper.get_battery_percentage(float(battery_voltage_mV)) + if helper.DEBUG: + print(f"Battery voltage: {battery_voltage_mV} mV") + print(f"Battery level: {percentage:.3f}%") + print("FLL Robot System Ready!") + await robot.hub.display.text(f"{percentage:.0f}") + break + elif pressed == None: + continue + + await robot.wait(10) +# Run the main function +robot.run_task(main()) \ No newline at end of file diff --git a/shout-fll-scrimmage/main.py b/shout-fll-scrimmage/main.py deleted file mode 100644 index bf984d0..0000000 --- a/shout-fll-scrimmage/main.py +++ /dev/null @@ -1,6 +0,0 @@ -''' - -This is where you write the main code to start all mission codes -Combine all call here - -''' diff --git a/shout-fll-scrimmage/missions/mission_1_2_3.py b/shout-fll-scrimmage/missions/mission_1_2_3.py deleted file mode 100644 index bf11581..0000000 --- a/shout-fll-scrimmage/missions/mission_1_2_3.py +++ /dev/null @@ -1 +0,0 @@ -#Write Code here for your missions From 73f96e649832aa37356e11a56f92704bc4ca18e6 Mon Sep 17 00:00:00 2001 From: nifrali Date: Mon, 12 Oct 2026 22:14:40 -0500 Subject: [PATCH 2/2] Rename all pynamics files --- .../{BatteryDiagnostics.py => PynamicsBatteryDiagnostics.py} | 0 ...olorSensorDiagnostics.py => PynamicsColorSensorDiagnostics.py} | 0 .../{DriveBaseDiagnostics.py => PynamicsDriveBaseDiagnostics.py} | 0 .../{FullDiagnostics.py => PynamicsFullDiagnostics.py} | 0 .../{HubDiagnostics.py => PynamicsHubDiagnostics.py} | 0 .../{MotorDiagnostics.py => PynamicsMotorDiagnostics.py} | 0 6 files changed, 0 insertions(+), 0 deletions(-) rename shout-fll-scrimmage/{BatteryDiagnostics.py => PynamicsBatteryDiagnostics.py} (100%) rename shout-fll-scrimmage/{ColorSensorDiagnostics.py => PynamicsColorSensorDiagnostics.py} (100%) rename shout-fll-scrimmage/{DriveBaseDiagnostics.py => PynamicsDriveBaseDiagnostics.py} (100%) rename shout-fll-scrimmage/{FullDiagnostics.py => PynamicsFullDiagnostics.py} (100%) rename shout-fll-scrimmage/{HubDiagnostics.py => PynamicsHubDiagnostics.py} (100%) rename shout-fll-scrimmage/{MotorDiagnostics.py => PynamicsMotorDiagnostics.py} (100%) diff --git a/shout-fll-scrimmage/BatteryDiagnostics.py b/shout-fll-scrimmage/PynamicsBatteryDiagnostics.py similarity index 100% rename from shout-fll-scrimmage/BatteryDiagnostics.py rename to shout-fll-scrimmage/PynamicsBatteryDiagnostics.py diff --git a/shout-fll-scrimmage/ColorSensorDiagnostics.py b/shout-fll-scrimmage/PynamicsColorSensorDiagnostics.py similarity index 100% rename from shout-fll-scrimmage/ColorSensorDiagnostics.py rename to shout-fll-scrimmage/PynamicsColorSensorDiagnostics.py diff --git a/shout-fll-scrimmage/DriveBaseDiagnostics.py b/shout-fll-scrimmage/PynamicsDriveBaseDiagnostics.py similarity index 100% rename from shout-fll-scrimmage/DriveBaseDiagnostics.py rename to shout-fll-scrimmage/PynamicsDriveBaseDiagnostics.py diff --git a/shout-fll-scrimmage/FullDiagnostics.py b/shout-fll-scrimmage/PynamicsFullDiagnostics.py similarity index 100% rename from shout-fll-scrimmage/FullDiagnostics.py rename to shout-fll-scrimmage/PynamicsFullDiagnostics.py diff --git a/shout-fll-scrimmage/HubDiagnostics.py b/shout-fll-scrimmage/PynamicsHubDiagnostics.py similarity index 100% rename from shout-fll-scrimmage/HubDiagnostics.py rename to shout-fll-scrimmage/PynamicsHubDiagnostics.py diff --git a/shout-fll-scrimmage/MotorDiagnostics.py b/shout-fll-scrimmage/PynamicsMotorDiagnostics.py similarity index 100% rename from shout-fll-scrimmage/MotorDiagnostics.py rename to shout-fll-scrimmage/PynamicsMotorDiagnostics.py