From 0bf91837598e1b9ffabafe61969cd5f4eb79a826 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Tue, 10 Feb 2026 20:14:24 -0500 Subject: [PATCH 01/11] Move Spark odometry I/O to separate thread. --- src/main/java/frc/robot/Robot.java | 16 +- .../subsystems/drive/DriveConstants.java | 1 + .../frc/robot/subsystems/drive/Module.java | 9 +- .../subsystems/launcher/HoodIOSpark.java | 26 +-- .../subsystems/launcher/TurretIOSpark.java | 27 +-- .../frc/robot/util/SparkOdometryThread.java | 204 ++++++++++++++++++ 6 files changed, 247 insertions(+), 36 deletions(-) create mode 100644 src/main/java/frc/robot/util/SparkOdometryThread.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d3cc206..fa8ccb1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -55,6 +55,7 @@ import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVision; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; +import frc.robot.util.SparkOdometryThread; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -206,6 +207,9 @@ public Robot() { break; } + // Start the SparkMax background thread (for non-blocking CAN reads) + SparkOdometryThread.getInstance().start(); + // Start AdvantageKit logger Logger.start(); @@ -226,6 +230,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 +251,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(); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 5ddb940..292b161 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/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); diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 52026c9..6222211 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -13,7 +13,6 @@ package frc.robot.subsystems.drive; -import static edu.wpi.first.units.Units.Meters; import static frc.robot.subsystems.drive.DriveConstants.*; import edu.wpi.first.math.geometry.Rotation2d; @@ -61,7 +60,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 +77,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 +100,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). */ diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java index a61fc5f..0715441 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java +++ b/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 AbsoluteEncoder hoodEncoder; 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); public HoodIOSpark() { hoodSpark = new SparkMax(CAN2.hood, MotorType.kBrushless); @@ -71,19 +73,19 @@ public HoodIOSpark() { () -> hoodSpark.configure( hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + + // Register with background thread for non-blocking CAN reads + sparkInputs = SparkOdometryThread.getInstance().registerSpark(hoodSpark, hoodEncoder); } @Override public void updateInputs(HoodIOInputs inputs) { - sparkStickyFault = false; - ifOk(hoodSpark, hoodEncoder::getPosition, (value) -> inputs.position = new Rotation2d(value)); - ifOk(hoodSpark, hoodEncoder::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 diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index acfdf4f..0272157 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -27,7 +27,8 @@ 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 { @@ -35,6 +36,7 @@ public class TurretIOSpark implements TurretIO { 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()); diff --git a/src/main/java/frc/robot/util/SparkOdometryThread.java b/src/main/java/frc/robot/util/SparkOdometryThread.java new file mode 100644 index 0000000..a816c32 --- /dev/null +++ b/src/main/java/frc/robot/util/SparkOdometryThread.java @@ -0,0 +1,204 @@ +package frc.robot.util; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading SparkMax/SparkFlex inputs on a background + * thread, avoiding blocking CAN calls on the main robot loop. + */ +public class SparkOdometryThread { + private static SparkOdometryThread instance; + private static final double UPDATE_FREQUENCY_HZ = 100.0; // 10ms update rate + + private final ReentrantLock lock = new ReentrantLock(); + private final Notifier notifier; + private final List registeredInputs = new ArrayList<>(); + + /** Cached inputs for a single SparkMax/SparkFlex device. */ + public static class SparkInputs { + private final SparkBase spark; + private final DoubleSupplier positionSupplier; + private final DoubleSupplier velocitySupplier; + private final DoubleSupplier[] additionalSuppliers; + + // Cached values (volatile for thread safety) + private volatile double position = 0.0; + private volatile double velocity = 0.0; + private volatile double appliedVolts = 0.0; + private volatile double outputCurrent = 0.0; + private volatile double[] additionalValues; + private volatile double timestamp = 0.0; + private volatile boolean connected = true; + + /** + * Creates SparkInputs for a device. + * + * @param spark The SparkBase device + * @param positionSupplier Supplier for encoder position + * @param velocitySupplier Supplier for encoder velocity + * @param additionalSuppliers Optional additional values to read + */ + public SparkInputs( + SparkBase spark, + DoubleSupplier positionSupplier, + DoubleSupplier velocitySupplier, + DoubleSupplier... additionalSuppliers) { + this.spark = spark; + this.positionSupplier = positionSupplier; + this.velocitySupplier = velocitySupplier; + this.additionalSuppliers = additionalSuppliers; + this.additionalValues = new double[additionalSuppliers.length]; + } + + /** Updates all cached values. Called from background thread. */ + private void update() { + boolean ok = true; + + double pos = positionSupplier.getAsDouble(); + if (spark.getLastError() != REVLibError.kOk) ok = false; + + double vel = velocitySupplier.getAsDouble(); + if (spark.getLastError() != REVLibError.kOk) ok = false; + + double output = spark.getAppliedOutput(); + if (spark.getLastError() != REVLibError.kOk) ok = false; + + double voltage = spark.getBusVoltage(); + if (spark.getLastError() != REVLibError.kOk) ok = false; + + double volts = output * voltage; + + double current = spark.getOutputCurrent(); + if (spark.getLastError() != REVLibError.kOk) ok = false; + + double[] additional = new double[additionalSuppliers.length]; + for (int i = 0; i < additionalSuppliers.length; i++) { + additional[i] = additionalSuppliers[i].getAsDouble(); + if (spark.getLastError() != REVLibError.kOk) ok = false; + } + + // Only update cached values if all reads succeeded + if (ok) { + position = pos; + velocity = vel; + appliedVolts = volts; + outputCurrent = current; + additionalValues = additional; + timestamp = RobotController.getFPGATime() / 1e6; + } + connected = ok; + } + + // Getters for cached values (called from main thread) + public double getPosition() { + return position; + } + + public double getVelocity() { + return velocity; + } + + public double getAppliedVolts() { + return appliedVolts; + } + + public double getOutputCurrent() { + return outputCurrent; + } + + public double getAdditionalValue(int index) { + return additionalValues[index]; + } + + public double getTimestamp() { + return timestamp; + } + + public boolean isConnected() { + return connected; + } + } + + /** Returns the singleton instance, creating it if necessary. */ + public static synchronized SparkOdometryThread getInstance() { + if (instance == null) { + instance = new SparkOdometryThread(); + } + return instance; + } + + private SparkOdometryThread() { + notifier = new Notifier(this::periodic); + notifier.setName("SparkOdometryThread"); + } + + /** Starts the background thread. Call after all devices are registered. */ + public void start() { + if (!registeredInputs.isEmpty()) { + notifier.startPeriodic(1.0 / UPDATE_FREQUENCY_HZ); + } + } + + /** + * Registers a SparkMax/SparkFlex device for background reading. + * + * @param spark The SparkBase device + * @param encoder The relative encoder + * @param additionalSuppliers Optional additional suppliers (e.g., absolute encoder position) + * @return SparkInputs object to read cached values from + */ + public SparkInputs registerSpark( + SparkBase spark, RelativeEncoder encoder, DoubleSupplier... additionalSuppliers) { + SparkInputs inputs = + new SparkInputs(spark, encoder::getPosition, encoder::getVelocity, additionalSuppliers); + lock.lock(); + try { + registeredInputs.add(inputs); + } finally { + lock.unlock(); + } + return inputs; + } + + /** + * Registers a SparkMax/SparkFlex device with an absolute encoder for background reading. + * + * @param spark The SparkBase device + * @param encoder The absolute encoder + * @param additionalSuppliers Optional additional suppliers + * @return SparkInputs object to read cached values from + */ + public SparkInputs registerSpark( + SparkBase spark, AbsoluteEncoder encoder, DoubleSupplier... additionalSuppliers) { + SparkInputs inputs = + new SparkInputs(spark, encoder::getPosition, encoder::getVelocity, additionalSuppliers); + lock.lock(); + try { + registeredInputs.add(inputs); + } finally { + lock.unlock(); + } + return inputs; + } + + /** Called periodically by the Notifier to update all registered inputs. */ + private void periodic() { + lock.lock(); + try { + for (SparkInputs inputs : registeredInputs) { + inputs.update(); + } + } finally { + lock.unlock(); + } + } +} From 5284f3aa616892efdebf4c002cdf168b9675da25 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Tue, 10 Feb 2026 20:36:24 -0500 Subject: [PATCH 02/11] Re-use colletions, pre-computed constants in vision --- .../frc/robot/subsystems/vision/Vision.java | 29 ++++++++++--------- .../subsystems/vision/VisionConstants.java | 9 ++++++ .../vision/VisionIOPhotonVision.java | 12 +++++--- 3 files changed, 33 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index dee85c3..8596986 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -7,7 +7,6 @@ package frc.robot.subsystems.vision; -import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.vision.VisionConstants.*; import edu.wpi.first.apriltag.AprilTagFieldLayout; @@ -25,7 +24,6 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.game.Field; import frc.robot.subsystems.vision.VisionIO.PoseObservation; import java.io.IOException; import java.util.ArrayList; @@ -34,6 +32,17 @@ import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { + // Cached arena boundary for withinBoundaries test (avoids allocations per observation) + private static final Rectangle2d arenaRectangle; + + static { + double halfWidth = minRobotWidthHalfMeters; + Translation2d cornerA = new Translation2d(halfWidth, halfWidth); + Translation2d cornerB = + new Translation2d(fieldXLenMeters - halfWidth, fieldYLenMeters - halfWidth); + arenaRectangle = new Rectangle2d(cornerA, cornerB); + } + private final VisionConsumer consumer; private final Supplier chassisPoseSupplier; private final VisionIO[] io; @@ -286,7 +295,7 @@ public double test(PoseObservation observation) { public double test(PoseObservation observation) { return 1.0 - normalizedSigmoid( - Math.abs(observation.pose().getRotation().getY()), pitchTolerance.in(Radians), 1.0); + Math.abs(observation.pose().getRotation().getY()), pitchToleranceRadians, 1.0); } }, rollError { @@ -301,7 +310,7 @@ public double test(PoseObservation observation) { public double test(PoseObservation observation) { return 1.0 - normalizedSigmoid( - Math.abs(observation.pose().getRotation().getX()), rollTolerance.in(Radians), 1.0); + Math.abs(observation.pose().getRotation().getX()), rollToleranceRadians, 1.0); } }, heightError { @@ -315,8 +324,7 @@ public double test(PoseObservation observation) { @Override public double test(PoseObservation observation) { return 1.0 - - normalizedSigmoid( - Math.abs(observation.pose().getZ()), elevationTolerance.in(Meters), 1.0); + - normalizedSigmoid(Math.abs(observation.pose().getZ()), elevationToleranceMeters, 1.0); } }, withinBoundaries { @@ -328,11 +336,7 @@ public double test(PoseObservation observation) { */ @Override public double test(PoseObservation observation) { - var cornerA = new Translation2d(minRobotWidth.div(2.0), minRobotWidth.div(2.0)); - var cornerB = new Translation2d(Field.field_x_len, Field.field_y_len).minus(cornerA); - var arena = new Rectangle2d(cornerA, cornerB); - boolean pass = arena.contains(observation.pose().toPose2d().getTranslation()); - + boolean pass = arenaRectangle.contains(observation.pose().toPose2d().getTranslation()); return (pass ? 1.0 : 0.0); } }, @@ -358,8 +362,7 @@ public double test(PoseObservation observation) { @Override public double test(PoseObservation observation) { return 1.0 - - normalizedSigmoid( - observation.averageTagDistance(), tagDistanceTolerance.in(Meters), 1.0); + - normalizedSigmoid(observation.averageTagDistance(), tagDistanceToleranceMeters, 1.0); } }; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 6ca4650..cdf2d12 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -46,10 +46,19 @@ public class VisionConstants { // Pose filtering thresholds public static double ambiguityTolerance = 0.15; public static Distance tagDistanceTolerance = Meters.of(4.0); + public static final double tagDistanceToleranceMeters = tagDistanceTolerance.in(Meters); public static Distance elevationTolerance = Meters.of(0.25); + public static final double elevationToleranceMeters = elevationTolerance.in(Meters); public static Angle rollTolerance = Degrees.of(5); + public static final double rollToleranceRadians = rollTolerance.in(Radians); public static Angle pitchTolerance = Degrees.of(5); + public static final double pitchToleranceRadians = pitchTolerance.in(Radians); + + // Cached values for arena boundary calculations + public static final double minRobotWidthHalfMeters = minRobotWidth.div(2.0).in(Meters); + public static final double fieldXLenMeters = frc.game.Field.field_x_len.in(Meters); + public static final double fieldYLenMeters = frc.game.Field.field_y_len.in(Meters); public static Distance maxTravelDistance = DriveConstants.maxDriveSpeed.times(Seconds.of(Robot.defaultPeriodSecs)); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 165ed26..49dffd2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -10,8 +10,8 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; +import java.util.ArrayList; import java.util.HashSet; -import java.util.LinkedList; import java.util.List; import java.util.Set; import org.photonvision.PhotonCamera; @@ -21,6 +21,10 @@ public class VisionIOPhotonVision implements VisionIO { protected final PhotonCamera camera; protected final Transform3d robotToCamera; + // Reusable collections to avoid allocations per loop + private final Set tagIds = new HashSet<>(); + private final List poseObservations = new ArrayList<>(); + /** * Creates a new VisionIOPhotonVision. * @@ -36,9 +40,9 @@ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { public void updateInputs(VisionIOInputs inputs) { inputs.connected = camera.isConnected(); - // Read new camera observations - Set tagIds = new HashSet<>(); - List poseObservations = new LinkedList<>(); + // Clear reusable collections + tagIds.clear(); + poseObservations.clear(); for (var result : camera.getAllUnreadResults()) { // Update latest target observation From a1c5076f9a7aff9e2ba2caf2b008ac2afc436790 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Tue, 10 Feb 2026 20:39:08 -0500 Subject: [PATCH 03/11] Batch Phoenix 6 signal refreshes in Drive --- .../frc/robot/subsystems/drive/Drive.java | 22 +++++++++++-- .../frc/robot/subsystems/drive/Module.java | 6 ++++ .../frc/robot/subsystems/drive/ModuleIO.java | 8 +++++ .../subsystems/drive/ModuleIOTalonFX.java | 33 +++++++++++++------ 4 files changed, 57 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b62c11e..ecbf9fb 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 6222211..7f5a691 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -15,6 +15,7 @@ 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; @@ -149,4 +150,9 @@ public void setTurnZero() { 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 8e8f218..0bb475f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/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]; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 26aa346..54a0c65 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/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 From e4ae16484712c3f14365a9a373cdd00be8401096 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Tue, 10 Feb 2026 20:56:26 -0500 Subject: [PATCH 04/11] Vision updates on separate thread --- src/main/java/frc/robot/Robot.java | 4 +- .../frc/robot/subsystems/vision/Vision.java | 22 ++- .../java/frc/robot/util/VisionThread.java | 145 ++++++++++++++++++ 3 files changed, 164 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/util/VisionThread.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fa8ccb1..7723bc3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -56,6 +56,7 @@ import frc.robot.subsystems.vision.VisionIOPhotonVision; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; 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; @@ -207,8 +208,9 @@ public Robot() { break; } - // Start the SparkMax background thread (for non-blocking CAN reads) + // Start background threads (for non-blocking CAN/network reads) SparkOdometryThread.getInstance().start(); + VisionThread.getInstance().start(); // Start AdvantageKit logger Logger.start(); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 8596986..09c073f 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -25,6 +25,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.vision.VisionIO.PoseObservation; +import frc.robot.util.VisionThread; +import frc.robot.util.VisionThread.VisionInputs; import java.io.IOException; import java.util.ArrayList; import java.util.EnumMap; @@ -46,6 +48,7 @@ public class Vision extends SubsystemBase { private final VisionConsumer consumer; private final Supplier chassisPoseSupplier; private final VisionIO[] io; + private final VisionInputs[] visionInputs; private final VisionIOInputsAutoLogged[] inputs; private final Alert[] disconnectedAlerts; @@ -79,7 +82,13 @@ public Vision(VisionConsumer consumer, Supplier chassisPoseSupplier, Vis this.chassisPoseSupplier = chassisPoseSupplier; this.io = io; - // Initialize inputs + // Register each VisionIO with VisionThread for background polling + this.visionInputs = new VisionInputs[io.length]; + for (int i = 0; i < io.length; i++) { + visionInputs[i] = VisionThread.getInstance().registerVisionIO(io[i]); + } + + // Initialize inputs for AdvantageKit logging this.inputs = new VisionIOInputsAutoLogged[io.length]; for (int i = 0; i < inputs.length; i++) { inputs[i] = new VisionIOInputsAutoLogged(); @@ -106,7 +115,8 @@ public Rotation2d getTargetX(int cameraIndex) { @Override public void periodic() { for (int i = 0; i < io.length; i++) { - io[i].updateInputs(inputs[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]); } @@ -241,11 +251,11 @@ public static record TestedObservation( EnumMap testResults, double score) {} - // Caching for AprilTag layout - public static AprilTagFieldLayout cachedLayout = null; + // Caching for AprilTag layout (volatile for thread-safe lazy initialization) + private static volatile AprilTagFieldLayout cachedLayout = null; - /** Returns the AprilTag layout to use, loading it if necessary. */ - public static AprilTagFieldLayout getAprilTagLayout() { + /** Returns the AprilTag layout to use, loading it if necessary. Thread-safe. */ + public static synchronized AprilTagFieldLayout getAprilTagLayout() { if (cachedLayout == null) { // Try to load custom layout only if requested and not connected to FMS if (useCustomAprilTagLayout && !DriverStation.isFMSAttached()) { diff --git a/src/main/java/frc/robot/util/VisionThread.java b/src/main/java/frc/robot/util/VisionThread.java new file mode 100644 index 0000000..5274e3c --- /dev/null +++ b/src/main/java/frc/robot/util/VisionThread.java @@ -0,0 +1,145 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.Notifier; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIO.PoseObservation; +import frc.robot.subsystems.vision.VisionIO.TargetObservation; +import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.locks.ReentrantLock; + +/** + * Provides an interface for asynchronously reading vision inputs on a background thread, avoiding + * blocking network calls on the main robot loop. + */ +public class VisionThread { + private static VisionThread instance; + private static final double UPDATE_FREQUENCY_HZ = 50.0; // 20ms update rate + + private final ReentrantLock lock = new ReentrantLock(); + private final Notifier notifier; + private final List registeredInputs = new ArrayList<>(); + + /** Cached inputs for a single VisionIO device. */ + public static class VisionInputs { + private final VisionIO io; + private final VisionIOInputs workingInputs = new VisionIOInputs(); + + // Cached snapshot (volatile reference for thread safety) + private volatile VisionIOInputsSnapshot snapshot = new VisionIOInputsSnapshot(); + + public VisionInputs(VisionIO io) { + this.io = io; + } + + /** Updates cached values. Called from background thread. */ + private void update() { + // Call the potentially-blocking updateInputs + io.updateInputs(workingInputs); + + // Create an immutable snapshot of the current state + snapshot = + new VisionIOInputsSnapshot( + workingInputs.connected, + workingInputs.latestTargetObservation, + workingInputs.poseObservations.clone(), + workingInputs.tagIds.clone()); + } + + /** Returns the cached snapshot. Safe to call from main thread. */ + public VisionIOInputsSnapshot getSnapshot() { + return snapshot; + } + } + + /** Immutable snapshot of VisionIOInputs for thread-safe reading. */ + public static class VisionIOInputsSnapshot { + public final boolean connected; + public final TargetObservation latestTargetObservation; + public final PoseObservation[] poseObservations; + public final int[] tagIds; + + public VisionIOInputsSnapshot() { + this.connected = false; + this.latestTargetObservation = + new TargetObservation( + edu.wpi.first.math.geometry.Rotation2d.kZero, + edu.wpi.first.math.geometry.Rotation2d.kZero, + edu.wpi.first.math.geometry.Rotation2d.kZero, + 0, + -1, + -1); + this.poseObservations = new PoseObservation[0]; + this.tagIds = new int[0]; + } + + public VisionIOInputsSnapshot( + boolean connected, + TargetObservation latestTargetObservation, + PoseObservation[] poseObservations, + int[] tagIds) { + this.connected = connected; + this.latestTargetObservation = latestTargetObservation; + this.poseObservations = poseObservations; + this.tagIds = tagIds; + } + + /** Copies this snapshot's values into a VisionIOInputs object for AdvantageKit logging. */ + public void copyTo(VisionIOInputs inputs) { + inputs.connected = connected; + inputs.latestTargetObservation = latestTargetObservation; + inputs.poseObservations = poseObservations; + inputs.tagIds = tagIds; + } + } + + /** Returns the singleton instance, creating it if necessary. */ + public static synchronized VisionThread getInstance() { + if (instance == null) { + instance = new VisionThread(); + } + return instance; + } + + private VisionThread() { + notifier = new Notifier(this::periodic); + notifier.setName("VisionThread"); + } + + /** Starts the background thread. Call after all devices are registered. */ + public void start() { + if (!registeredInputs.isEmpty()) { + notifier.startPeriodic(1.0 / UPDATE_FREQUENCY_HZ); + } + } + + /** + * Registers a VisionIO device for background reading. + * + * @param io The VisionIO device + * @return VisionInputs object to read cached values from + */ + public VisionInputs registerVisionIO(VisionIO io) { + VisionInputs inputs = new VisionInputs(io); + lock.lock(); + try { + registeredInputs.add(inputs); + } finally { + lock.unlock(); + } + return inputs; + } + + /** Called periodically by the Notifier to update all registered inputs. */ + private void periodic() { + lock.lock(); + try { + for (VisionInputs inputs : registeredInputs) { + inputs.update(); + } + } finally { + lock.unlock(); + } + } +} From 4fe5e9b3de2b401cd6136226bd3c983bedb54233 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Tue, 10 Feb 2026 23:07:13 -0500 Subject: [PATCH 05/11] Create CanandgyroThread for Redux gyro CAN calls --- src/main/java/frc/robot/Robot.java | 2 + .../robot/subsystems/drive/GyroIOBoron.java | 18 +-- .../java/frc/robot/util/CanandgyroThread.java | 114 ++++++++++++++++++ .../frc/robot/util/SparkOdometryThread.java | 6 +- .../java/frc/robot/util/VisionThread.java | 6 +- 5 files changed, 131 insertions(+), 15 deletions(-) create mode 100644 src/main/java/frc/robot/util/CanandgyroThread.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7723bc3..8f27d80 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -56,6 +56,7 @@ import frc.robot.subsystems.vision.VisionIOPhotonVision; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import frc.robot.util.SparkOdometryThread; +import frc.robot.util.CanandgyroThread; import frc.robot.util.VisionThread; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -211,6 +212,7 @@ public Robot() { // Start background threads (for non-blocking CAN/network reads) SparkOdometryThread.getInstance().start(); VisionThread.getInstance().start(); + CanandgyroThread.getInstance().start(); // Start AdvantageKit logger Logger.start(); diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOBoron.java b/src/main/java/frc/robot/subsystems/drive/GyroIOBoron.java index 5c0dab2..9e55d9a 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOBoron.java +++ b/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 yawTimestampQueue; private final Queue 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++) { diff --git a/src/main/java/frc/robot/util/CanandgyroThread.java b/src/main/java/frc/robot/util/CanandgyroThread.java new file mode 100644 index 0000000..85a42ce --- /dev/null +++ b/src/main/java/frc/robot/util/CanandgyroThread.java @@ -0,0 +1,114 @@ +package frc.robot.util; + +import com.reduxrobotics.sensors.canandgyro.Canandgyro; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.locks.ReentrantLock; + +/** + * Provides an interface for asynchronously reading Canandgyro inputs on a background thread, + * avoiding blocking CAN calls on the main robot loop. + */ +public class CanandgyroThread { + private static CanandgyroThread instance; + private static final double UPDATE_FREQUENCY_HZ = 100.0; // 10ms update rate + + private final ReentrantLock lock = new ReentrantLock(); + private final Notifier notifier; + private final List registeredInputs = new ArrayList<>(); + + /** Cached inputs for a single Canandgyro device. */ + public static class GyroInputs { + private final Canandgyro gyro; + + // Cached values (volatile for thread safety) + private volatile boolean connected = false; + private volatile boolean calibrating = true; + private volatile double yaw = 0.0; + private volatile double angularVelocityYaw = 0.0; + private volatile double timestamp = 0.0; + + public GyroInputs(Canandgyro gyro) { + this.gyro = gyro; + } + + /** Updates all cached values. Called from background thread. */ + private void update() { + connected = gyro.isConnected(); + calibrating = gyro.isCalibrating(); + yaw = gyro.getYaw(); + angularVelocityYaw = gyro.getAngularVelocityYaw(); + timestamp = RobotController.getFPGATime() / 1e6; + } + + // Getters for cached values (called from main thread) + public boolean isConnected() { + return connected; + } + + public boolean isCalibrating() { + return calibrating; + } + + public double getYaw() { + return yaw; + } + + public double getAngularVelocityYaw() { + return angularVelocityYaw; + } + + public double getTimestamp() { + return timestamp; + } + } + + /** Returns the singleton instance, creating it if necessary. */ + public static synchronized CanandgyroThread getInstance() { + if (instance == null) { + instance = new CanandgyroThread(); + } + return instance; + } + + private CanandgyroThread() { + notifier = new Notifier(this::periodic); + notifier.setName("CanandgyroThread"); + } + + /** Starts the background thread. */ + public void start() { + notifier.startPeriodic(1.0 / UPDATE_FREQUENCY_HZ); + } + + /** + * Registers a Canandgyro device for background reading. + * + * @param gyro The Canandgyro device + * @return GyroInputs object to read cached values from + */ + public GyroInputs registerCanandgyro(Canandgyro gyro) { + GyroInputs inputs = new GyroInputs(gyro); + lock.lock(); + try { + registeredInputs.add(inputs); + } finally { + lock.unlock(); + } + return inputs; + } + + /** Called periodically by the Notifier to update all registered inputs. */ + private void periodic() { + lock.lock(); + try { + for (GyroInputs inputs : registeredInputs) { + inputs.update(); + } + } finally { + lock.unlock(); + } + } +} diff --git a/src/main/java/frc/robot/util/SparkOdometryThread.java b/src/main/java/frc/robot/util/SparkOdometryThread.java index a816c32..74023d0 100644 --- a/src/main/java/frc/robot/util/SparkOdometryThread.java +++ b/src/main/java/frc/robot/util/SparkOdometryThread.java @@ -141,11 +141,9 @@ private SparkOdometryThread() { notifier.setName("SparkOdometryThread"); } - /** Starts the background thread. Call after all devices are registered. */ + /** Starts the background thread. */ public void start() { - if (!registeredInputs.isEmpty()) { - notifier.startPeriodic(1.0 / UPDATE_FREQUENCY_HZ); - } + notifier.startPeriodic(1.0 / UPDATE_FREQUENCY_HZ); } /** diff --git a/src/main/java/frc/robot/util/VisionThread.java b/src/main/java/frc/robot/util/VisionThread.java index 5274e3c..81b993c 100644 --- a/src/main/java/frc/robot/util/VisionThread.java +++ b/src/main/java/frc/robot/util/VisionThread.java @@ -107,11 +107,9 @@ private VisionThread() { notifier.setName("VisionThread"); } - /** Starts the background thread. Call after all devices are registered. */ + /** Starts the background thread. */ public void start() { - if (!registeredInputs.isEmpty()) { - notifier.startPeriodic(1.0 / UPDATE_FREQUENCY_HZ); - } + notifier.startPeriodic(1.0 / UPDATE_FREQUENCY_HZ); } /** From 6e7b96d47910508945035ba5f8708ab6d571f575 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Tue, 10 Feb 2026 23:08:22 -0500 Subject: [PATCH 06/11] replace repeated setUpdateFrequencyForAll with refreshAll --- .../robot/subsystems/intake/IntakeRollerIOTalonFX.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java index e0a5a4c..33a5baa 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java +++ b/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(); From 1803c9819360d9866b3f19dc1e205b7d294b9f3b Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Thu, 12 Feb 2026 13:24:05 -0500 Subject: [PATCH 07/11] Add documentation --- doc/TWEAKS.md | 338 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 338 insertions(+) create mode 100644 doc/TWEAKS.md diff --git a/doc/TWEAKS.md b/doc/TWEAKS.md new file mode 100644 index 0000000..81f9850 --- /dev/null +++ b/doc/TWEAKS.md @@ -0,0 +1,338 @@ +# Performance Tweaks + +This document describes all changes made on the `performance-tweaks` branch. These optimizations reduce main loop latency by moving blocking I/O operations to background threads and eliminating redundant allocations/computations. + +## Table of Contents + +1. [SparkOdometryThread](#1-sparkodometrythread---background-thread-for-rev-spark-motor-controllers) +2. [CanandgyroThread](#2-canandgyrothread---background-thread-for-redux-canandgyro) +3. [VisionThread](#3-visionthread---background-thread-for-photonvision) +4. [Batched Phoenix 6 Signal Refreshes](#4-batched-phoenix-6-signal-refreshes-in-drive) +5. [Pre-computed Constants in Vision](#5-pre-computed-constants-in-vision) +6. [Re-use Collections in VisionIOPhotonVision](#6-re-use-collections-in-visioniophotonvision) +7. [Pre-computed wheelRadiusMeters](#7-pre-computed-wheelradiusmeters-in-driveconstants) +8. [IntakeRollerIOTalonFX Fix](#8-intakerolleriotalonfx-fix) +9. [SmartDashboard.putData Optimization](#9-smartdashboardputdata-moved-to-robot-constructor) + +--- + +## 1. SparkOdometryThread - Background Thread for REV Spark Motor Controllers + +**New file:** `src/main/java/frc/robot/util/SparkOdometryThread.java` + +### How It Works + +This creates a singleton background thread using WPILib's `Notifier` that runs at 100Hz (10ms period). REV Spark motor controllers (SparkMax/SparkFlex) use blocking CAN calls - when you call `encoder.getPosition()`, the code blocks until the CAN frame returns. This can add several milliseconds of latency per device. + +The thread maintains a list of registered `SparkInputs` objects. Each `SparkInputs` wraps a SparkBase device and caches: + +- Position (from encoder) +- Velocity (from encoder) +- Applied voltage (computed as `getAppliedOutput() * getBusVoltage()`) +- Output current +- Connection status (based on `REVLibError.kOk` checks) +- Timestamp + +The `update()` method reads all values from the device, checks for errors after each read, and only updates the cached values if all reads succeeded. This ensures atomic updates - you never see partially updated data. + +**Thread safety:** All cached fields are `volatile`, ensuring visibility across threads. The `ReentrantLock` protects the registration list from concurrent modification. + +### Changes to Subsystems + +**HoodIOSpark.java:** + +Before: +```java +ifOk(hoodSpark, hoodEncoder::getPosition, (value) -> inputs.position = new Rotation2d(value)); +ifOk(hoodSpark, hoodEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); +ifOk(hoodSpark, new DoubleSupplier[] {hoodSpark::getAppliedOutput, hoodSpark::getBusVoltage}, + (values) -> inputs.appliedVolts = values[0] * values[1]); +``` + +After: +```java +inputs.position = new Rotation2d(sparkInputs.getPosition()); +inputs.velocityRadPerSec = sparkInputs.getVelocity(); +inputs.appliedVolts = sparkInputs.getAppliedVolts(); +inputs.currentAmps = sparkInputs.getOutputCurrent(); +inputs.connected = connectedDebounce.calculate(sparkInputs.isConnected()); +``` + +**Why behavior is unchanged:** The values read are identical - position, velocity, applied volts, current, and connection status. The only difference is *when* the CAN read happens (background thread vs main loop). The same `ifOk` pattern that validated `sparkStickyFault` is replaced by `SparkInputs.isConnected()`, which performs the same error checking inside `update()`. + +**TurretIOSpark.java:** Same pattern as HoodIOSpark. Note the absolute encoder (DIO) is still read directly since DIO reads are already fast. + +--- + +## 2. CanandgyroThread - Background Thread for Redux Canandgyro + +**New file:** `src/main/java/frc/robot/util/CanandgyroThread.java` + +### How It Works + +Same pattern as SparkOdometryThread, but for the Redux Canandgyro. Runs at 100Hz and caches: + +- `isConnected()` +- `isCalibrating()` +- `getYaw()` +- `getAngularVelocityYaw()` + +### Changes to GyroIOBoron + +**GyroIOBoron.java:** + +Before: +```java +inputs.connected = canandgyro.isConnected(); +inputs.calibrated = !canandgyro.isCalibrating(); +inputs.yawPosition = Rotation2d.fromRotations(canandgyro.getYaw()); +inputs.yawVelocityRadPerSec = Units.rotationsToRadians(canandgyro.getAngularVelocityYaw()); +``` + +After: +```java +inputs.connected = gyroInputs.isConnected(); +inputs.calibrated = !gyroInputs.isCalibrating(); +inputs.yawPosition = Rotation2d.fromRotations(gyroInputs.getYaw()); +inputs.yawVelocityRadPerSec = Units.rotationsToRadians(gyroInputs.getAngularVelocityYaw()); +``` + +**Why behavior is unchanged:** The exact same values are read and converted. The odometry queue registration with `PhoenixOdometryThread` is untouched - high-frequency odometry samples still flow through that existing mechanism. + +--- + +## 3. VisionThread - Background Thread for PhotonVision + +**New file:** `src/main/java/frc/robot/util/VisionThread.java` + +### How It Works + +Runs at 50Hz (20ms period). PhotonVision uses NetworkTables, which can have variable latency. Each registered `VisionIO` is polled on the background thread. + +Key design: Uses an **immutable snapshot** pattern. The background thread creates a new `VisionIOInputsSnapshot` object each update, containing cloned arrays. The main thread reads a volatile reference to this snapshot. This avoids any possibility of reading partially-updated data. + +```java +snapshot = new VisionIOInputsSnapshot( + workingInputs.connected, + workingInputs.latestTargetObservation, + workingInputs.poseObservations.clone(), // Clone to ensure immutability + workingInputs.tagIds.clone()); +``` + +### Changes to Vision Subsystem + +**Vision.java:** + +Before: +```java +for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); +} +``` + +After: +```java +for (int i = 0; i < io.length; i++) { + visionInputs[i].getSnapshot().copyTo(inputs[i]); + Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); +} +``` + +**Why behavior is unchanged:** The `copyTo()` method copies all four fields (`connected`, `latestTargetObservation`, `poseObservations`, `tagIds`) into the AdvantageKit input object. All downstream processing (pose estimation, scoring, etc.) operates on these inputs identically. + +--- + +## 4. Batched Phoenix 6 Signal Refreshes in Drive + +### How It Works + +Phoenix 6 (CTRE TalonFX) provides `BaseStatusSignal.refreshAll()` which can batch multiple signal reads into fewer CAN frames. Previously, each module called `refreshSignals()` individually. + +**ModuleIOTalonFX.java:** + +Added a cached array of all 9 signals per module: +```java +allSignals = new BaseStatusSignal[] { + drivePosition, driveVelocity, driveAppliedVolts, driveCurrent, + turnPosition, turnVelocity, turnAppliedVolts, turnCurrent, + turnAbsolutePosition +}; +``` + +Added `getStatusSignals()` method to expose these for batching. + +**Drive.java:** + +Before: +```java +for (var module : modules) { + module.refreshSignals(); +} +``` + +After: +```java +// Collect signals from all 4 modules into one array (done in constructor) +if (allModuleSignals.length > 0) { + BaseStatusSignal.refreshAll(allModuleSignals); // Single batched CAN call +} +``` + +**Why behavior is unchanged:** The same signals are refreshed with the same data. The only difference is that 4 separate `refreshAll()` calls (one per module, each with 9 signals = 36 individual signal refreshes) become 1 `refreshAll()` call with all 36 signals. The Phoenix 6 library optimizes this internally. + +--- + +## 5. Pre-computed Constants in Vision + +### How It Works + +WPILib's Units API (`Distance`, `Angle`) calls `.in(Units.X)` to convert to raw doubles. This involves virtual method calls and some overhead. For constants used every loop iteration, we pre-compute the double value once. + +**VisionConstants.java:** + +```java +public static Distance tagDistanceTolerance = Meters.of(4.0); +public static final double tagDistanceToleranceMeters = tagDistanceTolerance.in(Meters); // NEW + +public static Distance elevationTolerance = Meters.of(0.25); +public static final double elevationToleranceMeters = elevationTolerance.in(Meters); // NEW +// ... same for rollToleranceRadians, pitchToleranceRadians, field dimensions +``` + +**Vision.java:** + +The `VisionTest` enum methods now use pre-computed values: + +Before: +```java +normalizedSigmoid(Math.abs(observation.pose().getRotation().getY()), pitchTolerance.in(Radians), 1.0); +``` + +After: +```java +normalizedSigmoid(Math.abs(observation.pose().getRotation().getY()), pitchToleranceRadians, 1.0); +``` + +**Arena boundary optimization:** + +Before (called per observation): +```java +var cornerA = new Translation2d(minRobotWidth.div(2.0), minRobotWidth.div(2.0)); +var cornerB = new Translation2d(Field.field_x_len, Field.field_y_len).minus(cornerA); +var arena = new Rectangle2d(cornerA, cornerB); +boolean pass = arena.contains(observation.pose().toPose2d().getTranslation()); +``` + +After (computed once in static initializer): +```java +private static final Rectangle2d arenaRectangle; +static { + double halfWidth = minRobotWidthHalfMeters; + Translation2d cornerA = new Translation2d(halfWidth, halfWidth); + Translation2d cornerB = new Translation2d(fieldXLenMeters - halfWidth, fieldYLenMeters - halfWidth); + arenaRectangle = new Rectangle2d(cornerA, cornerB); +} +// In test method: +boolean pass = arenaRectangle.contains(observation.pose().toPose2d().getTranslation()); +``` + +**Why behavior is unchanged:** `pitchTolerance.in(Radians)` returns the same double as `pitchToleranceRadians` - it's just computed at class load time instead of every call. The rectangle dimensions are identical. + +--- + +## 6. Re-use Collections in VisionIOPhotonVision + +**VisionIOPhotonVision.java:** + +Before (allocates new collections every call): +```java +Set tagIds = new HashSet<>(); +List poseObservations = new LinkedList<>(); +``` + +After (reuses instance fields): +```java +// Instance fields: +private final Set tagIds = new HashSet<>(); +private final List poseObservations = new ArrayList<>(); + +// In updateInputs(): +tagIds.clear(); +poseObservations.clear(); +``` + +**Why behavior is unchanged:** The collections are cleared at the start of each call, then populated identically. The logic is unchanged; only the allocation strategy differs. Also changed `LinkedList` to `ArrayList` which has better cache locality for iteration. + +--- + +## 7. Pre-computed wheelRadiusMeters in DriveConstants + +**DriveConstants.java:** +```java +public static final Distance wheelRadius = Inches.of(2); +public static final double wheelRadiusMeters = wheelRadius.in(Meters); // NEW +``` + +**Module.java:** + +Before: +```java +double positionMeters = inputs.odometryDrivePositionsRad[i] * wheelRadius.in(Meters); +``` + +After: +```java +double positionMeters = inputs.odometryDrivePositionsRad[i] * wheelRadiusMeters; +``` + +**Why behavior is unchanged:** The same conversion factor is used; it's simply pre-computed at class load time. + +--- + +## 8. IntakeRollerIOTalonFX Fix + +**IntakeRollerIOTalonFX.java:** + +Before (incorrect - `setUpdateFrequencyForAll` changes frequency, doesn't refresh): +```java +BaseStatusSignal.setUpdateFrequencyForAll(50.0, intakeVelocity, intakeAppliedVolts, intakeCurrent).isOK(); +inputs.appliedVolts = intakeAppliedVolts.getValueAsDouble(); +``` + +After (correct - actually refreshes the signals): +```java +var status = BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent); +if (!status.isOK()) { + return; // Don't update inputs with stale data +} +inputs.appliedVolts = intakeAppliedVolts.getValueAsDouble(); +``` + +**Why behavior is unchanged (or improved):** The original code was calling `setUpdateFrequencyForAll` every loop, which is wasteful (frequency should only be set once in the constructor). It then read signal values that may not have been refreshed. The new code properly refreshes signals before reading them. This is a bug fix that makes behavior *more* correct. + +--- + +## 9. SmartDashboard.putData Moved to Robot Constructor + +**Robot.java:** + +`SmartDashboard.putData()` calls were moved from `robotPeriodic()` to the constructor. These calls register objects with SmartDashboard for Shuffleboard/Glass - they only need to be called once. + +**Why behavior is unchanged:** The same subsystems are registered. Calling `putData` repeatedly with the same key just overwrites the same entry. + +--- + +## Summary + +All changes fall into these categories: + +| Category | Changes | Benefit | +|----------|---------|---------| +| **Background threading** | SparkOdometryThread, CanandgyroThread, VisionThread | Move blocking I/O to background threads. Main loop reads cached values instead of blocking. | +| **Batching** | Drive signal refresh | Combine multiple CAN operations into one call that the library can optimize. | +| **Allocation avoidance** | Vision collections, arena Rectangle2d | Reuse objects instead of allocating new ones every loop. Reduces GC pressure. | +| **Pre-computation** | wheelRadiusMeters, tolerance constants | Compute unit conversions once at startup instead of every call. | +| **Bug fix** | IntakeRollerIOTalonFX | Replace incorrect `setUpdateFrequencyForAll` with correct `refreshAll`. | + +None of these changes alter the logical behavior of the robot code - they all produce identical values through more efficient means. From 7a0708b504eaa0b4f49bac531f1a8a4eb353c760 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Thu, 12 Feb 2026 17:53:25 -0500 Subject: [PATCH 08/11] Updated tweaks doc --- doc/TWEAKS.md | 144 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 144 insertions(+) diff --git a/doc/TWEAKS.md b/doc/TWEAKS.md index 81f9850..b6fa908 100644 --- a/doc/TWEAKS.md +++ b/doc/TWEAKS.md @@ -13,6 +13,7 @@ This document describes all changes made on the `performance-tweaks` branch. The 7. [Pre-computed wheelRadiusMeters](#7-pre-computed-wheelradiusmeters-in-driveconstants) 8. [IntakeRollerIOTalonFX Fix](#8-intakerolleriotalonfx-fix) 9. [SmartDashboard.putData Optimization](#9-smartdashboardputdata-moved-to-robot-constructor) +10. [Gotchas and Considerations](#gotchas-and-considerations) --- @@ -336,3 +337,146 @@ All changes fall into these categories: | **Bug fix** | IntakeRollerIOTalonFX | Replace incorrect `setUpdateFrequencyForAll` with correct `refreshAll`. | None of these changes alter the logical behavior of the robot code - they all produce identical values through more efficient means. + +--- + +## Gotchas and Considerations + +This section describes potential issues to be aware of when working with the background-threaded I/O system. + +### 1. Data Latency (Up to 10-20ms Stale) + +| Thread | Update Rate | Max Staleness | +|--------|-------------|---------------| +| SparkOdometryThread | 100Hz | 10ms | +| CanandgyroThread | 100Hz | 10ms | +| VisionThread | 50Hz | 20ms | + +**Impact:** Control loops reading from cached values may be working with slightly old data. For the **hood and turret** (slow-moving mechanisms), 10ms latency is negligible. For **vision pose estimation**, the timestamps are already used to account for latency, so this shouldn't cause issues. + +**No changes needed** for typical FRC control loops. + +--- + +### 2. First-Loop Initialization Race + +The background threads start **after** subsystem construction but the first `update()` hasn't run yet. Initial cached values are: + +```java +private volatile double position = 0.0; +private volatile double velocity = 0.0; +private volatile boolean connected = true; // Optimistically true! +``` + +**Impact:** For the first ~10-20ms after boot, subsystems will read zero position/velocity while `isConnected()` returns `true`. + +**Mitigation:** The Debouncer in HoodIOSpark/TurretIOSpark (0.5s falling edge) prevents spurious disconnection alerts. Position/velocity at zero for one loop iteration is unlikely to cause control issues since mechanisms start at rest anyway. + +**Consider:** If you add a new Spark-based subsystem that needs accurate initial readings (e.g., for homing), you may need to add explicit "first reading received" logic. + +--- + +### 3. Non-Atomic Multi-Field Reads + +Each field is `volatile`, but reading multiple fields is **not atomic**: + +```java +// These could be from different update cycles! +inputs.position = new Rotation2d(sparkInputs.getPosition()); +inputs.velocityRadPerSec = sparkInputs.getVelocity(); +``` + +**Impact:** Position might be from cycle N while velocity is from cycle N+1. At 100Hz updates, this represents a ~10ms inconsistency in the worst case. + +**In practice:** For hood/turret control, this is negligible. The position and velocity of these mechanisms don't change drastically in 10ms. + +**If you need atomicity:** You would need to add a snapshot pattern (like VisionThread uses) or a lock around reads. Currently not needed for hood/turret. + +--- + +### 4. Persistent CAN Errors = Stale Data + +In `SparkInputs.update()`: + +```java +if (ok) { + position = pos; + // ... update values +} +connected = ok; // Always updated +``` + +**Impact:** If CAN communication fails persistently, `isConnected()` becomes `false`, but `getPosition()` returns the **last good value**. The Debouncer delays marking disconnected by 0.5s. + +**This is acceptable behavior** - returning stale data is better than returning garbage or crashing. The `connected` flag lets you detect and handle this. + +**Be aware:** Don't assume position is valid just because it's non-zero. Check `isConnected()` if you need to know data freshness. + +--- + +### 5. Lock Held During I/O (VisionThread) + +```java +private void periodic() { + lock.lock(); + try { + for (VisionInputs inputs : registeredInputs) { + inputs.update(); // <-- This does network I/O while holding lock! + } + } finally { + lock.unlock(); + } +} +``` + +**Impact:** If you call `registerVisionIO()` at runtime (unlikely), it will block until all cameras finish their network calls. + +**In practice:** Registration only happens during construction, before the thread starts. No issue for normal use. + +--- + +### 6. Simulation Mode Works Differently + +In simulation: +- `HoodIOSim`, `TurretIOSim` are used instead of `HoodIOSpark`, `TurretIOSpark` +- The Sim classes **do not register** with SparkOdometryThread +- They read directly from physics simulation, no caching + +**This is correct behavior** - simulation doesn't need background threading since there's no real CAN latency. + +**Be aware:** If you test threading logic, you must test on real hardware or mock the threading behavior explicitly. + +--- + +### 7. High-Frequency Odometry Unaffected + +The swerve drive odometry uses `PhoenixOdometryThread` (250Hz) for TalonFX motors, which is **unchanged**. The `GyroIOBoron` still registers yaw samples with `PhoenixOdometryThread` for high-frequency odometry: + +```java +yawPositionQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); +yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(canandgyro::getYaw); +``` + +The new `CanandgyroThread` only caches the periodic `updateInputs()` values (connected, calibrating, etc.), **not** the odometry samples. + +**No impact on pose estimation accuracy.** + +--- + +### Summary: Do You Need to Change Anything? + +**For typical robot operation: No.** + +The changes are designed to be transparent. Subsystems read the same values through a different mechanism. + +**Situations where you might need changes:** + +| Scenario | Consideration | +|----------|---------------| +| Adding a new Spark subsystem | Register with `SparkOdometryThread.getInstance().registerSpark()` in the IO constructor | +| Need immediate accurate readings at boot | Add explicit "initialized" flag and wait for first update | +| Debugging timing issues | Log `sparkInputs.getTimestamp()` to see data age | +| Very fast control loops (>100Hz) | Spark data updates at 100Hz max - may need to increase `UPDATE_FREQUENCY_HZ` | +| AdvantageKit log replay | Background threads still run but IO implementations are replaced - should work fine | + +The main conceptual shift is: **CAN/network reads are now asynchronous.** You're always reading "recent" data rather than "current" data. For FRC applications with 20ms loop periods, this distinction is rarely meaningful. From 293b66e28fb7ecbd8e53c63d0f4f55d3b2809de2 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Thu, 12 Feb 2026 18:50:51 -0500 Subject: [PATCH 09/11] spotless --- src/main/java/frc/robot/Robot.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8f27d80..746296c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -55,8 +55,9 @@ import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVision; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; -import frc.robot.util.SparkOdometryThread; + 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; From 9767513ac635a91287b834e302628a9d350b2584 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Thu, 12 Feb 2026 18:53:56 -0500 Subject: [PATCH 10/11] spotless --- src/main/java/frc/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 746296c..a09c1e2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -55,7 +55,6 @@ 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; From 6cb749dcfc19db773cab632d6968de9617936c29 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 12 Feb 2026 19:03:06 -0500 Subject: [PATCH 11/11] rename encoder --- src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java index da31d58..7dfcd4f 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java @@ -81,7 +81,7 @@ public HoodIOSpark() { hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); // Register with background thread for non-blocking CAN reads - sparkInputs = SparkOdometryThread.getInstance().registerSpark(hoodSpark, hoodEncoder); + sparkInputs = SparkOdometryThread.getInstance().registerSpark(hoodSpark, encoderSpark); } @Override