Upload files to "hazmat_scrimmage"
This commit is contained in:
35
hazmat_scrimmage/PynamicsHubDiagnostics.py
Normal file
35
hazmat_scrimmage/PynamicsHubDiagnostics.py
Normal 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)
|
||||||
136
hazmat_scrimmage/PynamicsMotorDiagnostics.py
Normal file
136
hazmat_scrimmage/PynamicsMotorDiagnostics.py
Normal 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()
|
||||||
213
hazmat_scrimmage/bioglow_helpers.py
Normal file
213
hazmat_scrimmage/bioglow_helpers.py
Normal 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())
|
||||||
|
|
||||||
31
hazmat_scrimmage/bioglow_mission_1.py
Normal file
31
hazmat_scrimmage/bioglow_mission_1.py
Normal 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())
|
||||||
36
hazmat_scrimmage/bioglow_mission_11_experimental.py
Normal file
36
hazmat_scrimmage/bioglow_mission_11_experimental.py
Normal 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())
|
||||||
Reference in New Issue
Block a user