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