Upload files to "hazmat_scrimmage"

This commit is contained in:
2026-10-20 23:14:43 +00:00
parent 81842d5a75
commit 1ddab4463e
5 changed files with 451 additions and 0 deletions

View File

@@ -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)

View File

@@ -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()

View File

@@ -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())

View File

@@ -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())

View File

@@ -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())