diff --git a/doc/TWEAKS.md b/doc/TWEAKS.md new file mode 100644 index 0000000..b6fa908 --- /dev/null +++ b/doc/TWEAKS.md @@ -0,0 +1,482 @@ +# 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) +10. [Gotchas and Considerations](#gotchas-and-considerations) + +--- + +## 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. + +--- + +## 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. diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d3cc206..a09c1e2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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(); 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/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/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/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 52026c9..7f5a691 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/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(); + } } 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 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(); diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java index c693759..7dfcd4f 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 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 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/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index dee85c3..09c073f 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,8 +24,9 @@ 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 frc.robot.util.VisionThread; +import frc.robot.util.VisionThread.VisionInputs; import java.io.IOException; import java.util.ArrayList; import java.util.EnumMap; @@ -34,9 +34,21 @@ 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; + private final VisionInputs[] visionInputs; private final VisionIOInputsAutoLogged[] inputs; private final Alert[] disconnectedAlerts; @@ -70,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(); @@ -97,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]); } @@ -232,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()) { @@ -286,7 +305,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 +320,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 +334,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 +346,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 +372,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 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 new file mode 100644 index 0000000..74023d0 --- /dev/null +++ b/src/main/java/frc/robot/util/SparkOdometryThread.java @@ -0,0 +1,202 @@ +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. */ + public void start() { + 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(); + } + } +} 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..81b993c --- /dev/null +++ b/src/main/java/frc/robot/util/VisionThread.java @@ -0,0 +1,143 @@ +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. */ + public void start() { + 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(); + } + } +}