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