Performance tweaks

src/main/java/frc/robot/Robot.java

@@ -55,6 +55,9 @@
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOPhotonVision;
 import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
+import frc.robot.util.CanandgyroThread;
+import frc.robot.util.SparkOdometryThread;
+import frc.robot.util.VisionThread;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -206,6 +209,11 @@ public Robot() {
         break;
     }
 
+    // Start background threads (for non-blocking CAN/network reads)
+    SparkOdometryThread.getInstance().start();
+    VisionThread.getInstance().start();
+    CanandgyroThread.getInstance().start();
+
     // Start AdvantageKit logger
     Logger.start();
 
@@ -226,6 +234,12 @@ public Robot() {
     feeder.setDefaultCommand(Commands.run(feeder::spinForward, feeder).withName("Spin forward"));
 
     intake.setDefaultCommand(Commands.run(intake::intake, intake).withName("Intake Command"));
+    SmartDashboard.putData(CommandScheduler.getInstance());
+    SmartDashboard.putData(drive);
+    SmartDashboard.putData(vision);
+    SmartDashboard.putData(launcher);
+    SmartDashboard.putData(feeder);
+    SmartDashboard.putData(intake);
   }
 
   /** This function is called periodically during all modes. */
@@ -241,12 +255,6 @@ 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();
-    SmartDashboard.putData(CommandScheduler.getInstance());
-    SmartDashboard.putData(drive);
-    SmartDashboard.putData(vision);
-    SmartDashboard.putData(launcher);
-    SmartDashboard.putData(feeder);
-    SmartDashboard.putData(intake);
 
     GameState.logValues();
 

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -17,6 +17,7 @@
 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;
@@ -85,6 +86,9 @@ 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);
@@ -142,14 +146,28 @@ 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();
 
-    for (var module : modules) {
-      module.refreshSignals();
+    // Refresh all module signals in a single batched CAN call
+    if (allModuleSignals.length > 0) {
+      BaseStatusSignal.refreshAll(allModuleSignals);
     }
 
     odometryLock.lock(); // Prevents odometry updates while reading data

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -85,6 +85,7 @@ public class DriveConstants {
 
   // Drive motor configuration
   public static final Distance wheelRadius = Inches.of(2);
+  public static final double wheelRadiusMeters = wheelRadius.in(Meters);
   public static final double driveMotorReduction =
       (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); // SDS MK4 L2
   public static final DCMotor driveGearbox = DCMotor.getKrakenX60(1);

src/main/java/frc/robot/subsystems/drive/GyroIOBoron.java

@@ -17,27 +17,31 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants.CANBusPorts.CAN2;
+import frc.robot.util.CanandgyroThread;
+import frc.robot.util.CanandgyroThread.GyroInputs;
 import java.util.Queue;
 
-/** IO implementation for NavX. */
+/** IO implementation for Redux Canandgyro. */
 public class GyroIOBoron implements GyroIO {
-  private Canandgyro canandgyro;
+  private final Canandgyro canandgyro;
+  private final GyroInputs gyroInputs;
   private final Queue<Double> yawTimestampQueue;
   private final Queue<Double> yawPositionQueue;
 
   public GyroIOBoron() {
     canandgyro = new Canandgyro(CAN2.gyro);
+    gyroInputs = CanandgyroThread.getInstance().registerCanandgyro(canandgyro);
     yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
     yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(canandgyro::getYaw);
   }
 
-  // Check if gyro is calibrated
   @Override
   public void updateInputs(GyroIOInputs inputs) {
-    inputs.connected = canandgyro.isConnected();
-    inputs.calibrated = !canandgyro.isCalibrating();
-    inputs.yawPosition = Rotation2d.fromRotations(canandgyro.getYaw());
-    inputs.yawVelocityRadPerSec = Units.rotationsToRadians(canandgyro.getAngularVelocityYaw());
+    // Read from cached values (non-blocking) - updated by CanandgyroThread
+    inputs.connected = gyroInputs.isConnected();
+    inputs.calibrated = !gyroInputs.isCalibrating();
+    inputs.yawPosition = Rotation2d.fromRotations(gyroInputs.getYaw());
+    inputs.yawVelocityRadPerSec = Units.rotationsToRadians(gyroInputs.getAngularVelocityYaw());
 
     inputs.odometryYawTimestamps = new double[yawTimestampQueue.size()];
     for (int i = 0; i < inputs.odometryYawTimestamps.length; i++) {

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -13,9 +13,9 @@
 
 package frc.robot.subsystems.drive;
 
-import static edu.wpi.first.units.Units.Meters;
 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;
@@ -61,7 +61,7 @@ public void periodic() {
     int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
     odometryPositions = new SwerveModulePosition[sampleCount];
     for (int i = 0; i < sampleCount; i++) {
-      double positionMeters = inputs.odometryDrivePositionsRad[i] * wheelRadius.in(Meters);
+      double positionMeters = inputs.odometryDrivePositionsRad[i] * wheelRadiusMeters;
       Rotation2d angle = inputs.odometryTurnPositions[i];
       odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
     }
@@ -78,7 +78,7 @@ public void runSetpoint(SwerveModuleState state) {
     state.cosineScale(inputs.turnPosition);
 
     // Apply setpoints
-    io.setDriveVelocity(state.speedMetersPerSecond / wheelRadius.in(Meters));
+    io.setDriveVelocity(state.speedMetersPerSecond / wheelRadiusMeters);
     io.setTurnPosition(state.angle);
   }
 
@@ -101,12 +101,12 @@ public Rotation2d getAngle() {
 
   /** Returns the current drive position of the module in meters. */
   public double getPositionMeters() {
-    return inputs.drivePositionRad * wheelRadius.in(Meters);
+    return inputs.drivePositionRad * wheelRadiusMeters;
   }
 
   /** Returns the current drive velocity of the module in meters per second. */
   public double getVelocityMetersPerSec() {
-    return inputs.driveVelocityRadPerSec * wheelRadius.in(Meters);
+    return inputs.driveVelocityRadPerSec * wheelRadiusMeters;
   }
 
   /** Returns the module position (turn angle and drive position). */
@@ -150,4 +150,9 @@ public void setTurnZero() {
   public void refreshSignals() {
     io.refreshSignals();
   }
+
+  /** Returns the status signals for batched refresh. */
+  public BaseStatusSignal[] getStatusSignals() {
+    return io.getStatusSignals();
+  }
 }

src/main/java/frc/robot/subsystems/drive/ModuleIO.java

@@ -13,6 +13,7 @@
 
 package frc.robot.subsystems.drive;
 
+import com.ctre.phoenix6.BaseStatusSignal;
 import edu.wpi.first.math.geometry.Rotation2d;
 import org.littletonrobotics.junction.AutoLog;
 
@@ -59,4 +60,11 @@ 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];
+  }
 }

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -183,20 +183,33 @@ 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(
-        drivePosition,
-        driveVelocity,
-        driveAppliedVolts,
-        driveCurrent,
-        turnPosition,
-        turnVelocity,
-        turnAppliedVolts,
-        turnCurrent,
-        turnAbsolutePosition);
+    BaseStatusSignal.refreshAll(allSignals);
+  }
+
+  @Override
+  public BaseStatusSignal[] getStatusSignals() {
+    return allSignals;
   }
 
   @Override

src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java

@@ -51,10 +51,10 @@ public IntakeRollerIOTalonFX() {
 
   @Override
   public void updateInputs(IntakeRollerIOInputs inputs) {
-    BaseStatusSignal.setUpdateFrequencyForAll(
-            50.0, intakeVelocity, intakeAppliedVolts, intakeCurrent)
-        .isOK();
-
+    var status = BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent);
+    if (!status.isOK()) {
+      return;
+    }
     inputs.appliedVolts = intakeAppliedVolts.getValueAsDouble();
     inputs.connected = intakeMotor.isConnected();
     inputs.currentAmps = intakeCurrent.getValueAsDouble();

src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java

@@ -23,15 +23,17 @@
 import frc.robot.Constants.CANBusPorts.CAN2;
 import frc.robot.Constants.MotorConstants.NEO550Constants;
 import frc.robot.Constants.RobotConstants;
-import java.util.function.DoubleSupplier;
+import frc.robot.util.SparkOdometryThread;
+import frc.robot.util.SparkOdometryThread.SparkInputs;
 
 public class HoodIOSpark implements HoodIO {
 
   private final SparkBase hoodSpark;
   private final RelativeEncoder encoderSpark;
   private final SparkClosedLoopController hoodController;
-  private final Debouncer turnConnectedDebounce =
-      new Debouncer(0.5, Debouncer.DebounceType.kFalling);
+  private final SparkInputs sparkInputs;
+
+  private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling);
   private boolean relatvieEncoderSeeded = false;
 
   public HoodIOSpark() {
@@ -77,6 +79,9 @@ public HoodIOSpark() {
         () ->
             hoodSpark.configure(
                 hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
+
+    // Register with background thread for non-blocking CAN reads
+    sparkInputs = SparkOdometryThread.getInstance().registerSpark(hoodSpark, encoderSpark);
   }
 
   @Override
@@ -86,15 +91,12 @@ public void updateInputs(HoodIOInputs inputs) {
       relatvieEncoderSeeded = true;
     }
 
-    sparkStickyFault = false;
-    ifOk(hoodSpark, encoderSpark::getPosition, (value) -> inputs.position = new Rotation2d(value));
-    ifOk(hoodSpark, encoderSpark::getVelocity, (value) -> inputs.velocityRadPerSec = value);
-    ifOk(
-        hoodSpark,
-        new DoubleSupplier[] {hoodSpark::getAppliedOutput, hoodSpark::getBusVoltage},
-        (values) -> inputs.appliedVolts = values[0] * values[1]);
-    ifOk(hoodSpark, hoodSpark::getOutputCurrent, (value) -> inputs.currentAmps = value);
-    inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault);
+    // Read from cached values (non-blocking) - updated by SparkOdometryThread
+    inputs.position = new Rotation2d(sparkInputs.getPosition());
+    inputs.velocityRadPerSec = sparkInputs.getVelocity();
+    inputs.appliedVolts = sparkInputs.getAppliedVolts();
+    inputs.currentAmps = sparkInputs.getOutputCurrent();
+    inputs.connected = connectedDebounce.calculate(sparkInputs.isConnected());
   }
 
   @Override

src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java

@@ -27,14 +27,16 @@
 import frc.robot.Constants.DIOPorts;
 import frc.robot.Constants.MotorConstants.NEO550Constants;
 import frc.robot.Constants.RobotConstants;
-import java.util.function.DoubleSupplier;
+import frc.robot.util.SparkOdometryThread;
+import frc.robot.util.SparkOdometryThread.SparkInputs;
 
 public class TurretIOSpark implements TurretIO {
 
   private final SparkBase turnSpark;
   private final RelativeEncoder turnSparkEncoder;
   private final SparkClosedLoopController controller;
   private final DutyCycleEncoder absoluteEncoder;
+  private final SparkInputs sparkInputs;
 
   private final Debouncer motorControllerConnectedDebounce =
       new Debouncer(0.5, Debouncer.DebounceType.kFalling);
@@ -82,6 +84,9 @@ public TurretIOSpark() {
         () ->
             turnSpark.configure(
                 turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
+
+    // Register with background thread for non-blocking CAN reads
+    sparkInputs = SparkOdometryThread.getInstance().registerSpark(turnSpark, turnSparkEncoder);
   }
 
   @Override
@@ -91,19 +96,15 @@ public void updateInputs(TurretIOInputs inputs) {
       relativeEncoderSeeded = true;
     }
 
-    sparkStickyFault = false;
-    ifOk(
-        turnSpark,
-        turnSparkEncoder::getPosition,
-        (value) -> inputs.relativePosition = new Rotation2d(value).plus(mechanismOffset));
-    ifOk(turnSpark, turnSparkEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value);
-    ifOk(
-        turnSpark,
-        new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage},
-        (values) -> inputs.appliedVolts = values[0] * values[1]);
-    ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.currentAmps = value);
-    inputs.motorControllerConnected = motorControllerConnectedDebounce.calculate(!sparkStickyFault);
+    // Read from cached values (non-blocking) - updated by SparkOdometryThread
+    inputs.relativePosition = new Rotation2d(sparkInputs.getPosition()).plus(mechanismOffset);
+    inputs.velocityRadPerSec = sparkInputs.getVelocity();
+    inputs.appliedVolts = sparkInputs.getAppliedVolts();
+    inputs.currentAmps = sparkInputs.getOutputCurrent();
+    inputs.motorControllerConnected =
+        motorControllerConnectedDebounce.calculate(sparkInputs.isConnected());
 
+    // Absolute encoder is read directly (DIO, not CAN - already fast)
     inputs.absoluteEncoderConnected =
         absEncoderConnectedDebounce.calculate(absoluteEncoder.isConnected());
     inputs.absolutePosition = new Rotation2d(absoluteEncoder.get());