diff --git a/hazmat_scrimmage/PynamicsHubDiagnostics.py b/hazmat_scrimmage/PynamicsHubDiagnostics.py new file mode 100644 index 0000000..9bdf436 --- /dev/null +++ b/hazmat_scrimmage/PynamicsHubDiagnostics.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/hazmat_scrimmage/PynamicsMotorDiagnostics.py b/hazmat_scrimmage/PynamicsMotorDiagnostics.py new file mode 100644 index 0000000..b8e762b --- /dev/null +++ b/hazmat_scrimmage/PynamicsMotorDiagnostics.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/hazmat_scrimmage/bioglow_helpers.py b/hazmat_scrimmage/bioglow_helpers.py new file mode 100644 index 0000000..21dc5bd --- /dev/null +++ b/hazmat_scrimmage/bioglow_helpers.py @@ -0,0 +1,213 @@ +######################################################## +# bioglow_helpers.py +# - This file contain all the helper functions and tools +# - This is where we add special or custom functionality codes +######################################################## + +""" +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_robot_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) + ) + +async def drive_until_stalled(max_distance): + stalled_flag = False + + async def drive_task(): + await drive_base.straight(max_distance) + + async def stall_watch(): + nonlocal stalled_flag + while not (left_motor.control.stalled() and right_motor.control.stalled()): + await wait(10) + stalled_flag = True + drive_base.stop() + print("Both wheels stalled — robot is stuck!") + + await multitask(drive_task(), stall_watch(), race=True) + return stalled_flag + +def reset_arms(): + left_arm.reset_angle(0) + right_arm.reset_angle(0) + print('limits before:') + print(left_arm.control.limits()) + print(left_arm.control.limits()) + + left_arm.control.limits(speed=1100, acceleration=2000) + right_arm.control.limits(speed=1100, acceleration=2000) + + print('limits after:') + print(left_arm.control.limits()) + print(left_arm.control.limits()) + diff --git a/hazmat_scrimmage/bioglow_mission_1.py b/hazmat_scrimmage/bioglow_mission_1.py new file mode 100644 index 0000000..de8021b --- /dev/null +++ b/hazmat_scrimmage/bioglow_mission_1.py @@ -0,0 +1,31 @@ +##################################### +# Solution for BioGlow Mission 1 +##################################### + +import bioglow_helpers as helper +from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings + +async def solve_missions(): # Drone Survey - Johannes + + await multitask( + left_arm.run_angle(400,-300), + right_arm.run_angle(400,100), + drive_base.straight(900) + ) + await drive_base.turn(-25) + await drive_base.straight(110) + await drive_base.turn(-35) + await drive_base.turn(35) + await drive_base.straight(-30) + await drive_base.turn(25) + await drive_base.straight(-200) + await drive_base.turn(-45) + await drive_base.straight(500) + await drive_base.turn(75) + await drive_base.straight(700) + await drive_base.stop + + + +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/hazmat_scrimmage/bioglow_mission_11_experimental.py b/hazmat_scrimmage/bioglow_mission_11_experimental.py new file mode 100644 index 0000000..e7223f4 --- /dev/null +++ b/hazmat_scrimmage/bioglow_mission_11_experimental.py @@ -0,0 +1,36 @@ +##################################### +# Solution for BioGlow Mission 11 +##################################### + +import bioglow_helpers as helper +from bioglow_robot_settings import * # drive_base, left_arm, right_arm, ... all come from settings + +async def solve_missions(): # Windows to the past + + drive_base.reset(0,0) # always reset drivebase for heading reset especially with turns + + await multitask( + right_arm.run_angle(800,-90), + drive_base.straight(100) + ) + await drive_base.turn(-45) + await drive_base.straight(420) + helper.set_speed(100,100,100,100) + await multitask( + left_arm.run_angle(500,45), + right_arm.run_angle(800,140) + ) + helper.set_default_speed() + + #return home + await drive_base.straight(-500) + await multitask( + left_arm.run_angle(100,-45), + right_arm.run_angle(800,-90) + ) + #helper.reset_arms() + helper.set_default_speed() + drive_base.stop() + +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