diff --git a/doc/TWEAKS3.md b/doc/TWEAKS3.md new file mode 100644 index 0000000..abbbfd4 --- /dev/null +++ b/doc/TWEAKS3.md @@ -0,0 +1,190 @@ +# 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 | 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 | +| `Launcher.java` | `aim()` | >500μs | v0nom, baseSpeeds, v0replan, setPos, rest | +| `Hopper.java` | `periodic()` | >2ms | update, log | + +**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 + +### 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 + +**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 + +### 4. Zero-Velocity Guard + +Added protection against `Translation2d.getAngle()` errors when velocity vectors are near zero. + +**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 + +--- + +## 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, 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, 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()` | + +--- + +## 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. + +--- + +## 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) +- [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/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() { 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/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 d812279..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; @@ -78,20 +77,24 @@ 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()); + // 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/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/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 58a32d6..4a4f1df 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 fuelNominal = new ArrayList<>(); private final ArrayList 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,15 @@ 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 +188,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 +201,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 +219,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 +238,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 +295,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 +329,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 +385,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); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 09c073f..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 chassisPoseSupplier, VisionIO... io) { this.consumer = consumer; this.chassisPoseSupplier = chassisPoseSupplier; @@ -114,11 +117,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(); @@ -200,6 +215,8 @@ public void periodic() { allRobotPosesRejected.addAll(robotPosesRejected); } + long t3 = System.nanoTime(); + // Remove unacceptable observations observations.removeIf(o -> o.score < minScore); @@ -220,19 +237,42 @@ public void periodic() { Logger.recordOutput("Vision/Summary/ObservationScore", o.score); } + 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)); + // 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)); + } } - if (kLogRejectedPoses) { - Logger.recordOutput( - "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(Pose3d[]::new)); + long t5 = System.nanoTime(); + + // Profiling output + long totalMs = (t5 - visionStart) / 1_000_000; + if (totalMs > 5) { + System.out.println( + "[Vision] snapshot=" + + (t1 - visionStart) / 1_000_000 + + "ms processInputs=" + + (t2 - t1) / 1_000_000 + + "ms cameraLoop=" + + (t3 - t2) / 1_000_000 + + "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; }