Files
bioglow_solutions/shout-fll-scrimmage/bioglow_helpers.py

177 lines
6.6 KiB
Python

"""
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_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)
)