From 5d66c43ff3859e603ffdf46b24297e5043820bd9 Mon Sep 17 00:00:00 2001
From: Christopher Larrieu <christopher@larrieu.org>
Date: Thu, 26 Feb 2026 18:53:49 -0500
Subject: [PATCH 1/4] Add performance profiling and remove blocking CAN calls

Phase 1 of performance optimization work ported from performance-tweaks-2.

Profiling Infrastructure:
- Robot.java: Track scheduler and gameState timing (>20ms threshold)
- Drive.java: Track lock, gyro, modules, odometry timing (>5ms threshold)
- Module.java: Track updateInputs and logging timing (>2ms threshold)
- Vision.java: Track copyInputs, cameraLoop, consumer timing (>5ms threshold)
- Intake.java: Track update and logging timing (>2ms threshold)
- Feeder.java: Track spindexer and kicker timing (>2ms threshold)

CAN Optimization:
- Remove BaseStatusSignal.refreshAll() calls that block on CAN responses
- Rely on Phoenix 6 auto-update at configured frequency (50Hz telemetry)
- Position signals for odometry handled by PhoenixOdometryThread at 250Hz
- Check connection status via .getStatus().isOK() instead of blocking refresh

Files modified:
- Drive.java: Remove batched signal refresh, add profiling
- Module.java: Remove refreshSignals()/getStatusSignals(), add profiling
- ModuleIO.java: Remove interface methods for signal refresh
- ModuleIOTalonFX.java: Remove allSignals array and refresh methods
- IntakeRollerIOTalonFX.java: Use non-blocking status checks
- FlywheelIOTalonFX.java: Use non-blocking status checks

Documentation:
- Add doc/TWEAKS3.md explaining changes and methodology
---
 doc/TWEAKS3.md                                | 95 +++++++++++++++++++
 src/main/java/frc/robot/Robot.java            | 19 ++++
 .../frc/robot/subsystems/drive/Drive.java     | 52 +++++-----
 .../frc/robot/subsystems/drive/Module.java    | 30 +++---
 .../frc/robot/subsystems/drive/ModuleIO.java  | 11 ---
 .../subsystems/drive/ModuleIOTalonFX.java     | 29 +-----
 .../frc/robot/subsystems/feeder/Feeder.java   | 22 +++++
 .../frc/robot/subsystems/intake/Intake.java   | 16 ++++
 .../intake/IntakeRollerIOTalonFX.java         | 10 +-
 .../launcher/FlywheelIOTalonFX.java           | 10 +-
 .../frc/robot/subsystems/vision/Vision.java   | 24 +++++
 11 files changed, 239 insertions(+), 79 deletions(-)
 create mode 100644 doc/TWEAKS3.md

diff --git a/doc/TWEAKS3.md b/doc/TWEAKS3.md
new file mode 100644
index 0000000..086b7c5
--- /dev/null
+++ b/doc/TWEAKS3.md
@@ -0,0 +1,95 @@
+# Performance Tweaks 3
+
+This document describes the performance optimization work done on the `performance-tweaks-3` branch, building on the foundation established in the `performance-tweaks-2` branch.
+
+## Executive Summary
+
+This branch ports the profiling infrastructure and CAN optimization changes from `performance-tweaks-2` to the current `main` branch. The goal is to identify and reduce loop overruns caused by blocking I/O operations.
+
+---
+
+## Changes Implemented
+
+### 1. Profiling Infrastructure
+
+Timing instrumentation has been added throughout the codebase to identify bottlenecks empirically. Each subsystem reports timing breakdowns when thresholds are exceeded:
+
+| File | Method | Threshold | Metrics |
+|------|--------|-----------|---------|
+| `Robot.java` | `robotPeriodic()` | >20ms | scheduler, gameState |
+| `Drive.java` | `periodic()` | >5ms | lock, gyroUpdate, gyroLog, modules, disabled, odometry |
+| `Module.java` | `periodic()` | >2ms | updateInputs, log, rest |
+| `Vision.java` | `periodic()` | >5ms | copyInputs, cameraLoop, consumer, summaryLog |
+| `Intake.java` | `periodic()` | >2ms | update, log |
+| `Feeder.java` | `periodic()` | >2ms | spindexer, kicker, spindexerLog, kickerLog |
+
+**Example profiling output:**
+```
+[Drive] lock=0ms gyroUpdate=2ms gyroLog=1ms modules=105ms disabled=69ms odometry=144ms total=322ms
+[Robot] scheduler=350ms gameState=5ms total=355ms
+```
+
+### 2. Removed Blocking CAN Refreshes
+
+The codebase was calling `BaseStatusSignal.refreshAll()` synchronously in the main loop, which blocks until CAN responses are received. This has been replaced with relying on Phoenix 6's automatic signal updates.
+
+**Files Changed:**
+- `Drive.java` - Removed batched refresh of all module signals
+- `Module.java` - Removed `refreshSignals()` and `getStatusSignals()` methods
+- `ModuleIO.java` - Removed interface methods for signal refresh
+- `ModuleIOTalonFX.java` - Removed `allSignals` array and refresh methods
+- `IntakeRollerIOTalonFX.java` - Changed from `refreshAll()` to checking `.getStatus().isOK()`
+- `FlywheelIOTalonFX.java` - Changed from `refreshAll()` to checking `.getStatus().isOK()`
+
+**Why this works:**
+- Phoenix 6 automatically updates status signals at the configured frequency (50Hz for telemetry, 250Hz for odometry)
+- Position signals for odometry are handled by `PhoenixOdometryThread` at 250Hz in the background
+- The main loop reads cached values (memory read in nanoseconds) instead of blocking on CAN (milliseconds)
+
+---
+
+## Background Threading (Pre-existing)
+
+The codebase already implements background threading for sensor reads:
+
+- **PhoenixOdometryThread** - Reads TalonFX position signals at 250Hz
+- **SparkOdometryThread** - Reads SparkMax telemetry in background
+- **VisionThread** - Processes camera data asynchronously
+- **CanandgyroThread** - Reads gyro data in background
+
+---
+
+## Future Work (Phase 2)
+
+The following changes from `performance-tweaks-2` still need to be adapted for the current `main` branch:
+
+1. **Deferred Logging in Launcher** - Cache values during `aim()`, log them in `periodic()`
+2. **Zero-Velocity Guard** - Prevent `Translation2d.getAngle()` errors when velocity is zero
+3. **Launcher Profiling** - Add timing instrumentation to `periodic()` and `aim()`
+
+These changes require more careful adaptation due to structural differences in `Launcher.java` between the branches.
+
+---
+
+## Files Modified
+
+| File | Changes |
+|------|---------|
+| `Robot.java` | Added profiling to `robotPeriodic()` |
+| `Drive.java` | Removed blocking CAN refresh, added granular profiling |
+| `Module.java` | Added profiling, removed signal methods |
+| `ModuleIO.java` | Removed `refreshSignals()` and `getStatusSignals()` interface methods |
+| `ModuleIOTalonFX.java` | Removed signal caching and refresh code |
+| `Vision.java` | Added profiling to `periodic()` |
+| `Intake.java` | Added profiling to `periodic()` |
+| `Feeder.java` | Added profiling to `periodic()` |
+| `IntakeRollerIOTalonFX.java` | Changed to non-blocking status checks |
+| `FlywheelIOTalonFX.java` | Changed to non-blocking status checks |
+
+---
+
+## References
+
+- [Chief Delphi: Let's talk about CAN delays](https://www.chiefdelphi.com/t/lets-talk-about-can-delays-and-how-to-address-them/396566)
+- [Phoenix 6 Status Signals Documentation](https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/api-usage/status-signals.html)
+- [AdvantageKit TalonFX Swerve Template](https://docs.advantagekit.org/getting-started/template-projects/talonfx-swerve-template)
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 55a048a..91a7c5e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -270,6 +270,8 @@ public Robot() {
   /** This function is called periodically during all modes. */
   @Override
   public void robotPeriodic() {
+    long loopStart = System.nanoTime();
+
     // Optionally switch the thread to high priority to improve loop
     // timing (see the template project documentation for details)
     // Threads.setCurrentThreadPriority(true, 99);
@@ -280,8 +282,25 @@ public void robotPeriodic() {
     // This must be called from the robot's periodic block in order for anything in
     // the Command-based framework to work.
     CommandScheduler.getInstance().run();
+    long t1 = System.nanoTime();
 
     GameState.logValues();
+    long t2 = System.nanoTime();
+
+    // Profiling output
+    long schedulerMs = (t1 - loopStart) / 1_000_000;
+    long gameStateMs = (t2 - t1) / 1_000_000;
+    long totalMs = (t2 - loopStart) / 1_000_000;
+    if (totalMs > 20) {
+      System.out.println(
+          "[Robot] scheduler="
+              + schedulerMs
+              + "ms gameState="
+              + gameStateMs
+              + "ms total="
+              + totalMs
+              + "ms");
+    }
 
     // Return to non-RT thread priority (do not modify the first argument)
     // Threads.setCurrentThreadPriority(false, 10);
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index ecbf9fb..6a8739e 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -17,7 +17,6 @@
 import static frc.robot.subsystems.drive.DriveConstants.*;
 
 import choreo.trajectory.SwerveSample;
-import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.CANBus;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.PIDConstants;
@@ -86,9 +85,6 @@ public class Drive extends SubsystemBase {
   private SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
   private ChassisSpeeds chassisSpeeds;
 
-  // Cached array of all module signals for batched refresh
-  private BaseStatusSignal[] allModuleSignals;
-
   // PID controllers for following Choreo trajectories
   private final PIDController xController = new PIDController(5.0, 0.0, 0.0);
   private final PIDController yController = new PIDController(5.0, 0.0, 0.0);
@@ -146,36 +142,26 @@ public Drive(
                 (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
 
     headingController.enableContinuousInput(-Math.PI, Math.PI);
-
-    // Collect all module signals for batched refresh
-    int totalSignals = 0;
-    for (var module : modules) {
-      totalSignals += module.getStatusSignals().length;
-    }
-    allModuleSignals = new BaseStatusSignal[totalSignals];
-    int signalIndex = 0;
-    for (var module : modules) {
-      for (var signal : module.getStatusSignals()) {
-        allModuleSignals[signalIndex++] = signal;
-      }
-    }
   }
 
   @Override
   public void periodic() {
     long startNanos = System.nanoTime();
 
-    // Refresh all module signals in a single batched CAN call
-    if (allModuleSignals.length > 0) {
-      BaseStatusSignal.refreshAll(allModuleSignals);
-    }
+    // No explicit refresh - Phoenix 6 auto-updates signals at configured frequency (50Hz)
+    // Position signals for odometry are handled by PhoenixOdometryThread at 250Hz
+    // This avoids blocking CAN calls in the main loop
 
     odometryLock.lock(); // Prevents odometry updates while reading data
+    long t1 = System.nanoTime();
     gyroIO.updateInputs(gyroInputs);
+    long t2 = System.nanoTime();
     Logger.processInputs("Drive/Gyro", gyroInputs);
+    long t3 = System.nanoTime();
     for (var module : modules) {
       module.periodic();
     }
+    long t4 = System.nanoTime();
     odometryLock.unlock();
 
     // Stop moving when disabled
@@ -190,6 +176,7 @@ public void periodic() {
       Logger.recordOutput("SwerveStates/Setpoints", emptyModuleStates);
       Logger.recordOutput("SwerveStates/SetpointsOptimized", emptyModuleStates);
     }
+    long t5 = System.nanoTime();
 
     // Update odometry
     double[] sampleTimestamps =
@@ -223,10 +210,31 @@ public void periodic() {
 
       chassisSpeeds = kinematics.toChassisSpeeds(getModuleStates());
     }
+    long t6 = System.nanoTime();
 
     // Update gyro alert
     gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM);
-    Logger.recordOutput("Drive/Millis", (System.nanoTime() - startNanos) / 1_000_000L);
+
+    // Profiling output
+    long totalMs = (t6 - startNanos) / 1_000_000;
+    if (totalMs > 5) {
+      System.out.println(
+          "[Drive] lock="
+              + (t1 - startNanos) / 1_000_000
+              + "ms gyroUpdate="
+              + (t2 - t1) / 1_000_000
+              + "ms gyroLog="
+              + (t3 - t2) / 1_000_000
+              + "ms modules="
+              + (t4 - t3) / 1_000_000
+              + "ms disabled="
+              + (t5 - t4) / 1_000_000
+              + "ms odometry="
+              + (t6 - t5) / 1_000_000
+              + "ms total="
+              + totalMs
+              + "ms");
+    }
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
index 7f5a691..e7478b9 100644
--- a/src/main/java/frc/robot/subsystems/drive/Module.java
+++ b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -15,7 +15,6 @@
 
 import static frc.robot.subsystems.drive.DriveConstants.*;
 
-import com.ctre.phoenix6.BaseStatusSignal;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -54,8 +53,11 @@ public Module(ModuleIO io, int index) {
   }
 
   public void periodic() {
+    long t0 = System.nanoTime();
     io.updateInputs(inputs);
+    long t1 = System.nanoTime();
     Logger.processInputs("Drive/Module" + Integer.toString(index), inputs);
+    long t2 = System.nanoTime();
 
     // Calculate positions for odometry
     int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
@@ -69,6 +71,22 @@ public void periodic() {
     // Update alerts
     driveDisconnectedAlert.set(!inputs.driveConnected);
     turnDisconnectedAlert.set(!inputs.turnConnected);
+    long t3 = System.nanoTime();
+
+    // Profiling output
+    long totalMs = (t3 - t0) / 1_000_000;
+    if (totalMs > 2) {
+      System.out.println(
+          "[Module"
+              + index
+              + "] updateInputs="
+              + (t1 - t0) / 1_000_000
+              + "ms log="
+              + (t2 - t1) / 1_000_000
+              + "ms rest="
+              + (t3 - t2) / 1_000_000
+              + "ms");
+    }
   }
 
   /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */
@@ -145,14 +163,4 @@ public void setTurnZero() {
     io.setTurnZero(newTurnZero);
     Preferences.setDouble(zeroRotationKey + index, newTurnZero.getRadians());
   }
-
-  /** Refreshes the status signals (if applicable) before updating inputs. */
-  public void refreshSignals() {
-    io.refreshSignals();
-  }
-
-  /** Returns the status signals for batched refresh. */
-  public BaseStatusSignal[] getStatusSignals() {
-    return io.getStatusSignals();
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
index 0bb475f..fcefc7a 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
@@ -13,7 +13,6 @@
 
 package frc.robot.subsystems.drive;
 
-import com.ctre.phoenix6.BaseStatusSignal;
 import edu.wpi.first.math.geometry.Rotation2d;
 import org.littletonrobotics.junction.AutoLog;
 
@@ -57,14 +56,4 @@ public default void setTurnPosition(Rotation2d rotation) {}
 
   /** Update the turn zero position of the turn absolute encoder */
   public default void setTurnZero(Rotation2d rotation) {}
-
-  /** Refreshes the status signals (if applicable) before updating inputs. */
-  public default void refreshSignals() {}
-
-  /**
-   * Returns the status signals for batched refresh. Override in implementations that use Phoenix.
-   */
-  public default BaseStatusSignal[] getStatusSignals() {
-    return new BaseStatusSignal[0];
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
index 54a0c65..0e5d61f 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -183,37 +183,12 @@ public ModuleIOTalonFX(
         turnAppliedVolts,
         turnCurrent);
     ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon);
-
-    // Cache all signals for batched refresh
-    allSignals =
-        new BaseStatusSignal[] {
-          drivePosition,
-          driveVelocity,
-          driveAppliedVolts,
-          driveCurrent,
-          turnPosition,
-          turnVelocity,
-          turnAppliedVolts,
-          turnCurrent,
-          turnAbsolutePosition
-        };
-  }
-
-  // Cached array for getStatusSignals() to avoid allocation per call
-  private BaseStatusSignal[] allSignals;
-
-  @Override
-  public void refreshSignals() {
-    BaseStatusSignal.refreshAll(allSignals);
-  }
-
-  @Override
-  public BaseStatusSignal[] getStatusSignals() {
-    return allSignals;
   }
 
   @Override
   public void updateInputs(ModuleIOInputs inputs) {
+    // No explicit refresh - Phoenix 6 auto-updates signals at configured frequency (50Hz)
+    // This avoids blocking CAN calls in the main loop
 
     // Update drive inputs
     inputs.driveConnected =
diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java
index 684e3d1..d259c2b 100644
--- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java
+++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java
@@ -28,14 +28,36 @@ public Feeder(SpindexerIO spindexerIO, KickerIO kickerIO) {
 
   @Override
   public void periodic() {
+    long t0 = System.nanoTime();
     spindexerIO.updateInputs(spindexerInputs);
+    long t1 = System.nanoTime();
     kickerIO.updateInputs(kickerInputs);
+    long t2 = System.nanoTime();
 
     Logger.processInputs("Spindexer", spindexerInputs);
+    long t3 = System.nanoTime();
     Logger.processInputs("Kicker", kickerInputs);
+    long t4 = System.nanoTime();
 
     spindexerDisconnectedAlert.set(!spindexerInputs.connected);
     kickerDisconnectedAlert.set(!kickerInputs.connected);
+
+    // Profiling output
+    long totalMs = (t4 - t0) / 1_000_000;
+    if (totalMs > 2) {
+      System.out.println(
+          "[Feeder] spindexer="
+              + (t1 - t0) / 1_000_000
+              + "ms kicker="
+              + (t2 - t1) / 1_000_000
+              + "ms spindexerLog="
+              + (t3 - t2) / 1_000_000
+              + "ms kickerLog="
+              + (t4 - t3) / 1_000_000
+              + "ms total="
+              + totalMs
+              + "ms");
+    }
   }
 
   public void stop() {
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 32f0ac0..b64b522 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -30,13 +30,29 @@ public Intake(IntakeRollerIO intakeRollerIO, IntakeArmIO intakeArmIO) {
 
   @Override
   public void periodic() {
+    long t0 = System.nanoTime();
     intakeRollerIO.updateInputs(intakeRollerInputs);
     intakeArmIO.updateInputs(intakeArmInputs);
+    long t1 = System.nanoTime();
 
     Logger.processInputs("IntakeRoller", intakeRollerInputs);
     Logger.processInputs("IntakeArm", intakeArmInputs);
+    long t2 = System.nanoTime();
 
     disconnectedAlert.set(!intakeRollerInputs.connected);
+
+    // Profiling output
+    long totalMs = (t2 - t0) / 1_000_000;
+    if (totalMs > 2) {
+      System.out.println(
+          "[Intake] update="
+              + (t1 - t0) / 1_000_000
+              + "ms log="
+              + (t2 - t1) / 1_000_000
+              + "ms total="
+              + totalMs
+              + "ms");
+    }
   }
 
   public void stop() {
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java
index d812279..39eaed4 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java
@@ -78,18 +78,20 @@ public IntakeRollerIOTalonFX() {
 
   @Override
   public void updateInputs(IntakeRollerIOInputs inputs) {
+    // No explicit refresh - Phoenix 6 auto-updates signals at configured frequency (50Hz)
+    // This avoids blocking CAN calls in the main loop
     inputs.connected =
         connectedDebounce.calculate(
-            BaseStatusSignal.refreshAll(
-                    intakeVelocity, intakeAcceleration, intakeAppliedVolts, intakeCurrent)
-                .isOK());
+            intakeVelocity.getStatus().isOK()
+                && intakeAcceleration.getStatus().isOK()
+                && intakeAppliedVolts.getStatus().isOK()
+                && intakeCurrent.getStatus().isOK());
 
     inputs.appliedVolts = intakeAppliedVolts.getValueAsDouble();
     inputs.currentAmps = intakeCurrent.getValueAsDouble();
     inputs.velocityMetersPerSec =
         intakeVelocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction;
 
-    BaseStatusSignal.refreshAll(followerCurrent, followerAppliedVolts);
     Logger.recordOutput("Intake/Follower/Current", followerCurrent.getValue());
     Logger.recordOutput("Intake/Follower/Volts", followerAppliedVolts.getValue());
   }
diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java
index 84ee4f1..37e40b8 100644
--- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java
@@ -81,11 +81,14 @@ public FlywheelIOTalonFX() {
 
   @Override
   public void updateInputs(FlywheelIOInputs inputs) {
+    // No explicit refresh - Phoenix 6 auto-updates signals at configured frequency (50Hz)
+    // This avoids blocking CAN calls in the main loop
     inputs.connected =
         connectedDebounce.calculate(
-            BaseStatusSignal.refreshAll(
-                    flywheelVelocity, flywheelAcceleration, flywheelAppliedVolts, flywheelCurrent)
-                .isOK());
+            flywheelVelocity.getStatus().isOK()
+                && flywheelAcceleration.getStatus().isOK()
+                && flywheelAppliedVolts.getStatus().isOK()
+                && flywheelCurrent.getStatus().isOK());
 
     inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble();
     inputs.currentAmps = flywheelCurrent.getValueAsDouble();
@@ -93,7 +96,6 @@ public void updateInputs(FlywheelIOInputs inputs) {
         (flywheelVelocity.getValue().in(RadiansPerSecond) * wheelRadius.in(Meters))
             / motorReduction;
 
-    BaseStatusSignal.refreshAll(followerCurrent, followerAppliedVolts);
     Logger.recordOutput("Flywheel/Follower/Current", followerCurrent.getValue());
     Logger.recordOutput("Flywheel/Follower/Volts", followerAppliedVolts.getValue());
   }
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
index 09c073f..7634a96 100644
--- a/src/main/java/frc/robot/subsystems/vision/Vision.java
+++ b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -114,11 +114,14 @@ public Rotation2d getTargetX(int cameraIndex) {
 
   @Override
   public void periodic() {
+    long visionStart = System.nanoTime();
+
     for (int i = 0; i < io.length; i++) {
       // Copy cached inputs from background thread to inputs for AdvantageKit logging
       visionInputs[i].getSnapshot().copyTo(inputs[i]);
       Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]);
     }
+    long t1 = System.nanoTime();
 
     // Initialize logging values
     allTagPoses.clear();
@@ -200,6 +203,8 @@ public void periodic() {
       allRobotPosesRejected.addAll(robotPosesRejected);
     }
 
+    long t2 = System.nanoTime();
+
     // Remove unacceptable observations
     observations.removeIf(o -> o.score < minScore);
 
@@ -220,6 +225,7 @@ public void periodic() {
 
       Logger.recordOutput("Vision/Summary/ObservationScore", o.score);
     }
+    long t3 = System.nanoTime();
 
     // Log summary data
     if (kLogSummaryPoses) {
@@ -234,6 +240,24 @@ public void periodic() {
       Logger.recordOutput(
           "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(Pose3d[]::new));
     }
+    long t4 = System.nanoTime();
+
+    // Profiling output
+    long totalMs = (t4 - visionStart) / 1_000_000;
+    if (totalMs > 5) {
+      System.out.println(
+          "[Vision] copyInputs="
+              + (t1 - visionStart) / 1_000_000
+              + "ms cameraLoop="
+              + (t2 - t1) / 1_000_000
+              + "ms consumer="
+              + (t3 - t2) / 1_000_000
+              + "ms summaryLog="
+              + (t4 - t3) / 1_000_000
+              + "ms total="
+              + totalMs
+              + "ms");
+    }
   }
 
   @FunctionalInterface

From 55abc792dcb08ea66db913a781e4fdb5bfe46280 Mon Sep 17 00:00:00 2001
From: Christopher Larrieu <christopher@larrieu.org>
Date: Thu, 26 Feb 2026 19:00:37 -0500
Subject: [PATCH 2/4] Add deferred logging and profiling to Launcher
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Phase 2 of performance optimization work.

Profiling:
- Launcher.periodic(): Track update, log, aimLog, ballistics timing (>3ms)
- Launcher.aim(): Track v0nom, baseSpeeds, v0replan, setPos timing (>500μs)

Deferred Logging:
- Cache values in aim() instead of calling Logger.recordOutput() directly
- Log cached data in periodic() via logCachedAimData()
- Moves Logger overhead out of time-critical aim() hot path

Zero-Velocity Guard:
- Check v_r < 1e-6 before calling Translation2d.getAngle()
- Prevents ArithmeticException or NaN when velocity is near zero

Method Renames:
- getV0() → getV0Nominal() (for impact angle calculation)
- getV0() → getV0Replanned() (for flywheel speed recalculation)
---
 doc/TWEAKS3.md                                |  29 ++++-
 .../robot/subsystems/launcher/Launcher.java   | 120 +++++++++++++++---
 2 files changed, 125 insertions(+), 24 deletions(-)

diff --git a/doc/TWEAKS3.md b/doc/TWEAKS3.md
index 086b7c5..7a932ae 100644
--- a/doc/TWEAKS3.md
+++ b/doc/TWEAKS3.md
@@ -22,6 +22,8 @@ Timing instrumentation has been added throughout the codebase to identify bottle
 | `Vision.java` | `periodic()` | >5ms | copyInputs, cameraLoop, consumer, summaryLog |
 | `Intake.java` | `periodic()` | >2ms | update, log |
 | `Feeder.java` | `periodic()` | >2ms | spindexer, kicker, spindexerLog, kickerLog |
+| `Launcher.java` | `periodic()` | >3ms | update, log, aimLog, ballistics |
+| `Launcher.java` | `aim()` | >500μs | v0nom, baseSpeeds, v0replan, setPos, rest |
 
 **Example profiling output:**
 ```
@@ -57,17 +59,29 @@ The codebase already implements background threading for sensor reads:
 - **VisionThread** - Processes camera data asynchronously
 - **CanandgyroThread** - Reads gyro data in background
 
----
+### 3. Deferred Logging in Launcher
+
+The `aim()` method is called frequently and was experiencing significant overhead from `Logger.recordOutput()` calls. The fix is to cache values during `aim()` and log them in `periodic()` instead.
+
+**Implementation:**
+- Added cached fields: `cachedBaseSpeeds`, `cachedFlywheelVelocityTooLow`, `cachedActualD`, `cachedActualV`
+- `hasCachedAimData` flag indicates when cached data is available
+- `aim()` populates the cache instead of calling Logger directly
+- `periodic()` calls `logCachedAimData()` to flush the cache
 
-## Future Work (Phase 2)
+**Why this helps:**
+- `Logger.recordOutput()` can take 11-28ms per call due to serialization overhead
+- Moving logging out of `aim()` keeps the hot path fast
+- Logging happens once per robot loop instead of during time-critical aiming calculations
 
-The following changes from `performance-tweaks-2` still need to be adapted for the current `main` branch:
+### 4. Zero-Velocity Guard
 
-1. **Deferred Logging in Launcher** - Cache values during `aim()`, log them in `periodic()`
-2. **Zero-Velocity Guard** - Prevent `Translation2d.getAngle()` errors when velocity is zero
-3. **Launcher Profiling** - Add timing instrumentation to `periodic()` and `aim()`
+Added protection against `Translation2d.getAngle()` errors when velocity vectors are near zero.
 
-These changes require more careful adaptation due to structural differences in `Launcher.java` between the branches.
+**In `log()` method:**
+- Check if `v_r < 1e-6` before calling `.getAngle()` on velocity vectors
+- Log 0.0 for angles and travel time when velocity is too low
+- Prevents `ArithmeticException` or `NaN` propagation
 
 ---
 
@@ -85,6 +99,7 @@ These changes require more careful adaptation due to structural differences in `
 | `Feeder.java` | Added profiling to `periodic()` |
 | `IntakeRollerIOTalonFX.java` | Changed to non-blocking status checks |
 | `FlywheelIOTalonFX.java` | Changed to non-blocking status checks |
+| `Launcher.java` | Added profiling, deferred logging, zero-velocity guard |
 
 ---
 
diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java
index 58a32d6..2940bf8 100644
--- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java
+++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java
@@ -47,6 +47,13 @@ public class Launcher extends SubsystemBase {
   private Translation3d v0replannedLast = new Translation3d();
   private Rotation2d horizontalAimAngle = Rotation2d.kZero;
 
+  // Cached values for deferred logging (populated in aim(), logged in periodic())
+  private boolean hasCachedAimData = false;
+  private Translation3d cachedBaseSpeeds = new Translation3d();
+  private boolean cachedFlywheelVelocityTooLow = false;
+  private Translation3d cachedActualD = new Translation3d();
+  private Translation3d cachedActualV = new Translation3d();
+
   // Fuel ballistics simulation
   private final ArrayList<BallisticObject> fuelNominal = new ArrayList<>();
   private final ArrayList<BallisticObject> fuelReplanned = new ArrayList<>();
@@ -75,18 +82,29 @@ public Launcher(
 
   @Override
   public void periodic() {
+    long t0 = System.nanoTime();
+
     turretIO.updateInputs(turretInputs);
     flywheelIO.updateInputs(flywheelInputs);
     hoodIO.updateInputs(hoodInputs);
+    long t1 = System.nanoTime();
 
     Logger.processInputs("Turret", turretInputs);
     Logger.processInputs("Flywheel", flywheelInputs);
     Logger.processInputs("Hood", hoodInputs);
+    long t2 = System.nanoTime();
 
     turretDisconnectedAlert.set(!turretInputs.motorControllerConnected);
     flywheelDisconnectedAlert.set(!flywheelInputs.connected);
     hoodDisconnectedAlert.set(!hoodInputs.connected);
 
+    // Log cached aim data (deferred from aim() to avoid Logger overhead in hot path)
+    if (hasCachedAimData) {
+      logCachedAimData();
+      hasCachedAimData = false;
+    }
+    long t3 = System.nanoTime();
+
     // Update and plot ball trajectories
     if (logFuelTrajectories) {
       double dt = Robot.defaultPeriodSecs;
@@ -109,6 +127,24 @@ public void periodic() {
         ballisticLogTimer = 0.0;
       }
     }
+    long t4 = System.nanoTime();
+
+    // Profiling output
+    long totalMs = (t4 - t0) / 1_000_000;
+    if (totalMs > 3) {
+      System.out.println(
+          "[Launcher] update="
+              + (t1 - t0) / 1_000_000
+              + "ms log="
+              + (t2 - t1) / 1_000_000
+              + "ms aimLog="
+              + (t3 - t2) / 1_000_000
+              + "ms ballistics="
+              + (t4 - t3) / 1_000_000
+              + "ms total="
+              + totalMs
+              + "ms");
+    }
   }
 
   public void stop() {
@@ -118,13 +154,16 @@ public void stop() {
   }
 
   public void aim(Translation3d target) {
+    long aimStart = System.nanoTime();
+
     // Get vector from static target to turret
     turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase);
     vectorTurretBaseToTarget = target.minus(turretBasePose.getTranslation());
 
     // Set flywheel speed assuming a motionless robot
-    var v0_nominal = getV0(vectorTurretBaseToTarget, impactAngle, nominalKey);
+    var v0_nominal = getV0Nominal(vectorTurretBaseToTarget, impactAngle, nominalKey);
     flywheelIO.setVelocity(MetersPerSecond.of(ballToFlywheelFactor * v0_nominal.getNorm()));
+    long t1 = System.nanoTime();
 
     // Get translation velocities (m/s) of the turret caused by motion of the chassis
     var robotRelative = chassisSpeedsSupplier.get();
@@ -132,12 +171,14 @@ public void aim(Translation3d target) {
         ChassisSpeeds.fromRobotRelativeSpeeds(
             robotRelative, turretBasePose.toPose2d().getRotation());
     var v_base = getTurretBaseSpeeds(turretBasePose.toPose2d().getRotation(), fieldRelative);
+    long t2 = System.nanoTime();
 
     // Get actual flywheel speed
     double flywheelSpeedMetersPerSec = flywheelInputs.velocityMetersPerSec / ballToFlywheelFactor;
 
     // Replan shot using actual flywheel speed
-    var v0_total = getV0(vectorTurretBaseToTarget, flywheelSpeedMetersPerSec, replannedKey);
+    var v0_total = getV0Replanned(vectorTurretBaseToTarget, flywheelSpeedMetersPerSec, replannedKey);
+    long t3 = System.nanoTime();
 
     // Point turret to align velocity vectors
     var v0_flywheel = v0_total.minus(v_base);
@@ -146,11 +187,12 @@ public void aim(Translation3d target) {
     // Check if v0_flywheel has non-zero horizontal component
     double v0_horizontal = Math.hypot(v0_flywheel.getX(), v0_flywheel.getY());
     if (!Double.isFinite(v0_horizontal) || v0_horizontal < 1e-6) {
-      // Flywheel velocity is too low or target unreachable, stop mechanisms
-      Logger.recordOutput("Launcher/Flywheel velocity too low", true);
+      // Flywheel velocity is too low or target unreachable, cache for deferred logging
+      cachedFlywheelVelocityTooLow = true;
+      hasCachedAimData = true;
       return;
     }
-    Logger.recordOutput("Launcher/Flywheel velocity too low", false);
+    cachedFlywheelVelocityTooLow = false;
 
     horizontalAimAngle = new Rotation2d(v0_flywheel.getX(), v0_flywheel.getY());
     turretIO.setPosition(
@@ -158,6 +200,7 @@ public void aim(Translation3d target) {
         RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond).unaryMinus().times(2.0));
     Rotation2d hoodSetpoint = new Rotation2d(v0_horizontal, v0_flywheel.getZ());
     hoodIO.setPosition(hoodSetpoint.minus(ballToHoodOffset), RadiansPerSecond.of(0));
+    long t4 = System.nanoTime();
 
     // Get actual hood & turret position
     Rotation2d hoodPosition = hoodInputs.position.plus(ballToHoodOffset);
@@ -175,7 +218,12 @@ public void aim(Translation3d target) {
     double vy = hoodCos * turretSin * flywheelSpeedMetersPerSec + v_base.getY();
     double vz = hoodSin * flywheelSpeedMetersPerSec;
     Translation3d v0_actual = new Translation3d(vx, vy, vz);
-    log(vectorTurretBaseToTarget, v0_actual, actualKey);
+
+    // Cache values for deferred logging (will be logged in periodic())
+    cachedActualD = vectorTurretBaseToTarget;
+    cachedActualV = v0_actual;
+    hasCachedAimData = true;
+    long t5 = System.nanoTime();
 
     // Spawn simulated fuel
     fuelSpawnTimer += Robot.defaultPeriodSecs;
@@ -189,6 +237,25 @@ public void aim(Translation3d target) {
       fuelActual.add(
           new BallisticObject(turretBasePose.getTranslation(), v0_actual, target.getMeasureZ()));
     }
+
+    // Profiling output for aim()
+    long totalUs = (t5 - aimStart) / 1_000;
+    if (totalUs > 500) {
+      System.out.println(
+          "[Launcher.aim] v0nom="
+              + (t1 - aimStart) / 1_000
+              + "us baseSpeeds="
+              + (t2 - t1) / 1_000
+              + "us v0replan="
+              + (t3 - t2) / 1_000
+              + "us setPos="
+              + (t4 - t3) / 1_000
+              + "us rest="
+              + (t5 - t4) / 1_000
+              + "us total="
+              + totalUs
+              + "us");
+    }
   }
 
   @AutoLogOutput(key = "Launcher/TurretPose")
@@ -227,12 +294,13 @@ private Translation3d getTurretBaseSpeeds(Rotation2d rotation, ChassisSpeeds cha
 
     var baseSpeeds = new Translation3d(baseVx, baseVy, 0);
 
-    Logger.recordOutput("Launcher/BaseSpeeds", baseSpeeds);
+    // Cache for deferred logging (will be logged in periodic())
+    cachedBaseSpeeds = baseSpeeds;
 
     return baseSpeeds;
   }
 
-  private Translation3d getV0(Translation3d d, Rotation2d impactAngle, String key) {
+  private Translation3d getV0Nominal(Translation3d d, Rotation2d impactAngle, String key) {
     double dr = Math.hypot(d.getX(), d.getY());
     if (dr < 1e-6) {
       Logger.recordOutput("Launcher/" + key + "/Reachable", false);
@@ -260,7 +328,7 @@ private Translation3d getV0(Translation3d d, Rotation2d impactAngle, String key)
     return v0nominalLast;
   }
 
-  private Translation3d getV0(Translation3d d, double v_flywheel, String key) {
+  private Translation3d getV0Replanned(Translation3d d, double v_flywheel, String key) {
     // Geometry
     double dr = Math.hypot(d.getX(), d.getY());
     if (dr < 1e-6) {
@@ -316,21 +384,39 @@ private Translation3d getV0(Translation3d d, double v_flywheel, String key) {
     return v0replannedLast;
   }
 
+  /** Logs cached aim data that was deferred from aim() to avoid Logger overhead in hot path. */
+  private void logCachedAimData() {
+    Logger.recordOutput("Launcher/BaseSpeeds", cachedBaseSpeeds);
+    Logger.recordOutput("Launcher/Flywheel velocity too low", cachedFlywheelVelocityTooLow);
+
+    if (!cachedFlywheelVelocityTooLow) {
+      log(cachedActualD, cachedActualV, actualKey);
+    }
+  }
+
   private void log(Translation3d d, Translation3d v, String key) {
     Logger.recordOutput("Launcher/" + key + "/InitialVelocities", v);
     Logger.recordOutput("Launcher/" + key + "/InitialSpeedMetersPerSecond", v.getNorm());
 
     var v_r = v.toTranslation2d().getNorm();
     var v_z = v.getZ();
-    Logger.recordOutput(
-        "Launcher/" + key + "/VerticalLaunchAngleDegrees",
-        new Translation2d(v_r, v_z).getAngle().getDegrees());
-    Logger.recordOutput(
-        "Launcher/" + key + "/HorizontalLaunchAngleDegrees",
-        v.toTranslation2d().getAngle().getDegrees());
 
-    double dr = Math.hypot(d.getX(), d.getY());
-    Logger.recordOutput("Launcher/" + key + "/TravelTime", dr / v_r);
+    // Zero-velocity guard: avoid getAngle() on zero-length vectors
+    if (v_r < 1e-6) {
+      Logger.recordOutput("Launcher/" + key + "/VerticalLaunchAngleDegrees", 0.0);
+      Logger.recordOutput("Launcher/" + key + "/HorizontalLaunchAngleDegrees", 0.0);
+      Logger.recordOutput("Launcher/" + key + "/TravelTime", 0.0);
+    } else {
+      Logger.recordOutput(
+          "Launcher/" + key + "/VerticalLaunchAngleDegrees",
+          new Translation2d(v_r, v_z).getAngle().getDegrees());
+      Logger.recordOutput(
+          "Launcher/" + key + "/HorizontalLaunchAngleDegrees",
+          v.toTranslation2d().getAngle().getDegrees());
+
+      double dr = Math.hypot(d.getX(), d.getY());
+      Logger.recordOutput("Launcher/" + key + "/TravelTime", dr / v_r);
+    }
 
     var max_height = turretBasePose.getZ() + v_z * v_z / (2 * g);
     Logger.recordOutput("Launcher/" + key + "/MaxHeight", max_height);

From bd4cbb6a122f700593126d8e1c5f9212b80525e4 Mon Sep 17 00:00:00 2001
From: Christopher Larrieu <christopher@larrieu.org>
Date: Thu, 26 Feb 2026 19:10:09 -0500
Subject: [PATCH 3/4] Add profiling to Hopper, document Phase 3 analysis

Phase 3: Analyzed subsystems added since performance-tweaks-2 was
branched for blocking CAN calls and added profiling where needed.

Changes:
- Add profiling to Hopper.java periodic() with 2ms threshold
- Document Phase 3 analysis in TWEAKS3.md

Analysis findings:
- All Spark-based IOs (Spindexer, Kicker, Hood, Turret) already use
  SparkOdometryThread for non-blocking CAN reads
- GyroIOBoron uses CanandgyroThread for non-blocking reads
- Pneumatic IOs (Hopper, IntakeArm) use DoubleSolenoid which has no
  blocking CAN concerns

The codebase is well-structured with consistent use of background
threading for motor controller IO operations.
---
 doc/TWEAKS3.md                                | 20 +++++++++++++++++++
 .../frc/robot/subsystems/hopper/Hopper.java   | 16 +++++++++++++++
 2 files changed, 36 insertions(+)

diff --git a/doc/TWEAKS3.md b/doc/TWEAKS3.md
index 7a932ae..96d423a 100644
--- a/doc/TWEAKS3.md
+++ b/doc/TWEAKS3.md
@@ -24,6 +24,7 @@ Timing instrumentation has been added throughout the codebase to identify bottle
 | `Feeder.java` | `periodic()` | >2ms | spindexer, kicker, spindexerLog, kickerLog |
 | `Launcher.java` | `periodic()` | >3ms | update, log, aimLog, ballistics |
 | `Launcher.java` | `aim()` | >500μs | v0nom, baseSpeeds, v0replan, setPos, rest |
+| `Hopper.java` | `periodic()` | >2ms | update, log |
 
 **Example profiling output:**
 ```
@@ -100,6 +101,25 @@ Added protection against `Translation2d.getAngle()` errors when velocity vectors
 | `IntakeRollerIOTalonFX.java` | Changed to non-blocking status checks |
 | `FlywheelIOTalonFX.java` | Changed to non-blocking status checks |
 | `Launcher.java` | Added profiling, deferred logging, zero-velocity guard |
+| `Hopper.java` | Added profiling to `periodic()` |
+
+---
+
+## Phase 3: Analysis of New Subsystems
+
+Subsystems added since `performance-tweaks-2` was branched were analyzed for blocking calls:
+
+| Subsystem | IO Implementation | Status |
+|-----------|-------------------|--------|
+| `Hopper` | `HopperIOReal` | Uses DoubleSolenoid (pneumatics, not CAN) - no blocking issues. Added profiling. |
+| `Feeder/Spindexer` | `SpindexerIOSpark` | Uses `SparkOdometryThread` for non-blocking CAN reads. |
+| `Feeder/Kicker` | `KickerIOSpark` | Uses `SparkOdometryThread` for non-blocking CAN reads. |
+| `Launcher/Hood` | `HoodIOSpark` | Uses `SparkOdometryThread` for non-blocking CAN reads. |
+| `Launcher/Turret` | `TurretIOSpark` | Uses `SparkOdometryThread` for non-blocking CAN reads. |
+| `Intake/Arm` | `IntakeArmIOReal` | Uses DoubleSolenoid (pneumatics, not CAN) - no blocking issues. |
+| `Drive/Gyro` | `GyroIOBoron` | Uses `CanandgyroThread` for non-blocking CAN reads. |
+
+**Conclusion:** All motor controller IO implementations properly use background threading (`SparkOdometryThread` or `CanandgyroThread`) for CAN reads. Pneumatic subsystems use WPILib's DoubleSolenoid which doesn't have blocking CAN concerns.
 
 ---
 
diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java
index 7a26884..c34497e 100644
--- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java
+++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java
@@ -16,8 +16,24 @@ public Hopper(HopperIO hopperIO) {
 
   @Override
   public void periodic() {
+    long t0 = System.nanoTime();
     hopperIO.updateInputs(hopperInputs);
+    long t1 = System.nanoTime();
     Logger.processInputs("Hopper", hopperInputs);
+    long t2 = System.nanoTime();
+
+    // Profiling output
+    long totalMs = (t2 - t0) / 1_000_000;
+    if (totalMs > 2) {
+      System.out.println(
+          "[Hopper] update="
+              + (t1 - t0) / 1_000_000
+              + "ms log="
+              + (t2 - t1) / 1_000_000
+              + "ms total="
+              + totalMs
+              + "ms");
+    }
   }
 
   public void retract() {

From e665762e194dc2adb18455188e796731c1b56a4b Mon Sep 17 00:00:00 2001
From: Christopher Larrieu <christopher@larrieu.org>
Date: Thu, 26 Feb 2026 20:45:04 -0500
Subject: [PATCH 4/4] fix(intake): move Logger calls out of updateInputs to fix
 23ms spikes

Profiling showed [Intake] update=23ms spikes caused by Logger.recordOutput()
calls inside IntakeRollerIOTalonFX.updateInputs(). Each Logger call can
take 10-30ms due to serialization overhead.

Fix: Move follower telemetry into the IntakeRollerIOInputs struct so it
gets logged via Logger.processInputs() in the subsystem's periodic()
instead of blocking inside updateInputs().

Changes:
- IntakeRollerIO: Add followerAppliedVolts, followerCurrentAmps fields
- IntakeRollerIOTalonFX: Populate input fields instead of Logger calls
- TWEAKS3.md: Document Phase 5 fix
---
 doc/TWEAKS3.md                                | 66 ++++++++++++++++++-
 .../subsystems/intake/IntakeRollerIO.java     |  2 +
 .../intake/IntakeRollerIOTalonFX.java         |  7 +-
 .../robot/subsystems/launcher/Launcher.java   |  3 +-
 .../frc/robot/subsystems/vision/Vision.java   | 60 ++++++++++-------
 .../subsystems/vision/VisionConstants.java    |  4 ++
 6 files changed, 113 insertions(+), 29 deletions(-)

diff --git a/doc/TWEAKS3.md b/doc/TWEAKS3.md
index 96d423a..abbbfd4 100644
--- a/doc/TWEAKS3.md
+++ b/doc/TWEAKS3.md
@@ -19,7 +19,7 @@ Timing instrumentation has been added throughout the codebase to identify bottle
 | `Robot.java` | `robotPeriodic()` | >20ms | scheduler, gameState |
 | `Drive.java` | `periodic()` | >5ms | lock, gyroUpdate, gyroLog, modules, disabled, odometry |
 | `Module.java` | `periodic()` | >2ms | updateInputs, log, rest |
-| `Vision.java` | `periodic()` | >5ms | copyInputs, cameraLoop, consumer, summaryLog |
+| `Vision.java` | `periodic()` | >5ms | snapshot, processInputs, cameraLoop, consumer, summaryLog |
 | `Intake.java` | `periodic()` | >2ms | update, log |
 | `Feeder.java` | `periodic()` | >2ms | spindexer, kicker, spindexerLog, kickerLog |
 | `Launcher.java` | `periodic()` | >3ms | update, log, aimLog, ballistics |
@@ -95,10 +95,12 @@ Added protection against `Translation2d.getAngle()` errors when velocity vectors
 | `Module.java` | Added profiling, removed signal methods |
 | `ModuleIO.java` | Removed `refreshSignals()` and `getStatusSignals()` interface methods |
 | `ModuleIOTalonFX.java` | Removed signal caching and refresh code |
-| `Vision.java` | Added profiling to `periodic()` |
+| `Vision.java` | Added profiling, throttled logging |
+| `VisionConstants.java` | Added `kLoggingDivisor` constant |
 | `Intake.java` | Added profiling to `periodic()` |
 | `Feeder.java` | Added profiling to `periodic()` |
-| `IntakeRollerIOTalonFX.java` | Changed to non-blocking status checks |
+| `IntakeRollerIOTalonFX.java` | Changed to non-blocking status checks, moved Logger calls to inputs struct |
+| `IntakeRollerIO.java` | Added follower telemetry fields to inputs |
 | `FlywheelIOTalonFX.java` | Changed to non-blocking status checks |
 | `Launcher.java` | Added profiling, deferred logging, zero-velocity guard |
 | `Hopper.java` | Added profiling to `periodic()` |
@@ -123,6 +125,64 @@ Subsystems added since `performance-tweaks-2` was branched were analyzed for blo
 
 ---
 
+## Phase 4: Throttled Vision Logging
+
+Profiling revealed that `Logger.processInputs()` serialization was a major source of loop overruns, not CAN I/O. Vision logging was particularly expensive due to serializing `PoseObservation` arrays for multiple cameras.
+
+### Changes
+
+**VisionConstants.java:**
+- Added `kLoggingDivisor` constant (default: 1)
+- Set to 2+ to log every Nth cycle, reducing CPU load
+
+**Vision.java:**
+- Added `loopCounter` to track cycles
+- Throttled `Logger.processInputs()` calls based on `kLoggingDivisor`
+- Throttled summary pose logging similarly
+- Split profiling to separate `snapshot` (volatile read) from `processInputs` (serialization)
+
+**Trade-offs:**
+- `kLoggingDivisor = 1`: Full data for replay, higher CPU load
+- `kLoggingDivisor = 2`: Half the vision log data, ~50% reduction in vision logging overhead
+- Higher values lose more replay granularity but further reduce load
+
+**Profiling output now shows:**
+```
+[Vision] snapshot=0ms processInputs=12ms cameraLoop=3ms consumer=0ms summaryLog=2ms total=17ms
+```
+
+---
+
+## Phase 5: Intake Logger Fix
+
+Profiling revealed `[Intake] update=23ms` spikes. Investigation found `Logger.recordOutput()` calls inside `IntakeRollerIOTalonFX.updateInputs()`, which was measured as "update" time rather than "log" time.
+
+### Root Cause
+
+```java
+// In IntakeRollerIOTalonFX.updateInputs() - BEFORE
+Logger.recordOutput("Intake/Follower/Current", followerCurrent.getValue());
+Logger.recordOutput("Intake/Follower/Volts", followerAppliedVolts.getValue());
+```
+
+Each `Logger.recordOutput()` call can take 10-30ms due to serialization overhead, causing the 23ms "update" time.
+
+### Fix
+
+Moved follower data into the `IntakeRollerIOInputs` struct so it gets logged via `Logger.processInputs()` instead:
+
+**IntakeRollerIO.java:**
+- Added `followerAppliedVolts` and `followerCurrentAmps` fields to inputs struct
+
+**IntakeRollerIOTalonFX.java:**
+- Removed `Logger.recordOutput()` calls from `updateInputs()`
+- Populate `inputs.followerAppliedVolts` and `inputs.followerCurrentAmps` instead
+- Removed unused Logger import
+
+This follows the same pattern as the Launcher deferred logging fix - keep IO methods fast by avoiding Logger calls, and let the subsystem's `periodic()` handle logging via `Logger.processInputs()`.
+
+---
+
 ## References
 
 - [Chief Delphi: Let's talk about CAN delays](https://www.chiefdelphi.com/t/lets-talk-about-can-delays-and-how-to-address-them/396566)
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIO.java
index 87f9f25..cb052c2 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIO.java
@@ -11,6 +11,8 @@ public static class IntakeRollerIOInputs {
     public double velocityMetersPerSec = 0.0;
     public double appliedVolts = 0.0;
     public double currentAmps = 0.0;
+    public double followerAppliedVolts = 0.0;
+    public double followerCurrentAmps = 0.0;
   }
 
   public default void updateInputs(IntakeRollerIOInputs inputs) {}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java
index 39eaed4..19e2d4e 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java
@@ -22,7 +22,6 @@
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.Voltage;
 import frc.robot.Constants.CANBusPorts.CAN2;
-import org.littletonrobotics.junction.Logger;
 
 public class IntakeRollerIOTalonFX implements IntakeRollerIO {
   private final TalonFX intakeMotorLeader;
@@ -92,8 +91,10 @@ public void updateInputs(IntakeRollerIOInputs inputs) {
     inputs.velocityMetersPerSec =
         intakeVelocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction;
 
-    Logger.recordOutput("Intake/Follower/Current", followerCurrent.getValue());
-    Logger.recordOutput("Intake/Follower/Volts", followerAppliedVolts.getValue());
+    // Follower data goes into inputs struct to be logged via processInputs()
+    // instead of recordOutput() which can block for 10-30ms
+    inputs.followerAppliedVolts = followerAppliedVolts.getValueAsDouble();
+    inputs.followerCurrentAmps = followerCurrent.getValueAsDouble();
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java
index 2940bf8..4a4f1df 100644
--- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java
+++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java
@@ -177,7 +177,8 @@ public void aim(Translation3d target) {
     double flywheelSpeedMetersPerSec = flywheelInputs.velocityMetersPerSec / ballToFlywheelFactor;
 
     // Replan shot using actual flywheel speed
-    var v0_total = getV0Replanned(vectorTurretBaseToTarget, flywheelSpeedMetersPerSec, replannedKey);
+    var v0_total =
+        getV0Replanned(vectorTurretBaseToTarget, flywheelSpeedMetersPerSec, replannedKey);
     long t3 = System.nanoTime();
 
     // Point turret to align velocity vectors
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
index 7634a96..88090ca 100644
--- a/src/main/java/frc/robot/subsystems/vision/Vision.java
+++ b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -77,6 +77,9 @@ public class Vision extends SubsystemBase {
     LinearFilter.movingAverage(20)
   };
 
+  // Cycle counter for throttled logging
+  private int loopCounter = 0;
+
   public Vision(VisionConsumer consumer, Supplier<Pose2d> chassisPoseSupplier, VisionIO... io) {
     this.consumer = consumer;
     this.chassisPoseSupplier = chassisPoseSupplier;
@@ -115,14 +118,23 @@ public Rotation2d getTargetX(int cameraIndex) {
   @Override
   public void periodic() {
     long visionStart = System.nanoTime();
+    loopCounter++;
 
+    // Copy cached inputs from background thread (should be fast - volatile reads)
     for (int i = 0; i < io.length; i++) {
-      // Copy cached inputs from background thread to inputs for AdvantageKit logging
       visionInputs[i].getSnapshot().copyTo(inputs[i]);
-      Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]);
     }
     long t1 = System.nanoTime();
 
+    // Log inputs via AdvantageKit (throttled - serialization is expensive)
+    // Note: Throttling reduces CPU load but loses data granularity for replay
+    if (loopCounter % kLoggingDivisor == 0) {
+      for (int i = 0; i < io.length; i++) {
+        Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]);
+      }
+    }
+    long t2 = System.nanoTime();
+
     // Initialize logging values
     allTagPoses.clear();
     allRobotPoses.clear();
@@ -203,7 +215,7 @@ public void periodic() {
       allRobotPosesRejected.addAll(robotPosesRejected);
     }
 
-    long t2 = System.nanoTime();
+    long t3 = System.nanoTime();
 
     // Remove unacceptable observations
     observations.removeIf(o -> o.score < minScore);
@@ -225,35 +237,39 @@ public void periodic() {
 
       Logger.recordOutput("Vision/Summary/ObservationScore", o.score);
     }
-    long t3 = System.nanoTime();
+    long t4 = System.nanoTime();
 
-    // Log summary data
-    if (kLogSummaryPoses) {
-      Logger.recordOutput("Vision/Summary/TagPoses", allTagPoses.toArray(Pose3d[]::new));
-      Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(Pose3d[]::new));
-    }
-    if (kLogAcceptedPoses) {
-      Logger.recordOutput(
-          "Vision/Summary/RobotPosesAccepted", allRobotPosesAccepted.toArray(Pose3d[]::new));
-    }
-    if (kLogRejectedPoses) {
-      Logger.recordOutput(
-          "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(Pose3d[]::new));
+    // Log summary data (throttled along with processInputs)
+    if (loopCounter % kLoggingDivisor == 0) {
+      if (kLogSummaryPoses) {
+        Logger.recordOutput("Vision/Summary/TagPoses", allTagPoses.toArray(Pose3d[]::new));
+        Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(Pose3d[]::new));
+      }
+      if (kLogAcceptedPoses) {
+        Logger.recordOutput(
+            "Vision/Summary/RobotPosesAccepted", allRobotPosesAccepted.toArray(Pose3d[]::new));
+      }
+      if (kLogRejectedPoses) {
+        Logger.recordOutput(
+            "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(Pose3d[]::new));
+      }
     }
-    long t4 = System.nanoTime();
+    long t5 = System.nanoTime();
 
     // Profiling output
-    long totalMs = (t4 - visionStart) / 1_000_000;
+    long totalMs = (t5 - visionStart) / 1_000_000;
     if (totalMs > 5) {
       System.out.println(
-          "[Vision] copyInputs="
+          "[Vision] snapshot="
               + (t1 - visionStart) / 1_000_000
-              + "ms cameraLoop="
+              + "ms processInputs="
               + (t2 - t1) / 1_000_000
-              + "ms consumer="
+              + "ms cameraLoop="
               + (t3 - t2) / 1_000_000
-              + "ms summaryLog="
+              + "ms consumer="
               + (t4 - t3) / 1_000_000
+              + "ms summaryLog="
+              + (t5 - t4) / 1_000_000
               + "ms total="
               + totalMs
               + "ms");
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
index ee07af9..26c2b7b 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -95,4 +95,8 @@ public class VisionConstants {
   public static boolean kLogSummaryPoses = false;
   public static boolean kLogAcceptedPoses = true;
   public static boolean kLogRejectedPoses = false;
+
+  // Logging frequency (1 = every cycle, 2 = every other cycle, etc.)
+  // Higher values reduce CPU load but lose data granularity for replay
+  public static int kLoggingDivisor = 2;
 }