#!/usr/bin/env python3 """ PiCar-X Full Pipeline ===================== Phase 1: Calibration - tune servos, save offsets Phase 2: Line Follow - PID steering, dynamic speed, 90-degree turns, intersection handling, and line-loss recovery Run with: python3 final-calibration.py """ import os import sys from time import sleep, time import readchar # ----------------------- # TUNABLE CONSTANTS # ----------------------- # - Grayscale sensor thresholds and steering trim - # On THIS robot, the grayscale sensors read HIGHER on white and LOWER on the dark line # (verified empirically 2026-04-26 - see data/2026-04-26-sensor-baseline.txt). # Each sensor has its own scale, so we load per-sensor thresholds from a config file. # A sensor is 'on the line' when its value is BELOW its threshold. # Steering trim (mechanical offset to make 'angle 0' actually go straight) loaded the # same way - see DEVLOG entry for 2026-04-26 about the trim discovery (-6). _HW_DIR = os.path.join( os.path.dirname(os.path.abspath(__file__)), "..", "picar_hardware_test", ) _THRESH_FILE = os.path.join(_HW_DIR, "sensor_thresholds.txt") _TRIM_FILE = os.path.join(_HW_DIR, "steering_calibration.txt") def _load_thresholds(): try: with open(_THRESH_FILE) as f: parts = [p.strip() for p in f.read().strip().split(",")] return tuple(float(x) for x in parts[:3]) except Exception as e: print(f"[WARN] Could not load thresholds from {_THRESH_FILE}: {e}", flush=True) print("[WARN] Falling back to defaults (1100, 1300, 1100)", flush=True) return (1100.0, 1300.0, 1100.0) def _load_trim(): try: with open(_TRIM_FILE) as f: return int(f.read().strip()) except Exception as e: print(f"[WARN] Could not load steering trim from {_TRIM_FILE}: {e}", flush=True) return 0 DARK_THRESHOLDS = _load_thresholds() # (data[0]=right, data[1]=center, data[2]=left) STEERING_TRIM = _load_trim() # mechanical steering offset # Steering weights matched to the actual array order on this robot: # data[0] is the RIGHT sensor -> +1.0 (line on right -> positive error -> steer right) # data[2] is the LEFT sensor -> -1.0 SENSOR_WEIGHTS = (1.0, 0.0, -1.0) # - PID gains - # These were tuned down from KP=28, KI=0.3, KD=8 because the original gains # caused violent oscillation at BASE_SPEED=25. Diagnostic runs at speed=8 # with KP=12, KD=4 tracked the line cleanly. We ramped speed up modestly # while keeping the gentler gains. KP = 20.0 # proportional (turn sharper on curves to prevent understeer) KI = 0.0 # integral (off until straight tracking is reliable) KD = 5.0 # derivative (dampens oscillation) MAX_STEERING = 35 # degrees, hard cap on servo angle # - Speed - BASE_SPEED = 10 # normal cruising speed (0-100) MIN_SPEED = 6 # minimum speed while in a hard turn SPEED_REDUCTION = 0.8 # aggressive slowdown: drops to MIN_SPEED quickly on curves # Confidence-based slowdown. If the center sensor is only barely on the tape, # the robot is close to losing the line even if the instantaneous error is # near zero. Slow down before we fully lose it so PID has time to react. LOW_CONF_CENTER_DARKNESS = 160 VERY_LOW_CONF_CENTER_DARKNESS = 80 LOW_CONF_MAX_SPEED = 8 VERY_LOW_CONF_MAX_SPEED = 6 # If the line signal is weak and the current error collapses near 0, preserve # a small amount of the previous turning direction instead of snapping to # "perfectly centered". This is still controller logic, not corner detection: # it just adds continuity when the measurement gets weak. WEAK_LINE_BIAS_ERROR = 0.35 # - Intersection / turn detection - # This track has L-shaped 90 degree corners and no real intersections, so the # intersection handler is disabled for now. When all 3 sensors go dark at a # corner elbow, we want that to fall through to corner handling / recovery, # not force the robot to drive straight through the bend. ENABLE_INTERSECTION_HANDLER = False INTERSECTION_TIME = 0.7 # seconds all 3 sensors must be dark to confirm intersection TURN_DRIVE_TIME = 0.45 # seconds to drive forward before executing a 90 turn TURN_EXECUTE_TIME = 0.9 # seconds to hold full lock during a 90 turn TURN_ANGLE = 35 # steering angle during a 90 turn TURN_SPEED = 22 # speed during turn execution # Default intersection behaviour: 'left', 'right', or 'straight' INTERSECTION_DEFAULT = 'straight' # - Coast phase (line lost but maybe still close) - # When error goes None, keep driving at reduced speed with the last steering # for up to COAST_MAX cycles before triggering full recovery. Often the line # reappears on the other side of the sensor blind-spot within 2-4 cycles. COAST_MAX = 5 # max consecutive None cycles before recovery (~100ms at 50Hz) COAST_SPEED = 6 # speed during coast (slower = more time to re-acquire) # Note on COAST_MAX: keep this short. The car loses the line by overshooting # corners that are sharper than its turning radius - extending coast just # drives the car deeper into the white area before recovery can reverse. # - Recovery (line lost) - # Two modes selected automatically by |last_error|: # Drift mode (|last_error| < 0.7): gentle sweep at small angles # Corner mode (|last_error| >= 0.7): aggressive sweep at wide angles RECOVERY_SEARCH_SPEED = 6 RECOVERY_BACK_SPEED_DRIFT = 16 RECOVERY_BACK_SPEED_CORNER = 20 RECOVERY_BACK_TIME_DRIFT = 0.5 RECOVERY_BACK_TIME_CORNER = 0.8 # speed 20 * 0.8s = significant reverse MAX_RECOVERY_ATTEMPTS = 12 ALIGN_CENTER_SAMPLES = 2 # center-on-line reads to confirm alignment # If the robot stays at |error|=1.0 for too long, one side sensor is seeing # the line continuously but the center never comes back. That is not useful # tracking anymore; it means the robot is orbiting a sharp bend/corner and # needs an explicit recovery break-out. SATURATION_TIMEOUT_CYCLES = 70 # ~1.4s at 50 Hz # - Music - # Looping background mp3 that fades when the line is lost. Drop any .mp3 # into the project root (~/CS371/) and it will be picked up at startup. # Missing file or pygame failure both gracefully disable music without # affecting line-following. _PROJECT_ROOT = os.path.dirname( os.path.dirname(os.path.abspath(__file__)) ) # = ~/CS371/ MUSIC_VOLUME_ON_LINE = 1.0 # full volume while any sensor sees the line MUSIC_VOLUME_OFF_LINE = 0.0 # silence while line is lost MUSIC_FADE_PER_STEP = 0.04 # per audio thread tick; ~0.6s for full fade MUSIC_TICK_INTERVAL = 0.04 # 25 Hz audio thread # ----------------------- # UTILITY # ----------------------- def p(msg=""): """Print and immediately flush so output always appears in the terminal.""" print(msg, flush=True) def _safe_stop(px): """Best-effort motor stop with hard timeout. Will not hang the shell. Three layers of protection so a single Ctrl+C never gets you a frozen terminal: 1. SIGINT ignored during cleanup - additional Ctrl+C presses can't re-enter / re-panic the cleanup logic. 2. signal.alarm(1) per attempt - if an I2C call blocks (wedged bus), the alarm fires after 1 second and we move on instead of waiting for smbus's much longer default timeout. 3. After 3 failed attempts, os._exit(1) - bail out hard so the user is at least back at a shell prompt rather than staring at a hung script. """ import os import signal try: signal.signal(signal.SIGINT, signal.SIG_IGN) except Exception: pass def _alarm(*_): raise TimeoutError() for _ in range(3): try: signal.signal(signal.SIGALRM, _alarm) signal.alarm(1) px.stop() px.set_dir_servo_angle(0) signal.alarm(0) return except (TimeoutError, KeyboardInterrupt, Exception): try: signal.alarm(0) except Exception: pass os._exit(1) # ----------------------- # AUDIO CONTROLLER # ----------------------- class AudioController: """Background-thread MP3 player with smoothly slewed volume. Main loop calls set_target(0.0..1.0) each cycle based on line state. A daemon thread polls the target and slews the actual mixer volume toward it at MUSIC_FADE_PER_STEP per tick, so volume changes are smooth even when the main loop only updates the target sporadically (e.g. while blocked inside recover()). Any failure (no mp3, pygame missing, file unloadable) sets self.ok False and turns this into a no-op - line-following continues normally. """ def __init__(self): self.ok = False self.target = 0.0 self.current = 0.0 self._stop = None self._thread = None self._mp3 = None try: import glob mp3s = sorted(glob.glob(os.path.join(_PROJECT_ROOT, "*.mp3"))) if not mp3s: p(f"[AUDIO] no .mp3 found in {_PROJECT_ROOT} - music disabled") return self._mp3 = mp3s[0] except Exception as e: p(f"[AUDIO] file scan failed: {e}") return # The PiCar-X I2S speaker amp is gated by GPIO 20 - it has to be # pulled high before any audio reaches the speaker, otherwise pygame # plays into a silenced amplifier and you hear nothing. The official # robot_hat.Music() constructor does this via `pinctrl set 20 op dh` # as a side effect, so we instantiate it once just for that. We # don't actually use it for playback - pygame's volume API is what # we built the fade thread around. try: from robot_hat import Music as _RHMusic self._rh_music = _RHMusic() # side effect: enables speaker amp except Exception as e: p(f"[AUDIO] robot_hat.Music init failed (speaker amp may stay off): {e}") # Force SDL/pygame to use ALSA directly, bypassing PipeWire. # Raspberry Pi OS ships with PipeWire intercepting ALSA's default # device, and PipeWire's default sink is HDMI/headphones - not the # I2S speaker on the robot-hat board. /etc/asound.conf is already # configured to route ALSA's "default" through softvol -> dmixer -> # speakerbonnet (card 2 = sndrpihifiberry), so going straight to # ALSA hits the speaker correctly. os.environ.setdefault("SDL_AUDIODRIVER", "alsa") try: import pygame pygame.mixer.init() pygame.mixer.music.load(self._mp3) pygame.mixer.music.set_volume(0.0) pygame.mixer.music.play(-1) self._pygame = pygame except Exception as e: p(f"[AUDIO] pygame init/load failed for {self._mp3}: {e}") return import threading self._stop = threading.Event() self._thread = threading.Thread(target=self._run, daemon=True) self._thread.start() self.ok = True p(f"[AUDIO] looping {os.path.basename(self._mp3)}") def _run(self): while not self._stop.is_set(): if self.current < self.target: self.current = min(self.target, self.current + MUSIC_FADE_PER_STEP) elif self.current > self.target: self.current = max(self.target, self.current - MUSIC_FADE_PER_STEP) try: self._pygame.mixer.music.set_volume(self.current) except Exception: pass self._stop.wait(MUSIC_TICK_INTERVAL) def set_target(self, volume): if self.ok: self.target = max(0.0, min(1.0, volume)) def stop(self): if not self.ok: return if self._stop is not None: self._stop.set() try: self._pygame.mixer.music.stop() except Exception: pass # ----------------------- # PHASE 1 - CALIBRATION # ----------------------- def _apply_offset(px, offsets): px.dir_cali_val = offsets[0] px.cam_pan_cali_val = offsets[1] px.cam_tilt_cali_val = offsets[2] px.set_dir_servo_angle(0) px.set_cam_pan_angle(0) px.set_cam_tilt_angle(0) def _test_movement(px): p("Testing movement -- make sure the robot has room!") for angle in (0, 10, -10): px.set_dir_servo_angle(angle) px.forward(30) sleep(1) px.stop() sleep(0.5) for angle in (0, 10, -10): px.set_dir_servo_angle(angle) px.backward(30) sleep(1) px.stop() def _show_cal(servo, offsets): names = ["Steering", "Camera Pan", "Camera Tilt"] print("\033[H\033[J", end="", flush=True) p("========= PiCar-X CALIBRATION TOOL =========") p(f" Selected : {names[servo]}") p(f" Offsets : steering={offsets[0]} pan={offsets[1]} tilt={offsets[2]}") p() p(" 1/2/3 -> select servo") p(" W/D -> increase offset S/A -> decrease offset") p(" R -> test movement") p(" SPACE -> save & start line-following") p(" CTRL+C -> exit") p("=============================================") def run_calibration(px): """Interactive calibration. Returns when the user presses SPACE.""" servo = 0 # Pre-load steering trim from steering_calibration.txt so the calibration # tool starts from the value already in version control instead of zero. offsets = [STEERING_TRIM, 0, 0] _apply_offset(px, offsets) _show_cal(servo, offsets) while True: key = readchar.readkey().lower() if key in ('1', '2', '3'): servo = int(key) - 1 _show_cal(servo, offsets) elif key in ('w', 'd'): offsets[servo] += 1 _apply_offset(px, offsets) _show_cal(servo, offsets) elif key in ('s', 'a'): offsets[servo] -= 1 _apply_offset(px, offsets) _show_cal(servo, offsets) elif key == 'r': _test_movement(px) _show_cal(servo, offsets) elif key == readchar.key.SPACE: p("\nSaving calibration...") px.dir_servo_calibrate(offsets[0]) px.cam_pan_servo_calibrate(offsets[1]) px.cam_tilt_servo_calibrate(offsets[2]) # Also write the steering trim to our tracked file so it stays in # version control instead of only living in picar-x's hidden config. try: with open(_TRIM_FILE, "w") as f: f.write(f"{offsets[0]}\n") p(f"Steering trim {offsets[0]} also written to {_TRIM_FILE}") except Exception as e: p(f"[WARN] Could not write trim file: {e}") p("Calibration saved. Starting line-following in 2 seconds...") sleep(2) return # -> Phase 2 elif key == readchar.key.CTRL_C: p("Exiting.") sys.exit(0) # ----------------------- # PHASE 2 - LINE FOLLOWING HELPERS # ----------------------- def read_sensors(px): # On this robot the array is ordered [right, center, left] from the car's perspective # - NOT [left, center, right] as most SunFounder examples assume. return px.get_grayscale_data() def sensors_on_line(data): # Sensors read LOWER on the dark line, so on-line means value < per-sensor threshold. return [v < t for v, t in zip(data, DARK_THRESHOLDS)] def sensor_darkness(data): """How far below threshold each sensor is. 0 means fully off the line.""" return [max(0.0, t - v) for v, t in zip(data, DARK_THRESHOLDS)] def compute_error(data): """ Continuous position error in [-1, +1]. Negative = line is left of centre, positive = right. Instead of treating each sensor as a binary on/off bit and averaging the resulting pattern, use how far below threshold each sensor is ("darkness") as the weight. This keeps directional information when multiple sensors are on the tape at once, which is exactly what happens on wider bends and 90-degree elbows. Example: binary model: [True, False, True] -> (+1 + -1) / 2 = 0.0 analog model: if left is much darker than right, error stays negative Returns None only if no sensor is below threshold at all. """ darkness = sensor_darkness(data) total = sum(darkness) if total <= 0.0: return None weighted = sum(w * d for w, d in zip(SENSOR_WEIGHTS, darkness)) return weighted / total def is_intersection(data): return all(v < t for v, t in zip(data, DARK_THRESHOLDS)) # ----------------------- # INTERSECTION HANDLER # ----------------------- def handle_intersection(px, direction=INTERSECTION_DEFAULT): p(f"[INTERSECTION] -> {direction}") # Drive forward through the intersection box px.set_dir_servo_angle(0) px.forward(BASE_SPEED) sleep(TURN_DRIVE_TIME) if direction == 'straight': return turn_angle = -TURN_ANGLE if direction == 'left' else TURN_ANGLE px.set_dir_servo_angle(turn_angle) px.forward(TURN_SPEED) sleep(TURN_EXECUTE_TIME) px.set_dir_servo_angle(0) # ----------------------- # LINE-LOSS RECOVERY # ----------------------- def _try_align_center(px, timeout): """Drive forward slowly, return True once center sensor is on-line for ALIGN_CENTER_SAMPLES consecutive reads. Gives up after *timeout* seconds or if all sensors lose the line.""" streak = 0 start = time() while time() - start < timeout: px.forward(RECOVERY_SEARCH_SPEED) data = read_sensors(px) on = sensors_on_line(data) if on[1]: streak += 1 if streak >= ALIGN_CENTER_SAMPLES: px.stop() sleep(0.05) # DO NOT reset steering to 0 here. Leave the wheels pointed # towards the line so PID can cleanly take over. return True else: streak = 0 if not any(on): break sleep(0.04) return False def _sweep_forward(px, sweep, label, require_center=True): """Drive forward at each (angle, duration) in *sweep*, polling sensors. On first line contact, try to align center if *require_center* is True. In corner mode, just finding the line is enough -- the body is at a steep angle to the tape and the center sensor can't easily cross it. Returns True on success.""" for angle, duration in sweep: p(f"[RECOVERY] {label} {angle:+d} deg for {duration}s") px.set_dir_servo_angle(angle) end_time = time() + duration while time() < end_time: px.forward(RECOVERY_SEARCH_SPEED) data = read_sensors(px) if any(sensors_on_line(data)): if not require_center: p(f"[RECOVERY] line found at angle {angle:+d} (corner - skipping alignment)") px.stop() sleep(0.05) # Leave wheels at recovery angle for PID handoff return True p(f"[RECOVERY] line at angle {angle:+d} - aligning center") align_timeout = 1.2 if abs(angle) >= 25 else 0.6 if _try_align_center(px, align_timeout): p(f"[RECOVERY] center aligned") return True p(f"[RECOVERY] alignment failed, continuing") px.stop() sleep(0.05) break # try next angle sleep(0.04) return False def _sweep_backward(px, sweep, back_speed): """Drive backward at each (angle, duration) in *sweep*. Returns True if any sensor finds the line.""" for angle, duration in sweep: p(f"[RECOVERY] backward {angle:+d} deg for {duration}s") px.set_dir_servo_angle(angle) end_time = time() + duration while time() < end_time: px.backward(back_speed) data = read_sensors(px) if any(sensors_on_line(data)): p(f"[RECOVERY] line found backing at {angle:+d}") px.stop() sleep(0.05) return True sleep(0.04) return False def recover(px, last_error): """ Reverse-first line recovery. When the line is lost mid-tracking, by far the most common cause on this track is overshoot - the line bent or ended sharper than the car could turn, and the car drove past it. The line is *behind*, not in front. So the very first thing recovery does is reverse straight back until any sensor sees the line again. Forward sweeps only run as a last resort if reverse genuinely fails - that handles the rare case where the line drifted out of sensor reach laterally without the car overshooting it. """ if last_error is None: bias_label = "unknown" elif last_error < -0.3: bias_label = "left" elif last_error > 0.3: bias_label = "right" else: bias_label = "ahead" p(f"[RECOVERY] last_err={last_error} bias={bias_label}") # Phase 1: reverse straight back to find the overshot line. # Mostly straight-back with two mild angled probes for the rare case # the car was also rotated slightly off-axis when it overshot. # Steering capped at +/-12 deg - any sharper and the reverse arc # traces a tiny circle in place instead of translating rearward. bwd = [(0, 1.2), (-12, 0.5), (0, 0.3), ( 12, 0.5), (0, 0.3)] if _sweep_backward(px, bwd, RECOVERY_BACK_SPEED_DRIFT): return True # Phase 2 (last resort): reverse exhausted - line was not behind the car. # Try a forward sweep biased toward the side the line was last on. p("[RECOVERY] reverse exhausted - last-resort forward sweep") if last_error is not None and last_error < 0: fwd = [(-20, 0.5), (-30, 0.5), (15, 0.4)] elif last_error is not None and last_error > 0: fwd = [( 20, 0.5), ( 30, 0.5), (-15, 0.4)] else: fwd = [(0, 0.4), (-20, 0.4), (20, 0.4)] if _sweep_forward(px, fwd, "last_resort", require_center=False): return True px.stop() px.set_dir_servo_angle(0) p("[RECOVERY] Could not find line.") return False # ----------------------- # PHASE 2 - MAIN LINE-FOLLOWING LOOP # ----------------------- def run_line_following(px): print("\033[H\033[J", end="", flush=True) p("========= LINE FOLLOWING ACTIVE =========") p(" CTRL+C to stop") p("=========================================") p() # PID state integral = 0.0 prev_error = 0.0 last_error = None last_steering = 0.0 # last steering angle applied (for coast phase) # Intersection debounce intersection_start = None # Recovery counter recovery_attempts = 0 # Coast phase: consecutive cycles where error is None but we keep driving # at reduced speed with the last steering angle, hoping the line reappears # on the other side of the sensor blind-spot. coast_cycles = 0 # Rolling window of recent |error| values (last 20 cycles). Used to decide # recovery mode: if max(recent_abs_errors) >= 0.7, the robot was recently # at a corner even if last_error happened to be 0.0 from a momentary # center crossing. This prevents a single lucky reading from masking a # corner approach. RECENT_WINDOW = 20 recent_abs_errors = [] # Consecutive cycles where only one side sensor sees the line. Long runs # of saturation mean the robot is orbiting a sharp bend instead of making # progress, so we force recovery. saturation_cycles = 0 # Cycle counter and log throttle. At 50 Hz a per-cycle print is too noisy # to read live - we print every LOG_EVERY-th cycle (~5 Hz at LOG_EVERY=10), # but ALWAYS print on important events (line lost, recovery, intersection). n = 0 LOG_EVERY = 10 # Background music: fades up while tracking, fades down when off the line. audio = AudioController() # Zero everything before starting px.stop() px.set_dir_servo_angle(0) sleep(0.5) # Wait until the line is detected under at least one sensor before moving p("Waiting for line detection before starting...") while True: data = read_sensors(px) if any(sensors_on_line(data)): p(f"Line detected - starting! initial data={data}") break sleep(0.05) px.forward(BASE_SPEED) try: while True: data = read_sensors(px) # Music volume target: full volume any time at least one sensor is # on the line, silence otherwise. The audio thread slews the actual # output volume toward this target smoothly, so brief sensor blind # spots don't cause audible clicks - only sustained line loss # produces an audible fade. audio.set_target(MUSIC_VOLUME_ON_LINE if any(sensors_on_line(data)) else MUSIC_VOLUME_OFF_LINE) # - Intersection check (kept for completeness; track has none) --- if ENABLE_INTERSECTION_HANDLER and is_intersection(data): if intersection_start is None: intersection_start = time() p(f"[{n:5d}] data={data} ALL 3 DARK - intersection candidate") elif time() - intersection_start >= INTERSECTION_TIME: p(f"[{n:5d}] [INTERSECTION] confirmed - going straight") px.stop() handle_intersection(px, INTERSECTION_DEFAULT) intersection_start = None integral = 0.0 prev_error = 0.0 px.forward(BASE_SPEED) continue else: intersection_start = None # - PID error ------------------------------------ raw_error = compute_error(data) darkness = sensor_darkness(data) center_darkness = darkness[1] error = raw_error if error is None: # --- Coast phase: keep driving briefly before full recovery --- # The sensor blind-spot means the line can vanish for a few # cycles as it crosses the gap between center and side sensors. # Instead of immediately stopping for recovery (which jerks the # wheels), coast at reduced speed with the last steering angle. # If the line reappears within COAST_MAX cycles, resume PID # seamlessly -- no recovery needed. coast_cycles += 1 if coast_cycles <= COAST_MAX: px.set_dir_servo_angle(last_steering) px.forward(COAST_SPEED) if coast_cycles == 1: p(f"[{n:5d}] data={data} err=NONE last_err={last_error} -> COAST (steer={last_steering:+.1f})") n += 1 sleep(0.02) continue # Coast phase exhausted -- line is truly lost, run recovery. # Use the max of recent |error| values to pick drift vs corner # mode. This prevents a single brief center crossing from # resetting the mode to drift when we're clearly at a corner. recent_max = max(recent_abs_errors) if recent_abs_errors else 0.0 effective_error = last_error if recent_max >= 0.7 and (effective_error is None or abs(effective_error) < 0.7): # Override: recent history says corner, even if last_error doesn't effective_error = recent_max if (last_error is None or last_error >= 0) else -recent_max p(f"[{n:5d}] data={data} err=NONE coast={coast_cycles} last_err={last_error} recent_max={recent_max:.2f} -> RECOVERY (override to corner)") else: p(f"[{n:5d}] data={data} err=NONE coast={coast_cycles} last_err={last_error} recent_max={recent_max:.2f} -> RECOVERY") px.stop() recovered = recover(px, effective_error) recovery_attempts += 1 coast_cycles = 0 recent_abs_errors.clear() if not recovered or recovery_attempts >= MAX_RECOVERY_ATTEMPTS: p("[STOP] Line lost -- could not recover.") break # Post-recovery directional turn. If the line was clearly biased # to one side before we lost it, the car likely overshot a corner # bending that way. Recovery found the line again, but if we hand # straight back to PID at a corner apex (all 3 sensors dark), # PID sees error=0 and drives straight - right back off the apex # in the same direction we just overshot from. So we explicitly # rotate the chassis toward the bias side first, slow forward, # so we exit the apex pointing along the new line direction. if effective_error is not None and abs(effective_error) >= 0.4: bias_angle = -30 if effective_error < 0 else 30 p(f"[POST_RECOVERY] turning {bias_angle:+d} deg for 0.5s " f"(last_err={effective_error:+.2f})") px.set_dir_servo_angle(bias_angle) px.forward(MIN_SPEED) sleep(0.5) px.stop() sleep(0.05) # Do not blindly command BASE_SPEED here. We just recovered, # the wheels might be at a steep angle, and we are stopped. # Just reset PID state and continue; the next loop iteration # will immediately read sensors and calculate the correct # dynamic speed based on the new error. integral = 0.0 prev_error = 0.0 n += 1 continue recovery_attempts = 0 coast_cycles = 0 # Track recent |error| for recovery mode selection recent_abs_errors.append(abs(error)) if len(recent_abs_errors) > RECENT_WINDOW: recent_abs_errors.pop(0) # Weak-line steering continuity. If the line is fading under the # center sensor and the new error collapses near zero, keep a small # bias in the previous turn direction. This helps the robot keep # following the actual tape instead of driving straight until loss. if (abs(error) < 0.15 and center_darkness < LOW_CONF_CENTER_DARKNESS and last_error is not None and abs(last_error) >= 0.3): error = WEAK_LINE_BIAS_ERROR if last_error > 0 else -WEAK_LINE_BIAS_ERROR last_error = error if abs(error) >= 0.85: saturation_cycles += 1 else: saturation_cycles = 0 if saturation_cycles >= SATURATION_TIMEOUT_CYCLES: on = sensors_on_line(data) p(f"[{n:5d}] data={data} on={on} err={error:+.2f} saturated too long -> RECOVERY") px.stop() recovered = recover(px, error) recovery_attempts += 1 saturation_cycles = 0 if not recovered or recovery_attempts >= MAX_RECOVERY_ATTEMPTS: p("[STOP] Line lost -- could not recover.") break px.forward(BASE_SPEED) integral = 0.0 prev_error = 0.0 n += 1 continue # - PID calculation ------------------------------ integral += error integral = max(-10.0, min(10.0, integral)) # anti-windup derivative = error - prev_error prev_error = error # Saturation override. With weights (+1, 0, -1), |error|=1.0 # means ONLY a side sensor is on the line - we have lost precise # position information and the linear PID response (KP * 1.0 = # 14 deg) is not enough to chase the line on tighter curves or # at corner approach. Use full steering lock toward the line in # that case. PID still runs for non-saturated errors where its # gradient is meaningful. if abs(error) >= 0.85: steering = -MAX_STEERING if error < 0 else MAX_STEERING else: raw_steering = KP * error + KI * integral + KD * derivative steering = max(-MAX_STEERING, min(MAX_STEERING, raw_steering)) # - Dynamic speed -------------------------------- speed = BASE_SPEED * (1.0 - SPEED_REDUCTION * abs(error)) if center_darkness < LOW_CONF_CENTER_DARKNESS: speed = min(speed, LOW_CONF_MAX_SPEED) if center_darkness < VERY_LOW_CONF_CENTER_DARKNESS: speed = min(speed, VERY_LOW_CONF_MAX_SPEED) speed = max(MIN_SPEED, speed) if all(sensors_on_line(data)): # All 3 sensors dark! We are crossing a perpendicular line (a corner). # Brake immediately so we don't overshoot it at full speed. speed = MIN_SPEED # - Apply ---------------------------------------- px.set_dir_servo_angle(steering) px.forward(speed) last_steering = steering # save for coast phase # - Throttled telemetry -------------------------- # Always log a snapshot every LOG_EVERY cycles. Also log # immediately whenever |error| is at saturation (only one side # sensor on line) so we can study what happens at corners. if n % LOG_EVERY == 0 or abs(error) >= 0.85: on = sensors_on_line(data) p(f"[{n:5d}] data={data} on={on} err={error:+.2f} raw={raw_error if raw_error is not None else None} c_dark={center_darkness:4.0f} steer={steering:+5.1f} spd={speed:4.1f}") n += 1 sleep(0.02) # ~50 Hz loop except KeyboardInterrupt: p("\nStopped by user.") finally: audio.stop() _safe_stop(px) # ----------------------- # ENTRY POINT # ----------------------- if __name__ == "__main__": p("Initializing PiCar-X hardware...") try: from picarx import Picarx px = Picarx() except Exception as e: p(f"ERROR: Could not initialize Picarx: {e}") p("Make sure you are running on the Raspberry Pi with the PiCar-X library installed.") sys.exit(1) # Apply steering trim from steering_calibration.txt before doing anything else. px.dir_cali_val = STEERING_TRIM px.set_dir_servo_angle(0) p(f"Hardware ready. Loaded thresholds={DARK_THRESHOLDS} trim={STEERING_TRIM}") p(f"PID: KP={KP} KI={KI} KD={KD} BASE_SPEED={BASE_SPEED}") sleep(0.5) try: run_calibration(px) run_line_following(px) finally: _safe_stop(px)