2026-10-20 23:14:43 +00:00
########################################################
# 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 ( ) :
2026-10-26 00:11:58 +00:00
drive_base . settings ( 800 , 600 , 300 , 200 )
2026-10-20 23:14:43 +00:00
# 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 )
2026-10-26 00:11:58 +00:00
async def multilift ( rotation_angle ) :
2026-10-20 23:14:43 +00:00
await multitask (
right_arm . run_angle ( 400 , rotation_angle ) ,
left_arm . run_angle ( 400 , - 1 * rotation_angle )
)
2026-10-26 00:11:58 +00:00
2026-10-20 23:14:43 +00:00