diff --git a/.gitignore b/.gitignore index f809adc..8ba4568 100644 --- a/.gitignore +++ b/.gitignore @@ -38,6 +38,7 @@ bin/ # Simulator runtime files networktables.json simgui*.json +ctre_sim/ # JVM crash logs hs_err_pid*.log diff --git a/build.gradle b/build.gradle index 8c1b3c9..322a386 100644 --- a/build.gradle +++ b/build.gradle @@ -8,13 +8,6 @@ java { targetCompatibility = JavaVersion.VERSION_17 } -repositories { - mavenCentral() - maven { - url = uri("https://maven.ctr-electronics.com/release/") - } -} - def ROBOT_MAIN_CLASS = "frc.robot.Main" // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat deleted file mode 100644 index 3007495..0000000 Binary files a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat deleted file mode 100644 index 8871b54..0000000 Binary files a/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat deleted file mode 100644 index 44d4d24..0000000 Binary files a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat deleted file mode 100644 index 2bbdfc9..0000000 Binary files a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat deleted file mode 100644 index 28822ad..0000000 Binary files a/ctre_sim/CANCoder vers. H - 020 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat deleted file mode 100644 index 28822ad..0000000 Binary files a/ctre_sim/CANCoder vers. H - 021 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat deleted file mode 100644 index 2bbdfc9..0000000 Binary files a/ctre_sim/CANCoder vers. H - 022 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat deleted file mode 100644 index caadd26..0000000 Binary files a/ctre_sim/CANCoder vers. H - 07 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat deleted file mode 100644 index 31c730b..0000000 Binary files a/ctre_sim/Pigeon 2 - 017 - 0 - ext.dat and /dev/null differ diff --git a/ctre_sim/Pigeon 2 - 023 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 023 - 0 - ext.dat deleted file mode 100644 index 95ad7c5..0000000 Binary files a/ctre_sim/Pigeon 2 - 023 - 0 - ext.dat and /dev/null differ diff --git a/docs/2024-26-comparison-report.md b/docs/2024-26-comparison-report.md new file mode 100644 index 0000000..ce91588 --- /dev/null +++ b/docs/2024-26-comparison-report.md @@ -0,0 +1,1409 @@ +# 2024 → 2026 Swerve Codebase Comparison Report + +## Executive Summary + +*For coaches, mentors, and team leads — the key findings at a glance.* + +--- + +### Part A: Teleop Issues — Drift and Jitter + +**The two reported problems:** +1. The robot doesn't drive straight — it drifts/pulls to one side. +2. One swerve module jitters — a single angle motor oscillates back and forth when the robot is at rest. + +**Root causes found:** + +A systematic comparison of every drive-related file between the working 2024 codebase and the current 2026 codebase identified **three code-level issues** that are the most likely causes: + +| # | Issue | Impact | Fix Difficulty | +|---|-------|--------|----------------| +| 1 | **Stale calibration data overriding module angle offsets.** The 2026 code added a "Preferences" system that saves angle offsets to the roboRIO. Once a value is stored in Preferences, it **silently overrides** any subsequent changes to `Constants.java` on every boot — meaning the team can update Constants all day and the robot will never use the new values. The team has reported needing to recalibrate angle offsets repeatedly, which is a strong indicator that this is happening: the offsets aren't actually "changing," the correct values in Constants are simply being ignored in favor of stale Preferences data. This is the single most likely cause of *both* the drift and the single-module jitter. | 🔴 CRITICAL | Easy — clear stored Preferences and reboot | +| 2 | **CAN bus optimization is completely broken.** A library migration error caused the CAN frame-rate configuration to target analog sensor signals (which don't exist on these motors) instead of the integrated encoder signals. The optimization code runs without errors but does nothing. | 🔴 CRITICAL (code bug) | Easy — rename 2 method calls in `CANSparkUtil.java` | +| 3 | **Angle offsets need physical verification — after clearing Preferences.** The team has calibrated offsets multiple times using a metal alignment rod, but those values may never have taken effect because stored Preferences override Constants. After clearing Preferences (fix #1), verify one more time that the defaults in `Constants.java` are correct. | ⚠️ Verify | Medium — requires physical access to the robot | + +**Recommended teleop fixes (priority order):** + +1. Connect to the roboRIO and clear all `Swerve/Module*/AngleOffsetDegrees` Preferences entries. Reboot. Verify drift and jitter are resolved. +2. If issues persist, physically verify angle offsets: point all wheels straight forward, read CANcoder values, compare to `Constants.java` defaults. +3. Fix the `CANSparkUtil.java` CAN signal names (2 lines of code). +4. Verify `angleInvert = true` is correct for all four Mk4i modules. +5. Verify Pigeon gyro orientation matches expectations. + +**These teleop fixes are prerequisites for autonomous development.** Do not attempt to build auto routines until the robot drives straight and the jitter is resolved. + +--- + +### Part B: Autonomous Issues — Missing Infrastructure and Degraded Pose Accuracy + +**The critical discovery:** + +The 2024 team's top-tier autonomous routine (one of the best at that year's competition) was powered by a **three-layer navigation correction system**. This system continuously corrected the robot's position and path during autonomous. **Nearly all of this system is missing from the 2026 codebase.** + +A human driver can compensate for drift and jitter during teleop. In autonomous mode, the robot relies entirely on software to navigate — there is no human in the loop. The strategy team's planned auto routine requires three phases: (1) drive into the shooting band and shoot 8 preloaded fuel, (2) drive to the depot and collect more fuel, (3) drive back into the shooting band and shoot again. Each phase requires precise navigation and hub alignment. If the drivebase drifts and the software can't detect and correct for it, the robot will miss its targets — arriving at the wrong position in the shooting band, misaligning with the hub, or failing to reach the depot accurately. + +**What the 2024 autonomous system did (and what 2026 is missing):** + +| Navigation Layer | What It Did | 2026 Status | +|-----------------|-------------|-------------| +| **Vision-corrected positioning** | Every 20ms, fused wheel encoder data with AprilTag vision from *two* Limelight cameras. The robot always knew where it was, even if wheels slipped. Included safety checks: skipped updates on encoder errors (`isOdometryValid`), clamped position to field boundaries (`keepOdometryOnField`), required multi-tag sightings for trust. | ⚠️ **Partially present but degraded.** Vision fusion exists but safety checks were removed. The encoder error guard and field boundary clamp are gone. The vision trust model is more permissive (accepts single-tag sightings more readily). See §3.3. | +| **PathPlanner with active correction** | Configured PathPlanner with aggressive PID controllers (P=5.0) and on-the-fly replanning. When the robot deviated from its path, PathPlanner recalculated the trajectory from the robot's current position. | ❌ **Completely missing.** No PathPlanner configuration, no path-following PID constants, no auto builder setup. `getAutonomousCommand()` returns `"No autonomous command configured"`. | +| **Closed-loop motor control** | During auto, drive motors used PID + feedforward to hit exact commanded velocities (not just voltage percentages). This made motor response precise and repeatable. | ❌ **Missing.** The `closedLoopDrive()` method doesn't exist. | + +**Additional auto-specific concerns found in the comparison:** + +| Issue | Teleop Severity | Auto Severity | Why It's Worse for Auto | +|-------|----------------|---------------|------------------------| +| CAN signal bug in `CANSparkUtil` (§9.1) | 🔴 CRITICAL | 🔴 CRITICAL (amplified) | Auto uses closed-loop motor control, which requires accurate encoder data every 20ms. Stale data causes PID oscillation. | +| Removed `isOdometryValid()` check (§3.3) | MINOR | 🔴 CRITICAL | Bad encoder data corrupts the pose estimate that PathPlanner relies on for navigation. | +| Removed `keepOdometryOnField()` (§3.3) | MINOR | 🔴 CRITICAL | A runaway pose estimate could cause PathPlanner to generate paths that drive the robot into walls. | +| `maxSpeed` reduced to 3 m/s (§1.8) | MINOR | MODERATE | Caps PathPlanner at 60% of the 2024 speed. The planned auto (shoot preloads → depot → shoot again) requires multiple cross-field segments in 20 seconds — may not complete at this reduced speed. | +| MegaTag2 vision changes (§12) | Not relevant | MODERATE | Different vision trust model — may accept lower-quality pose estimates during fast maneuvers. Needs tuning. | +| 0.5 joystick multiplier (§5.1) | MODERATE | N/A | Only affects joystick input — PathPlanner bypasses it entirely. Not an auto concern. | + +**Recommended autonomous action plan:** + +*Phase 1 — Fix the drivebase first (see Part A above)* + +*Phase 2 — Restore pose estimation safety and calibration (1 session):* +1. Re-add `isOdometryValid()` to skip odometry updates when encoder data is bad. +2. Re-add `keepOdometryOnField()` to clamp the pose estimate to field boundaries. +3. Fix the CAN signal names in `CANSparkUtil.java`. +4. **Measure actual wheel diameter** — the 2024 team calibrated theirs (3.91" vs. the 4.00" factory spec, a 2.4% difference that causes ~12 cm of odometry drift per 5 m). Port the `MeasureWheelDiameter` command or manually measure, and update `wheelDiameter` in `Constants.java`. See §8.1. + +*Phase 3 — Port PathPlanner infrastructure from 2024 (1–2 sessions):* +4. Verify PathPlanner is in `vendordeps/` (if not, add the PathPlannerLib JSON). +5. Create `AutoConstants` class with path-following PID constants (start with 2024 values). +6. Add `closedLoopDrive()`, `getRobotRelativeSpeed()`, `followPathFromFile()`, and `startAutoAt()` to `SwerveSubsystem`. +7. Configure `AutoBuilder` in `SwerveSubsystem` constructor — **note:** the PathPlanner API changed between 2024 and 2026; `configureHolonomic()` was renamed. Use the 2026 PathPlanner docs. + +*Phase 4 — Build and test auto routines incrementally:* +8. Start with Phase 1 only: drive from starting position into the shooting band, align with hub, and shoot 8 preloaded fuel. This validates PathPlanner path following and hub alignment. +9. Add Phase 2: after shooting, drive to the depot and collect fuel. This validates multi-segment path following and depot navigation. +10. Add Phase 3: drive back into the shooting band, re-align, and shoot depot fuel. This validates the full planned auto routine. Use AdvantageScope to compare desired vs. actual path in real-time at each phase. +11. Increase `maxSpeed` from 3 to 4–5 m/s once path following is accurate at lower speeds. + +--- + +### Using AI to Accelerate These Fixes + +With competition approaching, the autonomous porting work (Phase 3–4 above) is significant — multiple methods across multiple files, with API migrations where 2024 method names no longer exist in 2026 libraries. Section §14 provides detailed recommendations for using AI tools responsibly to accelerate this work. + +The recommended approach is **not** "vibe coding" (asking AI to write code and deploying it unreviewed). Instead, the team should use AI as a **migration accelerator**: provide the working 2024 code as context, ask AI to produce the 2026 equivalent, then review the output side-by-side with the original before deploying. Key use cases include having AI explain unfamiliar 2024 code, translate specific methods to the 2026 API, and review ported code for the exact type of subtle API mapping errors found in this report (like the CANSparkUtil signal-name bug). See §14 for the full workflow, specific prompt examples, and a "what not to do" checklist. + +--- + +### What Was NOT a Problem + +The comparison also confirmed that many aspects of the 2024→2026 migration were done correctly: +- Motor PID values, feedforward constants, and idle modes were all migrated properly +- The REVLib imperative→declarative configuration migration was done correctly for all motor settings +- The custom `optimize()` method is mathematically equivalent to the 2024 version +- The Pigeon gyro inversion change is consistent with the API method change +- The swerve kinematics module ordering is internally consistent +- The drive and angle motor configurations match their 2024 equivalents (aside from intentional hardware changes) + +--- + +## Confirmation + +1. ✅ Both `2024-Season/` and `2026-Season/` directories are accessible. +2. ✅ All file paths match the actual structure. Verified: + - 2024: `src/main/java/frc/robot/subsystems/SwerveModule.java`, `SwerveSubsystem.java`; `frc/robot/commands/TeleopSwerve.java`; `frc/lib/OnboardModuleState.java`, `CANSparkUtil.java`, `SwerveModuleConstants.java` + - 2026: `src/main/java/frc/robot/SwerveModule.java`; `frc/robot/Subsystems/SwerveSubsystem.java`; `frc/robot/Command/TeleopSwerve.java`, `AutoAlign.java`; `frc/lib/CANSparkUtil.java` + +--- + +## Dependency File Scan + +### Imports traced from every listed file + +All `import frc.*` statements were scanned. The project-local dependency files are: + +| File | Imported by | +|------|------------| +| `frc.lib.CANSparkUtil` | SwerveModule (both years) | +| `frc.lib.OnboardModuleState` | SwerveModule 2024, TeleopSwerve 2024 | +| `frc.lib.SwerveModuleConstants` | SwerveModule 2024, Constants 2024 | +| `frc.lib.LimelightHelpers` | SwerveSubsystem (both years) | +| `frc.robot.Constants.SwerveConstants.ModuleData` | SwerveModule 2026, SwerveSubsystem 2026 | +| `frc.robot.Command.AutoAlign` | RobotContainer 2026 | + +No additional transitive `frc.*` imports were found beyond these. + +--- + +## 1. Constants.java + +**Files:** `2024-Season/src/main/java/frc/robot/Constants.java` vs `2026-Season/src/main/java/frc/robot/Constants.java` + +### 1.1 Numerical Constants — Side-by-Side + +| Constant | 2024 Value | 2026 Value | Changed? | +|----------|-----------|-----------|----------| +| inputDeadband | 0.1 | 0.1 | No | +| PIGEON_ID | 17 | 17 | No | +| **invertPigeon** | **true** | **false** | **YES** (see §1.2) | +| halfTrackWidth | `inchesToMeters(21.0/2.0)` ≈ 0.2667 m | `inchesToMeters(27/2.0)` ≈ 0.3429 m | YES — different robot frame | +| halfWheelBase | `inchesToMeters(21.0/2.0)` ≈ 0.2667 m | `inchesToMeters(27/2.0)` ≈ 0.3429 m | YES — different robot frame | +| wheelDiameter | 0.0992 m (direct) | `inchesToMeters(4.0)` ≈ 0.1016 m | YES — 2024 was calibrated, 2026 is nominal | +| **driveGearRatio** | 8.14 (Mk4 L1) | 6.75 (Mk4 L2) | YES — different modules | +| **angleGearRatio** | 12.8 (Mk4) | 21.4 (Mk4i) | YES — different modules | +| driveConversionPositionFactor | wheelCirc / 8.14 ≈ 0.0383 | wheelCirc / 6.75 ≈ 0.0473 | YES — follows from above | +| driveConversionVelocityFactor | above / 60 | above / 60 | YES — follows from above | +| angleConversionFactor | 360 / 12.8 = 28.125 | 360 / 21.4 ≈ 16.822 | YES — follows from above | +| **maxSpeed** | **5** m/s | **3** m/s | **YES** | +| maxAngularVelocity | 5/driveBaseRadius ≈ 13.3 rad/s | 3/driveBaseRadius ≈ 6.19 rad/s | YES — follows from above | +| voltageComp | 12.0 | 12.0 | No | +| angleContinuousCurrentLimit | 20 A | 20 A | No | +| **driveContinuousCurrentLimit** | **50 A** | **40 A** | **YES ⚠️** | +| driveKP | 0.1 | 0.1 | No | +| driveKI | 0.0 | 0.0 | No | +| driveKD | 0.0 | 0.0 | No | +| driveKFF | 0.0 | *(not defined)* | Removed — was 0.0, default is 0.0, no impact | +| driveKS | 0.667 | 0.667 | No | +| **driveKV** | **2.44** | **2.4** | **YES** (minor) | +| driveKA | 0.5 | 0.5 | No | +| angleKP | 0.01 | 0.01 | No | +| angleKI | 0.0 | 0.0 | No | +| angleKD | 0.0 | 0.0 | No | +| angleKFF | 0.0 | *(not defined)* | Removed — was 0.0, no impact | +| angleNeutralMode | kBrake | kBrake | No | +| driveNeutralMode | kBrake | kBrake | No | +| openLoopRamp | 0.25 | 0.25 | No (neither year actually applies it — see §2.5) | +| closedLoopRamp | 0.0 | 0.0 | No | +| **driveInvert** | false | false | No | +| **angleInvert** | **false** | **true** | **YES ⚠️** (expected for Mk4→Mk4i) | +| canCoderInvert | false | false | No | + +### 1.2 Pigeon Gyro Inversion — **invertPigeon changed from `true` to `false`** + +**File:** Constants.java, `SwerveConstants` class +**2024:** `invertPigeon = true` (line 168) +**2026:** `invertPigeon = false` (line 30) + +**Analysis:** This change is **correct and expected** given that the yaw reading method also changed: + +- **2024** `SwerveSubsystem.getYawAsDouble()` uses `pigeon.getAngle()` — this is the WPILib Gyro interface, which returns **clockwise-positive** (negated Phoenix6 yaw). With `invertPigeon = true`, the code computes `360 - pigeon.getAngle()`, which double-negates back to **counterclockwise-positive** (matching WPILib's coordinate system). +- **2026** `SwerveSubsystem.getYaw()` uses `pigeon.getYaw().getValueAsDouble()` — this is the Phoenix6 native API, which returns **counterclockwise-positive** directly. With `invertPigeon = false`, no inversion is applied. + +Both produce the **same effective sign convention** (CCW-positive). No bug here. + +**Severity:** Not a bug — the flag change is consistent with the API method change. + +**⚠️ However — verify the Pigeon is physically mounted with the same orientation as in 2024.** If the Pigeon is mounted upside-down or rotated compared to 2024, the yaw sign could be wrong, which would break field-oriented driving and cause the robot to steer in unexpected directions. + +### 1.3 angleInvert Changed: `false` → `true` + +**File:** Constants.java, `SwerveConstants` class +**2024:** `angleInvert = false` (line 243) +**2026:** `angleInvert = true` (line 104) + +**Impact:** This inversion is applied to the angle motors (steering motors). It changed because the module type changed from **SDS Mk4** (12.8:1 gear ratio, `angleGearRatio = 12.8`) to **SDS Mk4i** (21.4:1 gear ratio, `angleGearRatio = 21.4`). The Mk4i has a different internal gear train that requires the motor to spin in the opposite direction compared to the Mk4. + +**Severity:** Expected hardware change — **but verify this is correct for your specific Mk4i modules.** If even one module is a Mk4 (not Mk4i), or if the motor is wired differently, the inversion would be wrong for that module and could cause it to fight the PID controller (the jittering symptom). + +**Recommendation:** With the robot on blocks, command a known angle (e.g., 90°) and verify all four modules rotate in the correct direction and reach the target. + +### 1.4 SwerveDriveKinematics Module Ordering + +**File:** Constants.java, `swerveKinematics` + +**2024 (line 196–201):** +```java +new SwerveDriveKinematics( + new Translation2d(halfWheelBase, halfTrackWidth), // [0] Front Left + new Translation2d(-halfWheelBase, halfTrackWidth), // [1] Back Left + new Translation2d(-halfWheelBase, -halfTrackWidth), // [2] Back Right + new Translation2d(halfWheelBase, -halfTrackWidth) // [3] Front Right +); +``` +Module order: **FL, BL, BR, FR** → indices 0, 1, 2, 3 + +**2026 (line 50–56):** +```java +new SwerveDriveKinematics( + new Translation2d(halfTrackWidth, halfWheelBase), // [0] Front Left + new Translation2d(halfTrackWidth, -halfWheelBase), // [1] Front Right + new Translation2d(-halfTrackWidth, -halfWheelBase), // [2] Back Right + new Translation2d(-halfTrackWidth, halfWheelBase) // [3] Back Left +); +``` +Module order: **FL, FR, BR, BL** → indices 0, 1, 2, 3 + +**Analysis:** The module ordering changed. This is fine **as long as** the `moduleData` array (which assigns CAN IDs and offsets to each index) matches the new kinematics ordering. Verified: + +```java +// 2026 moduleData (line 117-122): +moduleData[0] = Front Left (CAN IDs 6,5,7) → kinematics[0] = Front Left ✓ +moduleData[1] = Front Right (CAN IDs 9,8,10) → kinematics[1] = Front Right ✓ +moduleData[2] = Back Right (CAN IDs 12,11,13)→ kinematics[2] = Back Right ✓ +moduleData[3] = Back Left (CAN IDs 15,14,16)→ kinematics[3] = Back Left ✓ +``` + +The ordering is internally consistent within 2026. **No bug here.** + +**⚠️ Note:** The 2026 code swaps the X and Y arguments in the Translation2d constructor (`halfTrackWidth, halfWheelBase` instead of `halfWheelBase, halfTrackWidth`). In WPILib, Translation2d is (X = forward, Y = left). The 2024 code correctly uses `halfWheelBase` (forward) as X and `halfTrackWidth` (left) as Y. The 2026 code swaps these. **Because the 2026 robot is square** (halfTrackWidth == halfWheelBase == 27/2 inches), this swap has no numerical effect. But it's a conceptual error that would cause problems if the robot were not square. + +**Severity:** MINOR (no effect on square robot, but indicates misunderstanding) + +### 1.5 Module-Specific Constants (Angle Offsets) + +**File:** Constants.java, module definitions + +| Module | 2024 CAN IDs (D/A/E) | 2024 Offset | 2026 CAN IDs (D/A/E) | 2026 Offset | 2026 Position | +|--------|----------------------|-------------|----------------------|-------------|---------------| +| Mod 0 | 3 / 2 / 11 | 160.2° (Rotation2d) | 6 / 5 / 7 | 31.46° (double) | Front Left | +| Mod 1 | 5 / 4 / 12 | 117.2° (Rotation2d) | 9 / 8 / 10 | 49.57° (double) | Front Right | +| Mod 2 | 7 / 6 / 13 | 141.0° (Rotation2d) | 12 / 11 / 13 | 33.13° (double) | Back Right | +| Mod 3 | 9 / 8 / 14 | −138.0° (Rotation2d) | 15 / 14 / 16 | 8.52° (double) | Back Left | + +Different robot, different CAN IDs and offsets — expected. The offset type changed from `Rotation2d` to `double` (degrees), which is handled by the new `ModuleData` record. + +**Key concern:** The 2026 code introduces a **WPILib Preferences override** for angle offsets (see §2.3). If stale or incorrect values exist in the roboRIO's Preferences storage, they will silently override the defaults above. **This is a likely cause of module-specific issues.** + +**Severity:** CRITICAL risk if Preferences data is stale (see §2.3) + +### 1.6 driveContinuousCurrentLimit: 50A → 40A + +**File:** Constants.java +**2024:** `driveContinuousCurrentLimit = 50` (line 209) +**2026:** `driveContinuousCurrentLimit = 40` (line 67) + +**Impact:** The drive motors in 2026 are limited to 40A instead of 50A. This reduces maximum torque by ~20%, which limits acceleration and peak speed under load. This would not cause the robot to drift, but it reduces overall drive performance. + +**Severity:** MODERATE — reduces performance but doesn't cause drift or jitter. + +### 1.7 driveKV: 2.44 → 2.4 + +**File:** Constants.java +**2024:** `driveKV = 2.44` (line 221) +**2026:** `driveKV = 2.4` (line 79) + +**Impact:** Negligible. The feedforward voltage-velocity constant changed by ~1.6%. This slightly affects closed-loop velocity control accuracy but would not cause drift. + +**Severity:** MINOR + +### 1.8 maxSpeed: 5 → 3 m/s + +**File:** Constants.java +**2024:** `maxSpeed = 5` (line 192) +**2026:** `maxSpeed = 3` (line 94) + +**Impact:** This affects: +1. Open-loop speed scaling in `setSpeed()`: `percentOutput = desiredSpeed / maxSpeed` +2. The low-speed angle-lock threshold in `setAngle()`: 0.05 m/s (2024) vs 0.03 m/s (2026) +3. Wheel speed desaturation in `driveFromChassisSpeeds()` + +Combined with the `* 0.5` multiplier in 2026's RobotContainer (see §5.1), the effective maximum teleop speed is 3 × 0.5 = **1.5 m/s** in 2026 vs **5 m/s** in 2024. This is likely intentional for safety during testing. + +**Severity (teleop):** MINOR — intentional, does not cause drift. + +**Severity (autonomous):** MODERATE — `maxSpeed` is passed to PathPlanner's `HolonomicPathFollowerConfig` as the maximum module speed. At 3 m/s, PathPlanner will constrain all path segments to this speed, making the robot significantly slower during auto. The 2024 robot ran at up to 5 m/s. The planned auto routine (shoot preloads → depot → shoot again) requires at least three cross-field path segments plus two alignment/shooting phases — all in 20 seconds. This 40% speed reduction could make it impossible to complete the full cycle. This should be increased once the drivebase is verified to be working correctly. + +--- + +## 2. SwerveModule.java + +**Files:** `2024-Season/src/main/java/frc/robot/subsystems/SwerveModule.java` vs `2026-Season/src/main/java/frc/robot/SwerveModule.java` + +### 2.1 REVLib Migration Motor Config Checklist — CRITICAL + +This is the most important comparison. Every 2024 motor setting must have an equivalent in 2026. + +#### Angle Motor Configuration + +| Setting | 2024 Code | 2026 Code | Status | +|---------|-----------|-----------|--------| +| Factory reset | `angleMotor.restoreFactoryDefaults()` | `ResetMode.kResetSafeParameters` | ✅ Equivalent | +| CAN bus optimization | `CANSparkUtil.setCANSparkBusUsage(angleMotor, kPositionOnly)` | `CANSparkUtil.setSparkBusUsage(sparkMaxConfig, kPositionOnly)` | ⚠️ **Wrong signals** — see §9.1 | +| Current limit | `angleMotor.setSmartCurrentLimit(20)` | `sparkMaxConfig.smartCurrentLimit(20)` | ✅ Equivalent | +| Inversion | `angleMotor.setInverted(false)` | `sparkMaxConfig.inverted(true)` | ✅ Value changed (Mk4→Mk4i) | +| Idle mode | `angleMotor.setIdleMode(kBrake)` | `sparkMaxConfig.idleMode(kBrake)` | ✅ Equivalent | +| Position conversion | `integratedAngleEncoder.setPositionConversionFactor(28.125)` | `sparkMaxConfig.encoder.positionConversionFactor(16.822)` | ✅ Values differ (gear ratio change) | +| PID P | `angleController.setP(0.01)` | `sparkMaxConfig.closedLoop.p(0.01)` | ✅ Equivalent | +| PID I | `angleController.setI(0.0)` | `sparkMaxConfig.closedLoop.i(0.0)` | ✅ Equivalent | +| PID D | `angleController.setD(0.0)` | `sparkMaxConfig.closedLoop.d(0.0)` | ✅ Equivalent | +| PID FF | `angleController.setFF(0.0)` | *(commented out)* | ✅ No impact — was 0.0, default is 0.0 | +| Voltage compensation | `angleMotor.enableVoltageCompensation(12.0)` | `sparkMaxConfig.voltageCompensation(12.0)` | ✅ Equivalent | +| Burn to flash | `angleMotor.burnFlash()` | `PersistMode.kPersistParameters` | ✅ Equivalent | +| Reset to absolute | Called after `Timer.delay(1.0)` | Called after `Timer.delay(1.0)` | ✅ Equivalent | + +**Result:** All angle motor settings were successfully migrated. No missing settings. + +#### Drive Motor Configuration + +| Setting | 2024 Code | 2026 Code | Status | +|---------|-----------|-----------|--------| +| Factory reset | `driveMotor.restoreFactoryDefaults()` | `ResetMode.kResetSafeParameters` | ✅ Equivalent | +| CAN bus optimization | `CANSparkUtil.setCANSparkBusUsage(driveMotor, kAll)` | `CANSparkUtil.setSparkBusUsage(sparkFlexConfig, kAll)` | ⚠️ **Wrong signals** — see §9.1 | +| Current limit | `driveMotor.setSmartCurrentLimit(50)` | `sparkFlexConfig.smartCurrentLimit(40)` | ⚠️ Value changed (50→40) | +| Inversion | `driveMotor.setInverted(false)` | `sparkFlexConfig.inverted(false)` | ✅ Equivalent | +| Idle mode | `driveMotor.setIdleMode(kBrake)` | `sparkFlexConfig.idleMode(kBrake)` | ✅ Equivalent | +| Velocity conversion | `driveEncoder.setVelocityConversionFactor(...)` | `sparkFlexConfig.encoder.velocityConversionFactor(...)` | ✅ Values differ (gear ratio change) | +| Position conversion | `driveEncoder.setPositionConversionFactor(...)` | `sparkFlexConfig.encoder.positionConversionFactor(...)` | ✅ Values differ (gear ratio change) | +| PID P | `driveController.setP(0.1)` | `sparkFlexConfig.closedLoop.p(0.1)` | ✅ Equivalent | +| PID I | `driveController.setI(0.0)` | `sparkFlexConfig.closedLoop.i(0.0)` | ✅ Equivalent | +| PID D | `driveController.setD(0.0)` | `sparkFlexConfig.closedLoop.d(0.0)` | ✅ Equivalent | +| PID FF | `driveController.setFF(0.0)` | *(not set)* | ✅ No impact — was 0.0, default is 0.0 | +| Voltage compensation | `driveMotor.enableVoltageCompensation(12.0)` | `sparkFlexConfig.voltageCompensation(12.0)` | ✅ Equivalent | +| Burn to flash | `driveMotor.burnFlash()` | `PersistMode.kPersistParameters` | ✅ Equivalent | +| Reset encoder | `driveEncoder.setPosition(0.0)` | `driveEncoder.setPosition(0.0)` | ✅ Equivalent | + +**Result:** All drive motor settings were successfully migrated. The only value change is the current limit (50→40A, covered in §1.6). + +#### MISSING from both years (never applied): +- `openLoopRamp` (0.25) — defined in Constants but never called via `setOpenLoopRampRate()` or equivalent config in either year +- `closedLoopRamp` (0.0) — defined in Constants but never applied in either year + +### 2.2 optimize() Method — Custom Implementation Replaced + +**2024 (line 107–108):** +```java +desiredState = OnboardModuleState.optimize(desiredState, getState().angle); +``` +Uses `OnboardModuleState.optimize()` which handles angle scoping via `placeInAppropriate0To360Scope()` and then performs the 90° flip optimization. + +**2026 (line 244–267):** +```java +private SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle){ + double difference = desiredState.angle.getDegrees() - currentAngle.getDegrees(); + double turnAmount = Math.IEEEremainder(difference, 360); + double speed = desiredState.speedMetersPerSecond; + if (turnAmount > 90){ turnAmount -= 180; speed *= -1; } + if (turnAmount < -90){ turnAmount += 180; speed *= -1; } + double direction = currentAngle.getDegrees() + turnAmount; + return new SwerveModuleState(speed, Rotation2d.fromDegrees(direction)); +} +``` + +**Analysis:** The 2026 `optimize()` is functionally equivalent to `OnboardModuleState.smolOptimize()` from 2024 (which used `Math.IEEEremainder` and the same 90° threshold logic). The 2024 codebase had TWO optimize implementations — `optimize()` (used) and `smolOptimize()` (not used). The 2026 version essentially implements `smolOptimize()`. + +Both correctly: +1. Normalize the angle difference to ±180° +2. Flip direction if |turn| > 90° +3. Produce the new target angle relative to the current angle (continuous, not wrapped to 0–360) + +**Severity:** Not a bug — functionally equivalent. + +> **📝 Note — Why Not Use WPILib's Built-in `SwerveModuleState.optimize()`?** +> +> WPILib provides its own [`optimize()` method](https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java) on `SwerveModuleState`. However, **it cannot be used as a drop-in replacement** for the custom implementation without additional changes. +> +> The reason: the angle SparkMax's integrated encoder reads in **continuous degrees** (e.g., 540° after 1.5 rotations), and `angleController.setReference()` expects a target in that same continuous domain. The custom `optimize()` handles this correctly — it computes the target as `currentAngle + turnAmount`, keeping it in the encoder's domain. WPILib's version outputs a `Rotation2d` normalized to [-180°, 180°], which would cause the PID to see huge errors (e.g., encoder at 540°, target at 210° → PID tries to spin 330° the wrong way). +> +> The 2024 codebase had a comment explaining this: *"custom optimize function because built-in doesn't work for some reason"* (`OnboardModuleState.java`, line 16). +> +> To use WPILib's version, you would also need to **enable PID position wrapping** on the angle motor: +> ```java +> sparkMaxConfig.closedLoop.positionWrappingEnabled(true); +> sparkMaxConfig.closedLoop.positionWrappingMinInput(0); +> sparkMaxConfig.closedLoop.positionWrappingMaxInput(360); +> ``` +> This tells the SparkMax PID that 0° and 360° are equivalent, so it always computes the shortest path regardless of encoder domain. +> +> WPILib also offers a bonus `cosineScale(currentAngle)` method that scales drive speed by `cos(angleError)` during turns, reducing sideways drift while the module rotates. Neither 2024 nor 2026 uses this. +> +> **Recommendation:** Keep the custom `optimize()` for now — it works correctly and changing it introduces risk during competition season. Switching to WPILib's `optimize()` + PID wrapping + `cosineScale()` would be a good **offseason cleanup** to reduce custom code and gain smoother driving. + +### 2.3 Angle Offset Preferences System — NEW in 2026 — CRITICAL + +**2024 (line 64):** +```java +angleOffset = moduleConstants.angleOffset; +``` +The offset comes directly from the `Rotation2d` constant defined in `Constants.java`. + +**2026 (line 78, 84–86):** +```java +this.angleOffsetPreferenceKey = "Swerve/Module" + moduleNumber + "/AngleOffsetDegrees"; +double storedOffset = Preferences.getDouble(angleOffsetPreferenceKey, moduleConstants.angleOffset()); +angleOffset = Rotation2d.fromDegrees(normalizeDegrees(storedOffset)); +``` + +**What this does:** On startup, the 2026 code checks WPILib's Preferences store (persistent key-value storage on the roboRIO) for a saved angle offset. If a value exists, it **overrides the default from Constants.java**. If no value exists, the default is used. + +**Why this is CRITICAL:** +1. If a previous calibration or test session wrote incorrect values to Preferences, they will silently override the correct defaults. +2. The `saveModuleOffsets()` and `saveCanCoderZero()` methods (accessible via controller buttons in RobotContainer) can write to Preferences. +3. If one module has a bad stored offset, **only that module** would be affected — explaining why only one module jitters. +4. The `normalizeDegrees()` function forces offsets into the 0–360° range. If an offset was originally intended to be negative (like 2024's Mod3 at −138°), it gets normalized to the equivalent positive angle (222°), which is mathematically correct but could mask issues if the original value was wrong. + +**Real-world confirmation:** The team has reported needing to recalibrate angle offsets multiple times — aligning wheels with a metal rod, reading CANcoder values, and updating `Constants.java`. **This should never be necessary more than once** (the CANcoder is an absolute encoder; its reading for a given physical position is fixed). The fact that the offsets appear to "change" is strong evidence that the Preferences system is the problem: + +- The team enters correct values into `Constants.java` and deploys +- On boot, `Preferences.getDouble()` finds a previously stored value and returns *that* instead of the new Constants default +- The robot uses the old/wrong offset, ignoring the updated Constants +- The team sees the alignment is still wrong, assumes the offset "changed," and recalibrates again +- This cycle repeats indefinitely because the stale Preference is never cleared + +**Additionally:** The "Save Offsets" button (Start/Menu on the controller) writes offsets to Preferences. If this was ever pressed accidentally during driving or with wheels not perfectly aligned, the bad value would persist across every subsequent reboot and code deploy, silently overriding any Constants.java corrections. + +**Severity:** 🔴 **CRITICAL** — Most likely cause of both the drift and single-module jittering issues. + +**Recommendation:** +1. **Check Preferences on the roboRIO.** Connect to the robot, open SmartDashboard or Shuffleboard, and check for keys matching `Swerve/Module*/AngleOffsetDegrees`. If any exist, compare them to the defaults in Constants.java. +2. **Delete all stored Preferences** related to swerve offsets and reboot. This forces the code to use the defaults from Constants. +3. **Verify defaults are correct.** With Preferences cleared, point all wheels straight forward, and verify the CANcoder readings minus the default offsets result in approximately 0° for all modules. +4. **Consider removing the Preferences override entirely.** If the team prefers to manage offsets in `Constants.java` (which is simpler and more transparent), change the constructor to always use `moduleConstants.angleOffset()` directly — matching the 2024 behavior. The Preferences system adds complexity without clear benefit unless the team has a deliberate workflow for saving offsets to the roboRIO. + +### 2.4 setAngle() Low-Speed Threshold + +**2024 (line 135):** +```java +Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.SwerveConstants.maxSpeed * 0.01)) + ? lastAngle : desiredState.angle; +``` +Threshold: 5 × 0.01 = **0.05 m/s** + +**2026 (line 318):** +```java +Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.SwerveConstants.maxSpeed * 0.01)) + ? lastAngle : desiredState.angle; +``` +Threshold: 3 × 0.01 = **0.03 m/s** + +**Analysis:** Identical logic. The threshold is slightly lower in 2026 (0.03 vs 0.05 m/s) due to the different maxSpeed. Both are well below any meaningful speed and effectively prevent angle changes when the robot is at rest. The threshold is still present and working correctly — it is **not** the cause of jittering. + +**Severity:** MINOR — negligible functional difference. + +### 2.5 openLoopRamp and closedLoopRamp — Never Applied + +**2024 Constants (line 237–238):** +```java +public static final double openLoopRamp = 0.25; +public static final double closedLoopRamp = 0.0; +``` +**2024 SwerveModule:** Neither `setOpenLoopRampRate()` nor `setClosedLoopRampRate()` is called. + +**2026 Constants (line 42–43):** +```java +public static final double openLoopRamp = 0.25; +public static final double closedLoopRamp = 0.0; +``` +**2026 SwerveModule:** No ramp rate configured in the SparkFlexConfig. + +**Analysis:** These constants exist in both years but are **never applied to the motor controllers**. The drive motors have no ramp rate (instant response). The SlewRateLimiter in TeleopSwerve provides acceleration smoothing instead. + +**Severity:** MINOR — identical behavior in both years (no ramp applied). + +### 2.6 setSpeed() — Closed Loop Slot Parameter + +**2024 (line 122–127):** +```java +driveController.setReference( + desiredState.speedMetersPerSecond, + ControlType.kVelocity, + 0, // PID slot 0 + feedforward.calculate(desiredState.speedMetersPerSecond)); +``` + +**2026 (line 295–299):** +```java +driveController.setReference( + desiredState.speedMetersPerSecond, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + feedforward.calculate(desiredState.speedMetersPerSecond)); +``` + +**Analysis:** Pure API migration — `0` → `ClosedLoopSlot.kSlot0`. Functionally identical. + +**Severity:** Not a bug. + +### 2.7 getCanCoder() — API Migration + +**2024 (line 157):** +```java +return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue()); +``` + +**2026 (line 200):** +```java +return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue().in(Units.Rotations)); +``` + +**Analysis:** In CTRE Phoenix6 2024, `getValue()` returned a `double` (rotations). In Phoenix6 2026, `getValue()` returns a `Measure`, requiring `.in(Units.Rotations)` to extract the double. Functionally identical. + +**Severity:** Not a bug. + +### 2.8 resetToAbsolute() — Functionally Identical + +**2024 (line 150–153):** +```java +private void resetToAbsolute() { + double absolutePosition = getCanCoder().getDegrees() - angleOffset.getDegrees(); + integratedAngleEncoder.setPosition(absolutePosition); +} +``` + +**2026 (line 404–407):** +```java +private void resetToAbsolute() { + double absolutePosition = getCanCoder().getDegrees() - angleOffset.getDegrees(); + integratedAngleEncoder.setPosition(absolutePosition); +} +``` + +**Analysis:** Identical logic. The only difference is how `angleOffset` is initialized (see §2.3 — Preferences system). + +**Severity:** Logic is fine; risk comes from incorrect `angleOffset` values (§2.3). + +--- + +## 3. SwerveSubsystem.java + +**Files:** `2024-Season/src/main/java/frc/robot/subsystems/SwerveSubsystem.java` vs `2026-Season/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java` + +### 3.1 getYaw() — Different API, Same Result + +**2024 (line 161–169):** +```java +public double getYawAsDouble(){ + double yaw = pigeon.getAngle(); + return (SwerveConstants.invertPigeon) ? 360 - yaw : yaw; +} +public Rotation2d getYaw() { + return Rotation2d.fromDegrees(getYawAsDouble()); +} +``` +Uses `pigeon.getAngle()` (WPILib Gyro interface, CW-positive, cumulative). +With `invertPigeon = true`: returns `360 - pigeon.getAngle()` = `360 + pigeon.getYaw()` (effectively CCW-positive). + +**2026 (line 179–184):** +```java +public Rotation2d getYaw() { + return (Constants.SwerveConstants.invertPigeon) + ? Rotation2d.fromDegrees(360 - pigeon.getYaw().getValueAsDouble()) + : Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()); +} +``` +Uses `pigeon.getYaw().getValueAsDouble()` (Phoenix6 native, CCW-positive, cumulative). +With `invertPigeon = false`: returns `pigeon.getYaw()` directly (CCW-positive). + +**Analysis:** Both produce CCW-positive yaw values, which is the correct convention for WPILib field coordinates. The combination of `invertPigeon` change + API method change produces identical results. See §1.2 for detailed analysis. + +**Severity:** Not a bug. + +### 3.2 drive() and driveFromChassisSpeeds() — Functionally Identical + +Both years implement the same flow: +1. Convert field-relative inputs to ChassisSpeeds +2. Convert ChassisSpeeds to SwerveModuleState[] +3. Desaturate wheel speeds +4. Set each module's desired state with `isOpenLoop = true` for teleop + +The 2026 version adds NetworkTables publishing of desired states for debugging — no functional impact. + +**Severity:** Not a bug. + +### 3.3 periodic() — Vision/Odometry Differences + +**2024:** +- Checks `isOdometryValid()` before updating odometry (skips update if any motor has errors) +- Uses MegaTag1 vision: `LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-a")` +- Complex fusion: trusts estimates when both cameras see 1 tag, or one camera sees ≥2 tags +- Calls `keepOdometryOnField()` to snap position back if it leaves field bounds + +**2026:** +- **Always** updates odometry (no validity check) +- Uses MegaTag2 vision: `LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-a")` +- Simpler rejection: rejects if gyro rotating > 720°/s or no tags visible +- Sets vision measurement standard deviations: `VecBuilder.fill(.7, .7, 99999)` (trusts translation, ignores rotation) +- Does NOT call `keepOdometryOnField()` + +**Impact on driving straight:** These differences affect pose estimation but do NOT affect teleop driving. The `drive()` method does not use the robot's estimated pose — it only uses the gyro yaw for field-oriented conversion. Vision/odometry issues would only affect auto-align commands and autonomous. + +**Severity (teleop):** MINOR — drift/jitter are not caused by vision differences. + +**Severity (autonomous):** 🔴 **CRITICAL** — These three removed safeguards directly degrade the pose accuracy that PathPlanner depends on: +1. **`isOdometryValid()` removed:** In 2024, odometry updates were skipped when any motor reported encoder errors, preventing corrupted data from poisoning the pose estimate. In 2026, bad encoder data is always accepted. During a fast autonomous routine, even a single cycle of garbage encoder data can shift the estimated position by centimeters — and PathPlanner will try to correct for a "deviation" that didn't actually happen, causing the robot to veer off course. +2. **`keepOdometryOnField()` removed:** In 2024, if the pose estimate drifted outside the field boundaries (impossible in reality), it was clamped back. This acted as a safety net that prevented runaway pose drift from causing PathPlanner to generate nonsensical paths. Without it, a bad vision measurement or encoder glitch during auto could push the estimated position off-field, and PathPlanner would attempt to drive the robot "back" onto the field — potentially into a wall or other robots. +3. **Different vision fusion logic:** The 2024 code required either both cameras to see a tag simultaneously, or one camera to see ≥2 tags, before trusting vision. The 2026 code trusts any single tag from any single camera (with only gyro-velocity and tag-count-zero rejection). This is more permissive and could accept lower-quality pose estimates during auto, especially when the robot is moving fast and only catches a brief glimpse of one tag. However, MegaTag2 is generally considered more robust than MegaTag1 for single-tag scenarios, so this may be acceptable if the standard deviations (`.7, .7, 99999`) are tuned correctly. + +### 3.4 Module Creation — Different Data Source + +**2024 (line 63–69):** +```java +new SwerveModule(0, SwerveConstants.Mod0.constants), +new SwerveModule(1, SwerveConstants.Mod1.constants), +new SwerveModule(2, SwerveConstants.Mod2.constants), +new SwerveModule(3, SwerveConstants.Mod3.constants) +``` + +**2026 (line 69–73):** +```java +mSwerveMods = new SwerveModule[4]; +for (int i = 0; i < 4; i++){ + ModuleData data = SwerveConstants.moduleData[i]; + mSwerveMods[i] = new SwerveModule(i, data); +} +``` + +**Analysis:** Functionally equivalent — just uses an array and loop instead of explicit construction. The module indices match the kinematics ordering in both years. + +**Severity:** Not a bug. + +--- + +## 4. TeleopSwerve.java + +**Files:** `2024-Season/src/main/java/frc/robot/commands/TeleopSwerve.java` vs `2026-Season/src/main/java/frc/robot/Command/TeleopSwerve.java` + +### 4.1 Simplified Command — Auto-Aim Modes Removed + +**2024:** TeleopSwerve includes 2024-game-specific auto-aim modes (speaker and amp scoring) using `OnboardModuleState.closestAngle()` and a `ProfiledPIDController`. Takes 7 constructor parameters. + +**2026:** TeleopSwerve is a simple manual swerve command with no auto-aim. Takes 6 constructor parameters (the `isAutoAlignSupplier` is accepted but never stored or used — see below). + +**Impact:** The auto-aim was 2024-game-specific and doesn't affect basic driving. Not a bug. + +### 4.2 Unused Constructor Parameter + +**2026 (line 32–46):** +```java +public TeleopSwerve(SwerveSubsystem SwerveSubsystem, + DoubleSupplier translationSupplier, + DoubleSupplier strafeSupplier, + DoubleSupplier rotationSupplier, + BooleanSupplier robotCentricSupplier, + BooleanSupplier isAutoAlignSupplier) { // ← accepted but never stored + ... + this.m_robotCentricSupplier = robotCentricSupplier; + // isAutoAlignSupplier is NOT stored as a field +} +``` + +The `isAutoAlignSupplier` parameter is declared but never assigned to a field or used. This is dead code but harmless — AutoAlign is a separate command. + +**Severity:** MINOR — dead code, no functional impact. + +### 4.3 Alliance Inversion Placement + +**2024:** Inversion is applied before slew rate limiting: +```java +double xVal = invert * xLimiter.calculate(MathUtil.applyDeadband(...)); +``` + +**2026:** Inversion is applied after slew rate limiting: +```java +double xVal = translationLimiter.calculate(MathUtil.applyDeadband(...)); +// ... later in drive() call: +xVal * SwerveConstants.maxSpeed * invert +``` + +**Impact:** Functionally equivalent for smooth driving. The slew rate limiter operates on the deadbanded value regardless of sign. The inversion (×-1) just flips the direction after limiting. No behavioral difference. + +**Severity:** MINOR. + +### 4.4 SlewRateLimiter Values + +Both years use `SlewRateLimiter(3.0)` for all three axes. + +**Severity:** No difference. + +--- + +## 5. RobotContainer.java + +**Files:** `2024-Season/src/main/java/frc/robot/RobotContainer.java` vs `2026-Season/src/main/java/frc/robot/RobotContainer.java` + +### 5.1 Joystick Input Scaling — 0.5 Multiplier Added — MODERATE + +**2024 (line 97–99):** +```java +() -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis), +() -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis), +() -> -driveController.getRawAxis(rotationAxis), +``` + +**2026 (line 116–120):** +```java +() -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.5, +() -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.5, +() -> -driveController.getRawAxis(rotationAxis) * 0.5, +``` + +**Impact:** All joystick inputs are halved in 2026. Combined with `maxSpeed = 3` (vs 5), the effective max teleop speed is **1.5 m/s** (2026) vs **5 m/s** (2024). This is likely a safety precaution during development/testing. It does NOT cause drift or jitter. + +Note: `Constants.motorSpeedMultiplier = 0.5` is defined in 2026 but is NOT used anywhere. The `* 0.5` is hardcoded directly in the lambda expressions. + +**Severity (teleop):** MODERATE — very slow driving, likely intentional, but **verify this is desired** for competition. If this multiplier is only for testing, remove it before competitions. + +**Severity (autonomous):** N/A — this multiplier is applied to joystick inputs only. PathPlanner drives the robot via `closedLoopDrive()` / `driveFromChassisSpeeds()`, which bypasses the joystick lambdas entirely. The 0.5 multiplier has zero effect on autonomous performance. + +### 5.2 Controller Bindings + +The following bindings are new in 2026 and absent from 2024: + +| Binding | Button | Action | +|---------|--------|--------| +| Resync encoders | Back (View) | `m_drive.resyncModuleEncoders()` | +| Save module offsets | Start (Menu) | `m_drive.saveModuleOffsets()` | +| Auto-align left | Left Trigger | `new AutoAlign(m_drive, true)` | +| Auto-align right | Right Trigger | `new AutoAlign(m_drive, false)` | + +**Impact:** The `saveModuleOffsets()` binding writes angle offsets to Preferences (§2.3). If accidentally pressed during testing with wheels not properly aligned, it would save incorrect offsets, causing module-specific drift/jitter issues. + +**Severity:** MODERATE — these utilities interact with the Preferences system that could cause issues. + +--- + +## 6. Robot.java + +**Files:** `2024-Season/src/main/java/frc/robot/Robot.java` vs `2026-Season/src/main/java/frc/robot/Robot.java` + +### 6.1 Initialization Pattern + +**2024:** Uses `robotInit()` override to create RobotContainer. +**2026:** Uses constructor `Robot()` to create RobotContainer. + +This is a WPILib 2026 version change. Functionally identical. + +### 6.2 Removed Periodic Callbacks + +**2024:** Has `disabledPeriodic()`, `autonomousPeriodic()`, `teleopPeriodic()` that call RobotContainer methods (for LEDs). +**2026:** These are empty stubs. + +**Impact:** No drive-related code was in these callbacks. Not a bug. + +**Severity:** MINOR. + +--- + +## 7. OnboardModuleState.java (2024 Only) + +**File:** `2024-Season/src/main/java/frc/lib/OnboardModuleState.java` — no 2026 equivalent + +### 7.1 Where Did This Functionality Go? + +The 2024 `OnboardModuleState` class provided: +1. `optimize()` — **(the version actually used in 2024)** — angle optimization with `placeInAppropriate0To360Scope()`, a verbose manual implementation that uses while-loops and boundary checks to place the target angle in a 360° window around the current angle +2. `smolOptimize()` — **(written but NOT used in 2024)** — a cleaner reimplementation using `Math.IEEEremainder()` that achieves the same result in far fewer lines +3. `closestAngle()` — find the closest equivalent angle (used by TeleopSwerve auto-aim) +4. `fixedMod()` — modulo that works correctly with negatives + +**In 2026:** +- The local `optimize()` method in `SwerveModule.java` (line 244–267) is based on `smolOptimize()`, not the 2024 `optimize()` that was actually used. However, **this is not a problem** — see analysis below. +- `closestAngle()` is not needed (TeleopSwerve no longer has auto-aim). +- `fixedMod()` is not needed. + +**The WPILib built-in `SwerveModuleState.optimize()` is NOT used.** The 2026 code uses its own custom implementation. (See §2.2 for a detailed explanation of why the WPILib version cannot be used as a drop-in replacement.) + +**Analysis — Are `optimize()` and `smolOptimize()` functionally equivalent?** + +Yes. The 2024 `placeInAppropriate0To360Scope()` is a verbose, manual implementation of what `Math.IEEEremainder()` does in a single call. Both compute "the angle closest to the current encoder reading that is rotationally equivalent to the target." Traced through the same inputs: + +| Example: current = 540°, target = 45° | `optimize()` (2024 used) | `smolOptimize()` / 2026 `optimize()` | +|---|---|---| +| **Scoping step** | `placeInAppropriate0To360Scope(540, 45)` → 405° | `IEEEremainder(45−540, 360)` = −135, closestAngle = 405° | +| **Delta** | 405 − 540 = −135° | −135° | +| **After 90° flip** | target = 585°, speed reversed | target = 585°, speed reversed | +| **Result** | **Identical** | **Identical** | + +After the scoping step, both methods apply the same 90° flip logic and output the target in the same continuous encoder domain (`current + adjusted_error`). The 2024 team likely wrote `smolOptimize` as a planned cleanup of their verbose `optimize` but never switched over. The 2026 students picked the cleaner version, which was the right call — no need to revert to the verbose 2024 `optimize()`. + +**Severity:** Not a bug — functionally equivalent. + +--- + +## 8. MeasureWheelDiameter.java and MoveToPose.java (2024 Only) + +**Teleop severity:** MINOR — neither command runs during teleop. +**Auto severity:** 🟡 MODERATE — both have direct impact on autonomous accuracy. + +### 8.1 MeasureWheelDiameter.java — Wheel Diameter Calibration Tool + +This diagnostic command spins the robot in place, compares encoder rotations against the gyro angle, and calculates the **actual** wheel diameter (accounting for wear). The result is displayed on SmartDashboard for the team to update in `Constants.java`. + +**Why this matters for auto:** The wheel diameter feeds directly into `driveConversionPositionFactor`, which converts encoder rotations to meters. This conversion is used by **all odometry** — every pose estimate depends on it. + +| Parameter | 2024 | 2026 | Difference | +|-----------|------|------|------------| +| `wheelDiameter` | `0.0992` m (≈3.91") — **measured** | `Units.inchesToMeters(4.0)` = `0.1016` m (4.00") — **nominal** | **2.4%** | + +The 2024 value (3.91") strongly suggests the team used this tool and found their wheels had worn down from the 4" nominal size. The 2026 code uses the factory nominal value. + +**Impact:** A 2.4% error compounds over distance. Over a 5-meter auto path, this produces **~12 cm of odometry drift** — enough to miss a shooting alignment or depot pickup. Over a full auto routine with multiple legs, the cumulative error could exceed 30 cm. + +**Recommendation:** Port `MeasureWheelDiameter.java` to 2026 and run it on the current robot. If the wheels are even slightly worn, update `wheelDiameter` in `Constants.java` with the measured value. This is a simple, high-value calibration step. + +### 8.2 MoveToPose.java — Closed-Loop Point-to-Point Navigation + +This command drives the robot to a target `Pose2d` using three `ProfiledPIDController`s (X, Y, rotation) with `AutoConstants` PID values and trapezoidal motion profiles. It is a **precision autonomous navigation primitive** — exactly the kind of command needed for the planned 2026 auto routine (drive into shooting band, drive to depot, drive back). + +Its absence in 2026, combined with the missing `AutoConstants` and `closedLoopDrive()` method (see §13), means the 2026 codebase has **no ready-made way to autonomously drive to a specific field position**. This would need to be rebuilt (or PathPlanner configured) before autonomous routines are possible. + +--- + +## 9. CANSparkUtil.java — CRITICAL BUG + +**Files:** `2024-Season/src/main/java/frc/lib/CANSparkUtil.java` vs `2026-Season/src/main/java/frc/lib/CANSparkUtil.java` + +### 9.1 Wrong Signal Names in 2026 — CAN Bus Frame Configuration Targets Analog Sensor Instead of Integrated Encoder + +This is the most significant code bug found in this comparison. + +**2024 (correct):** +```java +// Status0 = Applied output, faults +motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus0, period); +// Status1 = Motor velocity, bus voltage, temperature +motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus1, period); +// Status2 = Motor position +motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus2, period); +// Status3 = Analog sensor data +motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus3, period); +``` + +These correctly configure the **integrated encoder** data frame rates (Status1 for velocity, Status2 for position). + +**2026 (WRONG signal names):** +```java +// Applied output period (maps to old Status0) — CORRECT +config.signals.appliedOutputPeriodMs(period); +// Analog velocity (maps to old analog sensor, NOT integrated encoder) — WRONG +config.signals.analogVelocityPeriodMs(period); +// Analog position (maps to old analog sensor, NOT integrated encoder) — WRONG +config.signals.analogPositionPeriodMs(period); +// Analog voltage (maps to old Status3 analog sensor) — expected for analog +config.signals.analogVoltagePeriodMs(period); +``` + +**The problem:** In REVLib 2026, the signals API uses specific method names for each type of sensor data: + +| Signal Method | What It Controls | Corresponds to 2024 Frame | +|--------------|-----------------|--------------------------| +| `primaryEncoderPositionPeriodMs()` | **Integrated encoder position** | kStatus2 | +| `primaryEncoderVelocityPeriodMs()` | **Integrated encoder velocity** | kStatus1 (velocity part) | +| `analogPositionPeriodMs()` | External analog sensor position | kStatus3 (analog) | +| `analogVelocityPeriodMs()` | External analog sensor velocity | kStatus3 (analog) | +| `appliedOutputPeriodMs()` | Motor output/faults | kStatus0 | + +The 2026 code uses `analogVelocityPeriodMs` and `analogPositionPeriodMs` (external analog sensor) instead of `primaryEncoderVelocityPeriodMs` and `primaryEncoderPositionPeriodMs` (integrated encoder). Since these motors don't have external analog sensors, **these configuration calls have no effect**. + +**What this means:** +1. The CAN bus optimization is **completely non-functional** in 2026. All integrated encoder signals remain at their default rates (20ms for the integrated encoder, per REVLib defaults). +2. For the angle motor (configured as `kPositionOnly`), the intended optimization was to set velocity data to 1000ms and position data to 20ms. In 2026, both remain at 20ms — extra CAN bus traffic but functionally OK. +3. For the drive motor (configured as `kAll`), the intended configuration was position and velocity at 20ms. In 2026, they're at the defaults (also 20ms) — no effective difference. + +**Impact on driving:** The direct impact is **increased CAN bus traffic** (because the optimization isn't working). In most cases, this won't cause issues. However, if the CAN bus is overloaded (many other devices, frequent status frames), it could cause: +- Delayed motor controller responses +- Stale encoder data +- One module being affected more than others if CAN arbitration delays affect it disproportionately + +**Severity (teleop):** 🔴 **CRITICAL (code bug)** — the CAN bus optimization is completely non-functional. While this alone is unlikely to cause the driving-straight issue, it could contribute to jittering if CAN bus congestion is high. **Must be fixed regardless.** + +**Severity (autonomous):** 🔴 **CRITICAL (amplified)** — During autonomous, the drive motors run in closed-loop mode (PID + feedforward), which requires accurate, timely encoder velocity data every 20ms cycle. The broken CAN optimization means all encoder signals are at default rates rather than the optimized rates. If CAN bus utilization is high (multiple motors + sensors all at default rates), encoder data could arrive late or be stale, causing the closed-loop PID to overshoot or oscillate. This is worse in auto than teleop because teleop uses open-loop (voltage percentage) control where stale velocity data doesn't affect motor output. + +**Fix:** +```java +// Replace in 2026 CANSparkUtil.java: +config.signals.analogVelocityPeriodMs(...) → config.signals.primaryEncoderVelocityPeriodMs(...) +config.signals.analogPositionPeriodMs(...) → config.signals.primaryEncoderPositionPeriodMs(...) +config.signals.analogVoltagePeriodMs(...) → (remove, or keep for true analog sensors) +``` + +The corrected implementation should be: + +```java +if (usage == Usage.kAll) { + config.signals.primaryEncoderVelocityPeriodMs(20); + config.signals.primaryEncoderPositionPeriodMs(20); +} else if (usage == Usage.kPositionOnly) { + config.signals.primaryEncoderVelocityPeriodMs(1000); + config.signals.primaryEncoderPositionPeriodMs(20); +} else if (usage == Usage.kVelocityOnly) { + config.signals.primaryEncoderVelocityPeriodMs(20); + config.signals.primaryEncoderPositionPeriodMs(1000); +} else if (usage == Usage.kMinimal) { + config.signals.primaryEncoderVelocityPeriodMs(500); + config.signals.primaryEncoderPositionPeriodMs(500); +} +``` + +--- + +## 10. SwerveModuleConstants.java (2024) vs ModuleData record (2026) + +**Files:** `2024-Season/src/main/java/frc/lib/SwerveModuleConstants.java` vs `Constants.SwerveConstants.ModuleData` (inner record in 2026 Constants.java) + +### 10.1 Field Comparison + +| Field | 2024 SwerveModuleConstants | 2026 ModuleData | Type Change? | +|-------|---------------------------|-----------------|-------------| +| driveMotorID | `int driveMotorID` | `int driveMotorID` | No | +| angleMotorID | `int angleMotorID` | `int angleMotorID` | No | +| encoder ID | `int cancoderID` | `int encoderID` | Renamed only | +| angleOffset | `Rotation2d angleOffset` | `double angleOffset` | **YES — Rotation2d → double (degrees)** | +| location | *(not present)* | `Translation2d location` | **Added in 2026** | + +**Severity:** MINOR — structural refactoring only. The rename (`cancoderID` → `encoderID`) and the addition of `location` have no behavioral impact. The `angleOffset` type change is analyzed in §10.2 below. + +### 10.2 angleOffset Type Change + +In 2024, the offset is a `Rotation2d` object. In 2026, it's a raw `double` (degrees). This is handled correctly in `SwerveModule.java`: + +- 2024: `angleOffset = moduleConstants.angleOffset;` (already Rotation2d) +- 2026: `angleOffset = Rotation2d.fromDegrees(normalizeDegrees(storedOffset));` (converted from double) + +The `normalizeDegrees()` function wraps to 0–360°. Since `Rotation2d.fromDegrees()` handles any input angle, this is functionally equivalent but forces positive offsets. + +**Severity:** Not a bug — the type change is handled correctly at the point of use. + +### 10.3 Location Field Added + +The 2026 `ModuleData` includes a `Translation2d location` field, which stores the physical position of each module. This field is passed to each `ModuleData` but is **never actually used** inside `SwerveModule.java`. The kinematics still uses the positions from `swerveKinematics` (defined separately). This is dead data but harmless. + +**Severity:** MINOR. + +--- + +## 11. AutoAlign.java (2026 Only) + +**File:** `2026-Season/src/main/java/frc/robot/Command/AutoAlign.java` + +### 11.1 Could AutoAlign Interfere with Normal Teleop Driving? + +**Analysis:** +- Uses `addRequirements(m_SwerveSubsystem)` → properly requires the swerve subsystem +- Bound to trigger axes in RobotContainer: only runs **while trigger is held** +- When AutoAlign starts, it interrupts TeleopSwerve (default command). When it ends, TeleopSwerve resumes. +- `end()` method calls `driveFromChassisSpeeds(new ChassisSpeeds(), true)` — stops the robot cleanly +- `isFinished()` returns `false` — driver must release trigger to stop + +**Conclusion:** AutoAlign **cannot** interfere with normal teleop driving. It only activates when the trigger is explicitly held, and properly releases control when finished. + +### 11.2 API Differences from TeleopSwerve + +AutoAlign calls `driveFromChassisSpeeds(requestedSpeeds, false)` (closed-loop), while TeleopSwerve drives with open-loop via `drive()` → `driveFromChassisSpeeds(..., true)`. This is correct — autonomous/auto-align should use closed-loop for accuracy. + +**Severity:** Not a bug. + +--- + +## 12. LimelightHelpers Usage in SwerveSubsystem + +**2024 SwerveSubsystem calls:** +- `LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-a")` +- `LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-b")` +- Accesses `.pose`, `.avgTagDist`, `.tagCount`, `.timestampSeconds` on `PoseEstimate` + +**2026 SwerveSubsystem calls:** +- `LimelightHelpers.SetRobotOrientation(limelightName, yaw, 0, 0, 0, 0, 0)` +- `LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName)` +- Accesses `.pose`, `.tagCount`, `.timestampSeconds` on `PoseEstimate` + +**Analysis:** The 2026 code uses MegaTag2 (which requires setting robot orientation first) instead of MegaTag1. The `PoseEstimate` fields accessed are a subset of the 2024 usage (`.avgTagDist` no longer used). The LimelightHelpers v1.13 (2026) supports all these method signatures. + +**Impact:** Vision/odometry only — does not affect basic teleop driving, drift, or jitter. + +**Severity (teleop):** Not relevant to the reported issues. + +**Severity (autonomous):** MODERATE — The switch from MegaTag1 to MegaTag2 changes how vision measurements are generated and trusted. MegaTag2 uses the robot's gyro heading as a prior, which generally improves single-tag accuracy but means the vision estimate is only as good as the gyro data. The 2026 code also no longer checks `avgTagDist` (rejecting tags > 5m away), which means distant, less-accurate tag sightings could be fused into the pose during auto. The standard deviations `(.7, .7, 99999)` tell the estimator to heavily distrust vision rotation (good — the Pigeon is more accurate for heading) but to moderately trust vision translation. These values should be validated during auto testing and may need tuning based on observed accuracy. + +--- + +## 13. Autonomous Mode Assessment — 2024 Infrastructure vs 2026 Gaps + +### 13.1 Why This Matters + +The drift and jitter issues are frustrating in teleop, but a human driver can compensate. In autonomous mode, the robot relies entirely on odometry and path-following software to navigate. If the drivebase has a persistent directional error (drift) or a module that oscillates (jitter), every path segment accumulates error. The strategy team's planned auto routine — shoot 8 preloaded fuel, drive to the depot for more, return to the shooting band and shoot again — requires precise navigation to multiple field locations within 20 seconds. Even small per-segment errors compound into missed alignment with the hub, failed depot pickups, and wasted time. + +The 2024 team achieved a top-tier autonomous routine (one of the best at that year's competition). That was possible because of a **three-layer compensation system** that actively fought against drift and odometry error in real-time. The 2026 codebase is currently missing nearly all of this infrastructure. + +### 13.2 How 2024 Autonomous Navigation Worked + +The 2024 code did NOT blindly trust PathPlanner. It used three layers of compensation working together every 20ms cycle: + +#### Layer 1: Vision-Fused Pose Estimation (continuous during auto) + +The `periodic()` method in `SwerveSubsystem` runs every robot cycle, **including during autonomous**. It continuously fuses encoder-based odometry with AprilTag vision from **two Limelights**: + +```java +// 2024 SwerveSubsystem.periodic() — runs every 20ms, even during auto +odometry.update(getYaw(), getPositions()); // encoder + gyro update + +// Fuse vision from two cameras +LimelightHelpers.PoseEstimate estimateA = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-a"); +LimelightHelpers.PoseEstimate estimateB = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-b"); + +// If both cameras see one tag each, trust the combined estimate +if(estimateA.tagCount == 1 && estimateB.tagCount == 1 && AisValid && BisValid){ + odometry.addVisionMeasurement(estimateA.pose, estimateA.timestampSeconds); + odometry.addVisionMeasurement(estimateB.pose, estimateB.timestampSeconds); +} +// If either camera sees 2+ tags, trust that camera alone +else if(AisValid && estimateA.tagCount >= 2){ + odometry.addVisionMeasurement(estimateA.pose, estimateA.timestampSeconds); +} + +// Sanity check: snap position back if it drifts outside field bounds +keepOdometryOnField(); +``` + +This means every time PathPlanner asked "where am I?" via `getPose()`, it got a **vision-corrected answer** — not just raw wheel odometry. + +#### Layer 2: PathPlanner Configuration with Active Correction + +PathPlanner was configured with translation and rotation PID controllers, plus replanning: + +```java +// 2024 Constants.AutoConstants +public static final HolonomicPathFollowerConfig pathConfig = new HolonomicPathFollowerConfig( + new PIDConstants(5.0, 0.00001, 0.0), // Translation PID (P=5.0, I=0.00001) + new PIDConstants(5.0, 0.0005, 0.001), // Rotation PID (P=5.0, I=0.0005, D=0.001) + SwerveConstants.maxSpeed, // 5 m/s max module speed + SwerveConstants.driveBaseRadius, // drive base geometry + new ReplanningConfig() // enables on-the-fly replanning when robot deviates +); +``` + +```java +// 2024 SwerveSubsystem.configPathPlanner() +AutoBuilder.configureHolonomic( + this::getPose, // ← pose supplier (returns vision-fused estimate) + this::resetOdometry, // ← pose reset + this::getRobotRelativeSpeed, // ← current chassis speeds + this::closedLoopDrive, // ← drive consumer (closed-loop, not open-loop) + AutoConstants.pathConfig, // ← PID + replanning config + () -> (DriverStation.getAlliance().get() == Alliance.Red), + this +); +``` + +Key design decisions: +- **`this::getPose`** feeds the vision-corrected pose to PathPlanner, so its error calculations are based on the best available position estimate +- **`ReplanningConfig()`** (default) enables on-the-fly replanning — if the robot deviates from the planned path, PathPlanner regenerates the trajectory from the robot's current position rather than trying to drive back to the old trajectory +- **P=5.0 for both translation and rotation** — these are aggressive gains that correct for errors quickly +- **Non-zero I terms** — ensure even small persistent errors (like the drift you're experiencing) are eventually eliminated + +#### Layer 3: Closed-Loop Velocity Control During Auto + +PathPlanner drove the robot through `closedLoopDrive()`, which used PID + feedforward motor control: + +```java +// 2024 SwerveSubsystem +public void closedLoopDrive(ChassisSpeeds speeds){ + driveFromChassisSpeeds(speeds, false); // false = closed-loop +} +``` + +In closed-loop mode, each drive motor uses its onboard PID controller + feedforward to hit the exact commanded velocity. In teleop, the robot uses open-loop (voltage percentage) for driver feel. The closed-loop mode during auto ensures each wheel actually spins at the speed PathPlanner requests, not just "approximately" that speed. + +#### The Complete Feedback Loop + +Every 20ms during autonomous: + +``` +1. periodic() updates pose from wheel encoders + Pigeon gyro +2. periodic() fuses in AprilTag vision corrections from both Limelights +3. periodic() clamps pose to field boundaries (keepOdometryOnField) +4. PathPlanner calls getPose() → gets vision-corrected position +5. PathPlanner compares actual vs. desired position on the path +6. PathPlanner's PID controllers compute translational + rotational corrections +7. If deviation exceeds threshold, ReplanningConfig triggers a full path replan +8. closedLoopDrive() sends corrected velocities to each module +9. Drive motor onboard PID ensures actual wheel speeds match commanded speeds +10. Repeat +``` + +This is why the 2024 auto was so effective — the robot was **never more than one or two cycles behind** on correcting for any drift or disturbance. + +### 13.3 What 2026 Has and What It's Missing + +| Component | 2024 Status | 2026 Status | Severity | +|-----------|-------------|-------------|----------| +| `SwerveDrivePoseEstimator` (odometry + vision fusion) | ✅ Present | ✅ Present | — | +| Dual Limelight vision integration in `periodic()` | ✅ MegaTag1, dual cameras | ✅ MegaTag2, dual cameras | OK | +| `configPathPlanner()` / `AutoBuilder` setup | ✅ Called in constructor | ❌ **Missing entirely** | 🔴 CRITICAL | +| `AutoConstants` class (path PID, replanning config) | ✅ P=5.0 translation, P=5.0 rotation | ❌ **Doesn't exist** | 🔴 CRITICAL | +| `closedLoopDrive(ChassisSpeeds)` method | ✅ Present, used by PathPlanner | ❌ **Missing** | 🔴 CRITICAL | +| `getRobotRelativeSpeed()` method | ✅ Present | ❌ **Missing** | 🔴 CRITICAL | +| `followPathFromFile(String)` convenience method | ✅ Present | ❌ **Missing** | 🔴 CRITICAL | +| `startAutoAt(x, y, heading)` method | ✅ Sets starting pose + gyro | ❌ **Missing** | 🔴 CRITICAL | +| `backupCommand()` (dynamic path generation) | ✅ Present | ❌ **Missing** | MODERATE | +| Auto mode selector (`AutoModeSelector` class) | ✅ 11 routines (various strategies) | ❌ Returns `"No autonomous command configured"` | 🔴 CRITICAL | +| `keepOdometryOnField()` sanity check | ✅ Present | ❌ **Removed** | MODERATE | +| `isOdometryValid()` encoder error check | ✅ Present (skips update on error) | ❌ **Removed** | MODERATE | +| PathPlanner vendordep | ✅ Present | ⚠️ **Verify** — not imported in any Java file | ⚠️ Verify | + +### 13.4 Impact of Current Drivebase Bugs on Autonomous + +Even with the full PathPlanner infrastructure ported, the currently identified drivebase issues would severely degrade autonomous performance: + +| Issue | Impact on Auto | Why Compensation Can't Fully Fix It | +|-------|---------------|-------------------------------------| +| **Bad angle offsets** (§2.3) | Robot physically drives at an angle to the intended direction | Vision corrects the pose *estimate*, but the correction command is also misdirected because the modules are pointing wrong. PathPlanner would see the error and try to correct, but each correction is itself slightly off. This creates jerky, oscillatory path following. | +| **Module jitter** (§2.3, §1.5) | Jittering module introduces vibration and inconsistent wheel contact | PathPlanner expects smooth module responses. A jittering module provides inconsistent wheel speed data, corrupting the odometry that feeds the pose estimator. Vision can partially compensate, but the robot will follow a wobbly path instead of a smooth one. | +| **CAN signal bug** (§9.1) | Possible stale encoder data during fast maneuvers | PathPlanner commands velocity changes every 20ms. If encoder data is delayed due to CAN congestion, the drive motor PID in closed-loop mode reacts to stale data, causing velocity oscillation. | + +**For the planned autonomous routine:** The timing margin is tight — shoot 8 preloads, drive to depot, collect fuel, drive back, and shoot again, all in 20 seconds. Even if PathPlanner successfully compensates for drift, the compensation itself costs time — every correction is distance the robot travels that isn't along the optimal path. A clean drivebase that drives straight without correction saves ~0.2–0.5 seconds per path segment. Over the five segments of the planned routine (start → band, align + shoot, band → depot, depot → band, align + shoot), those savings add up to 1–2.5 seconds — often the difference between completing the depot cycle or running out of time after the first volley. + +### 13.5 Recommended Path to a Competitive Autonomous + +**Phase 1 — Fix the drivebase (prerequisite, do first):** +1. Clear WPILib Preferences and verify angle offsets (§2.3, §1.5) +2. Fix CANSparkUtil signal names (§9.1) +3. Verify angleInvert and Pigeon orientation (§1.3, §1.2) +4. Test: robot should drive straight with no drift and no jittering at rest + +**Phase 2 — Port PathPlanner infrastructure from 2024:** +1. Verify PathPlanner is in `vendordeps/` (if not, add the PathPlannerLib JSON) +2. Create `AutoConstants` class in 2026 `Constants.java` with path-following PID constants — start with the 2024 values but adjust `maxSpeed` to 3 m/s (or increase `maxSpeed` once the drivebase is verified) +3. Add `closedLoopDrive()` and `getRobotRelativeSpeed()` to `SwerveSubsystem` +4. Add `configPathPlanner()` to `SwerveSubsystem` — **note: the PathPlanner API changed between 2024 and 2026**; `configureHolonomic()` was renamed and the configuration object changed. Use the 2026 PathPlanner docs for the exact API. +5. Add `followPathFromFile()` and `startAutoAt()` convenience methods +6. Re-add `keepOdometryOnField()` to `periodic()` as a safety net +7. Consider re-adding `isOdometryValid()` to skip odometry updates during encoder errors + +**Phase 3 — Build and test the planned auto routine incrementally:** +1. Phase 1 only: drive from start into shooting band, align with hub, shoot 8 preloaded fuel. Validates path following and hub alignment. +2. Add Phase 2: after shooting, drive to depot and collect fuel. Validates multi-segment navigation. +3. Add Phase 3: drive back to shooting band, re-align, shoot depot fuel. Completes the full planned routine. +4. Use AdvantageScope or Shuffleboard to compare desired vs. actual path in real-time +5. Tune PathPlanner PID constants if the robot overshoots or oscillates during path following + +**Phase 4 — Optimize for competition:** +1. Increase `maxSpeed` (currently capped at 3 m/s, 2024 used 5 m/s) +2. Remove the `* 0.5` speed multiplier in `RobotContainer` +3. Tune path constraints (max velocity, max acceleration) for each path segment +4. Consider adding path-specific vision rejection (e.g., ignore vision updates during fast maneuvers to avoid pose jumps) + +--- + +## Final Summary + +### Part A: Teleop Issues + +#### Prioritized Causes: Straight-Line Driving Issue + +| Rank | Issue | Severity | Section | Explanation | +|------|-------|----------|---------|-------------| +| **1** | **Stale Preferences overriding angle offsets** | 🔴 CRITICAL | §2.3 | If one or more modules have incorrect stored offsets from a previous calibration, they would point in subtly wrong directions. Even 2–3° of error would cause the robot to pull to one side when driving straight. | +| **2** | **CANSparkUtil configuring wrong signal names** | 🔴 CRITICAL (code bug) | §9.1 | CAN bus optimization is non-functional. Increased CAN traffic could cause delayed encoder updates for some modules, leading to inconsistent module behavior. | +| **3** | **angleInvert changed to `true`** | ⚠️ Verify | §1.3 | If the 2026 robot uses Mk4i modules, `true` is correct. If any module is a Mk4, or if the physical setup differs, this would cause that module's steering to be backwards. | +| **4** | **Default angle offsets in Constants.java may be incorrect** | ⚠️ Verify | §1.5 | The default offsets (31.46°, 49.57°, 33.13°, 8.52°) must match the physical CANcoder positions when wheels are straight. These should be verified with physical testing. | +| **5** | **Pigeon gyro inversion / API change** | ⚠️ Verify | §1.2, §3.1 | The `invertPigeon` change is correct for the API change, but if the Pigeon is mounted differently than expected, field-oriented driving would be wrong. | +| **6** | **drive current limit reduced (50A → 40A)** | MODERATE | §1.6 | Asymmetric current limiting across modules is unlikely, but if one motor hits the limit while others don't, it could cause drift under heavy load. | +| **7** | **0.5 speed multiplier in RobotContainer** | MODERATE | §5.1 | Makes the robot very slow; doesn't cause drift but could mask other issues at low speed that become apparent at higher speeds. | + +#### Prioritized Causes: Single-Module Jittering Issue + +| Rank | Issue | Severity | Section | Explanation | +|------|-------|----------|---------|-------------| +| **1** | **Stale Preferences overriding angle offset for that module** | 🔴 CRITICAL | §2.3 | **Most likely cause.** If the `saveModuleOffsets()` button was pressed while wheels weren't perfectly straight, one module could have an incorrect stored offset. The PID would constantly fight to correct to the wrong position, causing oscillation/jitter. | +| **2** | **Incorrect default angle offset for that specific module** | 🔴 CRITICAL | §1.5 | Even without Preferences, if the default offset in `moduleData` is wrong for one module, it would jitter as the PID hunts for the wrong zero position. | +| **3** | **CANSparkUtil wrong signal names** | ⚠️ POSSIBLE | §9.1 | If CAN bus congestion causes stale position data for one module's angle encoder, its PID loop could oscillate. This is more likely to affect one module if CAN arbitration timing varies per motor ID. | +| **4** | **angleInvert wrong for that specific module** | ⚠️ Verify | §1.3 | If one physical module has a different internal gear orientation (e.g., it's a Mk4 while the others are Mk4i), the inversion would be wrong for only that module, causing its PID to fight itself. | +| **5** | **Hardware issue** | — | — | Loose encoder connector, damaged CANcoder, or mechanical issue with that one module. This cannot be detected via code comparison but should be checked. | + +#### Why Only ONE Module Jitters — Most Likely Explanations + +1. **Stored Preferences:** The `saveModuleOffsets()` / `saveCanCoderZero()` functions write offsets per-module. If a calibration was performed with one wheel not properly aligned, only that module's stored offset would be wrong. + +2. **Incorrect default offset:** The `moduleData` array has one entry per module. If one entry has a wrong `angleOffset` value (and Preferences don't override it), only that module is affected. + +3. **Hardware:** A loose CANcoder connector or intermittent CAN connection on one module would cause position data glitches that only affect that module. + +#### Recommended Teleop Debugging Steps + +1. **Check and clear WPILib Preferences:** + - Connect to the roboRIO + - Look for keys matching `Swerve/Module*/AngleOffsetDegrees` + - Delete all swerve-related Preferences + - Reboot the robot + +2. **Verify angle offsets:** + - With cleared Preferences and wheels pointed straight forward + - Read each module's CANcoder value from SmartDashboard + - Subtract the default offset from Constants + - The result should be approximately 0° for all modules + - If any module is significantly off, update its `angleOffset` in `moduleData` + +3. **Fix CANSparkUtil signal names:** + - Replace `analogVelocityPeriodMs` → `primaryEncoderVelocityPeriodMs` + - Replace `analogPositionPeriodMs` → `primaryEncoderPositionPeriodMs` + - Remove `analogVoltagePeriodMs` (or repurpose for actual analog sensors) + +4. **Verify angleInvert:** + - With robot on blocks, command each module to 90° + - Verify all four rotate the correct direction + - If one doesn't, check its physical module type (Mk4 vs Mk4i) + +5. **Remove the 0.5 speed multiplier** (when ready for faster driving): + - In RobotContainer, remove the `* 0.5` from the TeleopSwerve lambdas + - Test at higher speeds to see if drift is more apparent + +6. **Verify Pigeon mounting orientation:** + - Zero the gyro, then physically rotate the robot 90° counterclockwise + - The reported yaw should increase by ~90° + - If it decreases, the Pigeon inversion is wrong + +7. **Check CAN bus health:** + - Monitor CAN utilization in the Driver Station + - Look for CAN errors or timeouts + - If utilization is high, the CANSparkUtil fix (step 3) becomes more urgent + +--- + +### Part B: Autonomous Issues + +#### Prioritized Causes: Autonomous Navigation Readiness + +| Rank | Issue | Severity | Section | Explanation | +|------|-------|----------|---------|-------------| +| **1** | **PathPlanner not configured — no auto routines exist** | 🔴 CRITICAL | §13.3 | No `AutoBuilder` setup, no path-following PID constants, no `closedLoopDrive()` method. `getAutonomousCommand()` returns a print statement. The robot literally cannot run any autonomous routine. | +| **2** | **`isOdometryValid()` check removed** | 🔴 CRITICAL | §3.3 | In 2024, odometry updates were skipped when encoders reported errors. In 2026, bad encoder data is always accepted into the pose estimate. During fast auto maneuvers, a single corrupt reading can shift the position by centimeters, causing PathPlanner to "correct" for a deviation that never happened. | +| **3** | **`keepOdometryOnField()` removed** | 🔴 CRITICAL | §3.3 | In 2024, if the pose estimate drifted outside field boundaries (e.g., from a bad vision measurement), it was clamped back. Without this safety net, a runaway estimate could cause PathPlanner to generate paths that drive the robot into walls or across the field. | +| **4** | **CAN signal bug — amplified impact in auto** | 🔴 CRITICAL | §9.1 | Auto uses closed-loop motor control (PID + feedforward), which requires accurate encoder velocity data every 20ms. The broken CAN optimization means encoder signals may be stale, causing the drive motor PID to overshoot or oscillate. This is worse in auto than teleop, which uses open-loop (voltage) control. | +| **5** | **`maxSpeed` reduced to 3 m/s** | MODERATE | §1.8 | PathPlanner uses `maxSpeed` as the module speed cap. At 3 m/s (vs. 2024's 5 m/s), the robot is 40% slower during auto. Complex autonomous routines requiring field traversal may not complete within the 20-second autonomous period at this reduced speed. | +| **6** | **Vision fusion trust model changed (MegaTag1 → MegaTag2)** | MODERATE | §3.3, §12 | The 2026 code uses MegaTag2 with simpler rejection logic (no `avgTagDist` filter, no multi-camera triangulation requirement). This may accept lower-quality pose estimates during fast auto maneuvers. Standard deviations (`.7, .7, 99999`) need validation. | +| **7** | **Missing `closedLoopDrive()` and `getRobotRelativeSpeed()` methods** | 🔴 CRITICAL | §13.3 | These are required by PathPlanner's `AutoBuilder` to command the robot during auto. Without them, PathPlanner cannot be configured. | + +#### Why This Matters for Competition + +The 2024 team achieved one of the best autonomous routines at that year's competition. The 2026 strategy team's planned auto routine is similarly demanding — it requires the robot to: +- Drive into the shooting band and align with the hub +- Shoot 8 preloaded fuel +- Navigate to the depot and collect more fuel +- Drive back into the shooting band, re-align, and shoot again +- Complete all of this within the 20-second autonomous period + +This was possible because the 2024 code had a continuous feedback loop: vision-corrected pose → PathPlanner deviation detection → on-the-fly replanning → closed-loop motor execution → repeat every 20ms. The robot was never more than one or two cycles behind on correcting for any drift. + +Without this feedback loop, any autonomous routine will accumulate uncorrected errors. Even the teleop drivebase bugs (which a human driver can compensate for) become fatal in auto — a module that drifts 2° per second will be 40° off by the end of the 20-second autonomous period. + +#### Recommended Autonomous Development Steps + +*Prerequisites: Complete all teleop fixes (Part A) first. The robot must drive straight with no jitter before auto development begins.* + +1. **Restore pose estimation safety nets and calibration:** + - Re-add `isOdometryValid()` to skip odometry updates on encoder errors + - Re-add `keepOdometryOnField()` to clamp pose to field boundaries + - Fix CAN signal names in `CANSparkUtil.java` (shared with teleop fix) + - **Measure actual wheel diameter** and update `wheelDiameter` in `Constants.java` — the 2024 team found theirs was 3.91" (not the 4.00" factory spec), a 2.4% error that causes ~12 cm odometry drift per 5 m of travel (see §8.1) + +2. **Port PathPlanner infrastructure from 2024:** + - Verify PathPlanner is in `vendordeps/` + - Create `AutoConstants` with path-following PID constants (start with 2024 values: P=5.0 translation, P=5.0 rotation) + - Add `closedLoopDrive()` and `getRobotRelativeSpeed()` to `SwerveSubsystem` + - Add `configPathPlanner()` to `SwerveSubsystem` — use 2026 PathPlanner API (method names changed from 2024) + - Add `followPathFromFile()` and `startAutoAt()` convenience methods + +3. **Build auto routines incrementally:** + - Phase 1 only: drive into shooting band, align with hub, shoot 8 preloaded fuel → validates path following and hub alignment + - Add Phase 2: after shooting, drive to depot and collect fuel → validates multi-segment navigation + - Add Phase 3: drive back to shooting band, re-align, shoot depot fuel → completes the full planned routine + - Use AdvantageScope to compare desired vs. actual path in real-time + +4. **Optimize for competition speed:** + - Increase `maxSpeed` from 3 m/s toward 5 m/s once path following is accurate + - Tune path constraints per segment + - Validate vision standard deviations under match conditions + - See §13.5 for the full phased roadmap + +--- + +## 14. Recommendations for Using AI Tools + +*For team leads and mentors — practical guidance on leveraging AI to accelerate the fixes and porting work identified in this report, without sacrificing understanding.* + +### 14.1 The Problem: Time Pressure vs. Code Complexity + +The autonomous infrastructure that needs to be ported from 2024 (PathPlanner configuration, closed-loop drive, odometry safety checks) spans multiple files and involves API migrations where the 2024 method names no longer exist in the 2026 libraries. Doing this work entirely by hand — reading WPILib/REVLib/PathPlanner changelogs, finding the renamed methods, rewriting code — is doable but time-consuming. With competition approaching, the team may not have enough sessions to complete all of this manually. + +At the same time, blindly asking AI to "write me an autonomous system" and deploying the output without review ("vibe coding") is dangerous on a physical robot. Incorrect motor configurations can damage hardware, and untested path-following code can send the robot into walls. + +### 14.2 The Recommended Approach: AI as a Migration Accelerator + +The sweet spot for this team's situation is using AI as a **code migration assistant** — somewhere between "better Google" and "vibe coding." The key principle: **the team should always understand what the code does before deploying it, but AI can dramatically speed up the process of getting from a 2024 reference to a working 2026 draft.** + +Here's how this works in practice: + +#### Level 1: AI as Explainer (Low Risk, Immediate Value) + +Use AI to understand the 2024 code that needs to be ported. This is the "better Google" end of the spectrum, but far more effective than Google for understanding specific code. + +**Examples tied to this report:** +- *"Explain what the `configPathPlanner()` method in our 2024 `SwerveSubsystem.java` does. What is each parameter for?"* +- *"What does `isOdometryValid()` check for? Why would encoder data ever be invalid?"* +- *"What's the difference between open-loop and closed-loop driving in our `driveFromChassisSpeeds()` method?"* + +**Why this is safe:** No code is generated. The team builds understanding that makes them better at writing and reviewing code. This is the single highest-value use of AI for a student team. + +#### Level 2: AI as API Translator (Medium Risk, High Value) + +Give AI the 2024 code and ask it to produce the 2026 equivalent, **with the specific library versions identified.** This is where the biggest time savings are. + +**Examples tied to this report:** +- *"Here is our 2024 `CANSparkUtil.java` that uses `CANSparkBase.setPeriodicFramePeriod()`. Rewrite it for REVLib 2025.1 using `SparkBaseConfig.signals`. Make sure to use `primaryEncoderPositionPeriodMs` and `primaryEncoderVelocityPeriodMs` for the integrated encoder, NOT the analog sensor methods."* +- *"Here is our 2024 `configPathPlanner()` method that uses `AutoBuilder.configureHolonomic()`. The 2026 PathPlanner API renamed this method. Rewrite it for PathPlannerLib 2026.x."* +- *"Here is our 2024 `closedLoopDrive()` method. Port it to work with the 2026 `SwerveSubsystem` class. Here is the current 2026 `SwerveSubsystem.java` for context: [paste file]."* + +**Critical rule:** Always provide the 2024 code as context. AI is much more accurate when translating known-working code than when generating from scratch. The 2024 codebase is a massive advantage — use it. + +**Review checklist before deploying AI-translated code:** +1. Does every method call exist in the 2026 library? (Check imports — if the IDE shows red, something's wrong.) +2. Do the parameter types and order match? (Especially for PID constructors — P, I, D order varies between libraries.) +3. Are motor/sensor IDs correct? (AI doesn't know your CAN bus wiring.) +4. Does the logic match the 2024 version? (Read the AI output side-by-side with the 2024 original.) + +#### Level 3: AI as Code Reviewer (Medium Risk, High Value) + +After writing or porting code, ask AI to review it — especially for the types of migration bugs found in this report. + +**Examples tied to this report:** +- *"Here is our 2026 `CANSparkUtil.java`. Compare it to the 2024 version. Are we configuring the correct CAN signals for integrated encoders?"* +- *"Here is our 2026 `SwerveModule.java` constructor. Does every motor configuration setting from the 2024 imperative API have an equivalent in the 2026 declarative API? List any settings that are missing."* +- *"Review our 2026 `SwerveSubsystem.periodic()` method. Compare it to the 2024 version. Are there any safety checks or odometry guards that were removed?"* + +**Why this is valuable:** The CANSparkUtil signal-name bug (§9.1) — the single most insidious bug in this report — would likely have been caught by an AI code review comparing the 2024 and 2026 versions side-by-side. These are exactly the kinds of subtle API mapping errors that humans miss and AI catches. + +#### Level 4: AI for Debugging and Diagnostics (Medium Risk, Situational) + +When testing on the physical robot, use AI to help diagnose unexpected behavior. + +**Examples:** +- *"Our swerve module 2 jitters back and forth by about 3 degrees when the robot is stationary. The other three modules are fine. Here is the SwerveModule code and the Constants for module 2. What could cause only one module to behave differently?"* +- *"Our robot drifts to the left when we command it to drive straight forward. Here are our module angle offsets and kinematics configuration. What should we check?"* +- *"PathPlanner shows the robot deviating from the path by 30cm on turns. Here are our path-following PID constants and the AdvantageScope log. What should we tune?"* + +### 14.3 What NOT to Do + +| ❌ Don't | ✅ Do Instead | +|----------|--------------| +| "Write me a complete swerve drive subsystem" | "Port this specific method from 2024 to the 2026 API" | +| Accept AI code without reading it | Read AI output side-by-side with the 2024 original | +| Deploy AI-generated code directly to competition | Test every change on the physical robot before competition | +| Ask AI to pick PID constants | Use the 2024 values as a starting point and tune on the robot | +| Trust AI to know your CAN IDs or motor wiring | Always verify hardware-specific values against the physical robot | +| Use AI-generated code you can't explain to a teammate | If you can't explain it, you don't understand it — ask AI to explain it first | + +### 14.4 Suggested Workflow for Porting Auto Infrastructure + +For the specific task of porting the autonomous system from 2024 to 2026, here's a practical workflow that balances speed with understanding: + +1. **Understand first (Level 1):** Have each team member working on auto read the relevant 2024 code and ask AI to explain any parts they don't understand. Target: every team member can explain the three-layer correction system (vision-fused odometry → PathPlanner → closed-loop motors) in their own words. + +2. **Draft the port (Level 2):** Give AI the 2024 file and the current 2026 file. Ask it to produce the 2026 equivalent of each missing method, one method at a time. Don't ask for the entire file at once — work method by method so each piece can be reviewed and understood. + +3. **Aggressive code review (Level 3) — the critical step:** Don't just skim the AI output. Integrate it into the project, then conduct a **genuinely adversarial code review** where team members challenge each other to explain the code. This is the step that turns AI-generated code into *the team's* code. Specific practices: + + - **The "explain every line" rule:** The person integrating the code must be able to explain what each line does *and why it's there* to a teammate. If they can't explain it, they don't understand it — and they should ask AI to explain that specific part before proceeding. + - **Add comments that prove understanding:** After review, the team should add inline comments in their own words explaining the logic. Not redundant comments like `// set PID` — substantive comments like `// Skip this odometry update if any encoder reports a position jump > 1m in a single cycle, which indicates a sensor glitch rather than real movement`. If you can't write a meaningful comment, you don't understand the code well enough. + - **Cross-reference against 2024:** For every method ported, a reviewer should have the 2024 original open side-by-side and verify that the behavior is preserved. Use the §14.2 checklist: do all method calls exist? Are parameter types correct? Are hardware-specific values right? + - **The "CANSparkUtil test":** As a gut check, ask: *"Could the type of bug found in CANSparkUtil (§9.1) — where a method name looks plausible but targets the wrong hardware signal — be hiding in this code?"* This is the exact class of bug that slips past casual review but gets caught by deliberate scrutiny. + + > **Why this approach works better than "type it yourself":** Transcribing code by hand under time pressure introduces typos that waste hours to debug and teach nothing about robotics. In contrast, explaining code to a teammate and writing substantive comments tests understanding at a deeper level — you can type code on autopilot, but you can't explain code you don't understand. With competition approaching, the team's scarce practice time should be spent on understanding and testing, not on retyping. + +4. **Test incrementally:** Deploy to the robot after each method is added. Don't port everything and test at the end — one method at a time, verify it compiles, verify it doesn't break teleop, then move to the next. + +### 14.5 A Note on AI Limitations for FRC + +AI tools have specific weaknesses in the FRC context that the team should be aware of: + +- **Library version knowledge may be stale.** REVLib 2025/2026 and PathPlannerLib 2026 are relatively new. AI may suggest deprecated method names or pre-2026 APIs. Always verify that suggested method names actually exist by checking imports and IDE autocomplete. +- **AI doesn't know your hardware.** It can't know which CAN ID is wired to which motor, which direction your Pigeon is mounted, or whether your modules are Mk4 or Mk4i. Never trust AI for hardware-specific values. +- **AI is best when given context.** The more of your actual code you provide as context, the better the output. "Port this method" with the actual code attached is far more useful than "write me a PathPlanner config." +- **AI excels at exactly the type of bugs in this report.** API migration errors (wrong signal names, missing configuration, renamed methods) are pattern-matching tasks where AI is often better than humans. Use it for code review after every migration change. diff --git a/docs/2024-auto-mode.md b/docs/2024-auto-mode.md new file mode 100644 index 0000000..4f30604 --- /dev/null +++ b/docs/2024-auto-mode.md @@ -0,0 +1,179 @@ +# Autonomous Mode Assessment — 2024 Infrastructure vs 2026 Gaps + +## 1. Why This Matters + +The drift and jitter issues are frustrating in teleop, but a human driver can compensate. In autonomous mode, the robot relies entirely on odometry and path-following software to navigate. If the drivebase has a persistent directional error (drift) or a module that oscillates (jitter), every path segment accumulates error. The strategy team's planned auto routine — shoot 8 preloaded fuel, drive to the depot for more, return to the shooting band and shoot again — requires precise navigation to multiple field locations within 20 seconds. Even small per-segment errors compound into missed alignment with the hub, failed depot pickups, and wasted time. + +The 2024 team achieved a top-tier autonomous routine (one of the best at that year's competition). That was possible because of a **three-layer compensation system** that actively fought against drift and odometry error in real-time. The 2026 codebase is currently missing nearly all of this infrastructure. + +## 2. How 2024 Autonomous Navigation Worked + +The 2024 code did NOT blindly trust PathPlanner. It used three layers of compensation working together every 20ms cycle: + +### Layer 1: Vision-Fused Pose Estimation (continuous during auto) + +The `periodic()` method in `SwerveSubsystem` runs every robot cycle, **including during autonomous**. It continuously fuses encoder-based odometry with AprilTag vision from **two Limelights**: + +```java +// File: 2024-Season/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +// Method: periodic(), lines 332–374 (condensed — SmartDashboard calls omitted) + +332 public void periodic() { + // ... +336 odometry.update(getYaw(), getPositions()); // encoder + gyro update +338 LimelightHelpers.PoseEstimate estimateA = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-a"); +339 LimelightHelpers.PoseEstimate estimateB = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-b"); + // ... + // If both cameras see one tag each, trust the combined estimate +352 if(estimateA.tagCount == 1 && estimateB.tagCount == 1 && AisValid && BisValid){ +355 odometry.addVisionMeasurement(estimateA.pose, estimateA.timestampSeconds); +356 odometry.addVisionMeasurement(estimateB.pose, estimateB.timestampSeconds); +357 } +358 else{ + // If either camera sees 2+ tags, trust that camera alone +360 if(AisValid){ +361 if(estimateA.tagCount >= 2){ +362 odometry.addVisionMeasurement(estimateA.pose, estimateA.timestampSeconds); + } + } + // ... (similar check for limelight-b, lines 366–371) +372 } + // Sanity check: snap position back if it drifts outside field bounds +374 keepOdometryOnField(); +``` + +This means every time PathPlanner asked "where am I?" via `getPose()`, it got a **vision-corrected answer** — not just raw wheel odometry. + +### Layer 2: PathPlanner Configuration with Active Correction + +PathPlanner was configured with translation and rotation PID controllers, plus replanning: + +```java +// File: 2024-Season/src/main/java/frc/robot/Constants.java +// Class: AutoConstants, lines 135–164 + +137 public static final HolonomicPathFollowerConfig pathConfig = new HolonomicPathFollowerConfig( +138 new PIDConstants(5.0, 0.00001, 0.0), // Translation PID (P=5.0, I=0.00001) +139 new PIDConstants(5.0, 0.0005, 0.001), // Rotation PID (P=5.0, I=0.0005, D=0.001) +140 SwerveConstants.maxSpeed, // 5 m/s max module speed +141 SwerveConstants.driveBaseRadius, // Drive base radius in meters +142 new ReplanningConfig() // enables on-the-fly replanning when robot deviates +143 ); +``` + +```java +// File: 2024-Season/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +// Method: configPathPlanner(), lines 223–233 + +223 public void configPathPlanner(){ +224 AutoBuilder.configureHolonomic( +225 this::getPose, // ← pose supplier (returns vision-fused estimate) +226 this::resetOdometry, // ← pose reset +227 this::getRobotRelativeSpeed, // ← current chassis speeds +228 this::closedLoopDrive, // ← drive consumer (closed-loop, not open-loop) +229 AutoConstants.pathConfig, // ← PID + replanning config +230 () -> (DriverStation.getAlliance().get() == Alliance.Red), +231 this +232 ); +233 } +``` + +Key design decisions: +- **`this::getPose`** feeds the vision-corrected pose to PathPlanner, so its error calculations are based on the best available position estimate +- **`ReplanningConfig()`** (default) enables on-the-fly replanning — if the robot deviates from the planned path, PathPlanner regenerates the trajectory from the robot's current position rather than trying to drive back to the old trajectory +- **P=5.0 for both translation and rotation** — these are aggressive gains that correct for errors quickly +- **Non-zero I terms** — ensure even small persistent errors (like the drift you're experiencing) are eventually eliminated + +### Layer 3: Closed-Loop Velocity Control During Auto + +PathPlanner drove the robot through `closedLoopDrive()`, which used PID + feedforward motor control: + +```java +// File: 2024-Season/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +// Method: closedLoopDrive(), lines 96–98 + + 96 public void closedLoopDrive(ChassisSpeeds speeds){ + 97 driveFromChassisSpeeds(speeds, false); // false = closed-loop + 98 } +``` + +In closed-loop mode, each drive motor uses its onboard PID controller + feedforward to hit the exact commanded velocity. In teleop, the robot uses open-loop (voltage percentage) for driver feel. The closed-loop mode during auto ensures each wheel actually spins at the speed PathPlanner requests, not just "approximately" that speed. + +### The Complete Feedback Loop + +Every 20ms during autonomous: + +``` +1. periodic() updates pose from wheel encoders + Pigeon gyro +2. periodic() fuses in AprilTag vision corrections from both Limelights +3. periodic() clamps pose to field boundaries (keepOdometryOnField) +4. PathPlanner calls getPose() → gets vision-corrected position +5. PathPlanner compares actual vs. desired position on the path +6. PathPlanner's PID controllers compute translational + rotational corrections +7. If deviation exceeds threshold, ReplanningConfig triggers a full path replan +8. closedLoopDrive() sends corrected velocities to each module +9. Drive motor onboard PID ensures actual wheel speeds match commanded speeds +10. Repeat +``` + +This is why the 2024 auto was so effective — the robot was **never more than one or two cycles behind** on correcting for any drift or disturbance. + +## 3. What 2026 Has and What It's Missing + +| Component | 2024 Status | 2026 Status | Severity | +|-----------|-------------|-------------|----------| +| `SwerveDrivePoseEstimator` (odometry + vision fusion) | ✅ Present | ✅ Present | — | +| Dual Limelight vision integration in `periodic()` | ✅ MegaTag1, dual cameras | ✅ MegaTag2, dual cameras | OK | +| `configPathPlanner()` / `AutoBuilder` setup | ✅ Called in constructor | ❌ **Missing entirely** | 🔴 CRITICAL | +| `AutoConstants` class (path PID, replanning config) | ✅ P=5.0 translation, P=5.0 rotation | ❌ **Doesn't exist** | 🔴 CRITICAL | +| `closedLoopDrive(ChassisSpeeds)` method | ✅ Present, used by PathPlanner | ❌ **Missing** | 🔴 CRITICAL | +| `getRobotRelativeSpeed()` method | ✅ Present | ❌ **Missing** | 🔴 CRITICAL | +| `followPathFromFile(String)` convenience method | ✅ Present | ❌ **Missing** | 🔴 CRITICAL | +| `startAutoAt(x, y, heading)` method | ✅ Sets starting pose + gyro | ❌ **Missing** | 🔴 CRITICAL | +| `backupCommand()` (dynamic path generation) | ✅ Present | ❌ **Missing** | MODERATE | +| Auto mode selector (`AutoModeSelector` class) | ✅ 11 routines (various strategies) | ❌ Returns `"No autonomous command configured"` | 🔴 CRITICAL | +| `keepOdometryOnField()` sanity check | ✅ Present | ❌ **Removed** | MODERATE | +| `isOdometryValid()` encoder error check | ✅ Present (skips update on error) | ❌ **Removed** | MODERATE | +| PathPlanner vendordep | ✅ Present | ⚠️ **Verify** — not imported in any Java file | ⚠️ Verify | + +## 4. Impact of Current Drivebase Bugs on Autonomous + +Even with the full PathPlanner infrastructure ported, the currently identified drivebase issues would severely degrade autonomous performance: + +| Issue | Impact on Auto | Why Compensation Can't Fully Fix It | +|-------|---------------|-------------------------------------| +| **Bad angle offsets** (§2.3) | Robot physically drives at an angle to the intended direction | Vision corrects the pose *estimate*, but the correction command is also misdirected because the modules are pointing wrong. PathPlanner would see the error and try to correct, but each correction is itself slightly off. This creates jerky, oscillatory path following. | +| **Module jitter** (§2.3, §1.5) | Jittering module introduces vibration and inconsistent wheel contact | PathPlanner expects smooth module responses. A jittering module provides inconsistent wheel speed data, corrupting the odometry that feeds the pose estimator. Vision can partially compensate, but the robot will follow a wobbly path instead of a smooth one. | +| **CAN signal bug** (§9.1) | Possible stale encoder data during fast maneuvers | PathPlanner commands velocity changes every 20ms. If encoder data is delayed due to CAN congestion, the drive motor PID in closed-loop mode reacts to stale data, causing velocity oscillation. | + +**For the planned autonomous routine:** The timing margin is tight — shoot 8 preloads, drive to depot, collect fuel, drive back, and shoot again, all in 20 seconds. Even if PathPlanner successfully compensates for drift, the compensation itself costs time — every correction is distance the robot travels that isn't along the optimal path. A clean drivebase that drives straight without correction saves ~0.2–0.5 seconds per path segment. Over the five segments of the planned routine (start → band, align + shoot, band → depot, depot → band, align + shoot), those savings add up to 1–2.5 seconds — often the difference between completing the depot cycle or running out of time after the first volley. + +## 5. Recommended Path to a Competitive Autonomous + +**Phase 1 — Fix the drivebase (prerequisite, do first):** +1. Clear WPILib Preferences and verify angle offsets (§2.3, §1.5) +2. Fix CANSparkUtil signal names (§9.1) +3. Verify angleInvert and Pigeon orientation (§1.3, §1.2) +4. Test: robot should drive straight with no drift and no jittering at rest + +**Phase 2 — Port PathPlanner infrastructure from 2024:** +1. Verify PathPlanner is in `vendordeps/` (if not, add the PathPlannerLib JSON) +2. Create `AutoConstants` class in 2026 `Constants.java` with path-following PID constants — start with the 2024 values but adjust `maxSpeed` to 3 m/s (or increase `maxSpeed` once the drivebase is verified) +3. Add `closedLoopDrive()` and `getRobotRelativeSpeed()` to `SwerveSubsystem` +4. Add `configPathPlanner()` to `SwerveSubsystem` — **note: the PathPlanner API changed between 2024 and 2026**; `configureHolonomic()` was renamed and the configuration object changed. Use the 2026 PathPlanner docs for the exact API. +5. Add `followPathFromFile()` and `startAutoAt()` convenience methods +6. Re-add `keepOdometryOnField()` to `periodic()` as a safety net +7. Consider re-adding `isOdometryValid()` to skip odometry updates during encoder errors + +**Phase 3 — Build and test the planned auto routine incrementally:** +1. Phase 1 only: drive from start into shooting band, align with hub, shoot 8 preloaded fuel. Validates path following and hub alignment. +2. Add Phase 2: after shooting, drive to depot and collect fuel. Validates multi-segment navigation. +3. Add Phase 3: drive back to shooting band, re-align, shoot depot fuel. Completes the full planned routine. +4. Use AdvantageScope or Shuffleboard to compare desired vs. actual path in real-time +5. Tune PathPlanner PID constants if the robot overshoots or oscillates during path following + +**Phase 4 — Optimize for competition:** +1. Increase `maxSpeed` (currently capped at 3 m/s, 2024 used 5 m/s) +2. Remove the `* 0.5` speed multiplier in `RobotContainer` +3. Tune path constraints (max velocity, max acceleration) for each path segment +4. Consider adding path-specific vision rejection (e.g., ignore vision updates during fast maneuvers to avoid pose jumps) \ No newline at end of file diff --git a/docs/architecture-patterns-comparison.md b/docs/architecture-patterns-comparison.md new file mode 100644 index 0000000..668fd86 --- /dev/null +++ b/docs/architecture-patterns-comparison.md @@ -0,0 +1,802 @@ +# Dependency Injection vs Singleton Pattern Comparison + +## Overview + +This document compares the **Dependency Injection** pattern (currently used in this codebase) with the **Singleton** pattern (used by KnightKrawlers and many FRC teams) for managing subsystems and cross-subsystem communication. + +--- + +## Dependency Injection Pattern (Current Approach) + +### How It Works + +Subsystems are created in `RobotContainer` and passed as constructor parameters to commands and other subsystems that need them. + +**Example from your codebase:** + +```java +// RobotContainer.java +public class RobotContainer { + private final SwerveSubsystem m_drive = new SwerveSubsystem(); + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + private final IntakeSubsystem m_intake = new IntakeSubsystem(); + + public RobotContainer() { + // Pass subsystems to commands + new TeleopSwerve(m_drive, ...); + new AutoAlign(m_drive, true); + } +} + +// TeleopSwerve.java +public class TeleopSwerve extends Command { + private SwerveSubsystem m_SwerveSubsystem; // Injected dependency + + public TeleopSwerve(SwerveSubsystem swerveSubsystem, ...) { + this.m_SwerveSubsystem = swerveSubsystem; // Dependency injected + addRequirements(m_SwerveSubsystem); + } +} +``` + +### Pros ✅ + +1. **Explicit Dependencies** + - Dependencies are visible in constructor signatures + - Easy to see what a class needs to function + - Self-documenting code + +2. **Testability** + - Easy to create mock/test versions of subsystems + - Can inject test doubles for unit testing + - No global state to worry about + +3. **Flexibility** + - Can have multiple instances if needed (e.g., for testing) + - Easy to swap implementations + - Supports dependency inversion principle + +4. **Type Safety** + - Compiler enforces correct dependencies + - IDE autocomplete works well + - Refactoring tools can track dependencies + +5. **No Hidden Dependencies** + - All dependencies are explicit + - No surprise "where did this come from?" moments + - Easier to understand code flow + +### Cons ❌ + +1. **Constructor Parameter Lists** + - Can get long if a class needs many dependencies + - Example: `CandleSubsystem(shooter, intake, swerve, vision, ...)` + +2. **Wiring Complexity** + - Must pass dependencies through multiple layers + - `RobotContainer` becomes the central wiring point + - More boilerplate code + +3. **Cross-Subsystem Access** + - If `CandleSubsystem` needs to read from `ShooterSubsystem`, must pass it in constructor + - Can create circular dependency issues + - Requires planning dependency graph + +4. **Access from Anywhere** + - Can't easily access subsystems from utility classes or static methods + - Must thread dependencies through call chain + +--- + +## Singleton Pattern (KnightKrawlers Approach) + +### How It Works + +Each subsystem has a private static instance and a public `getInstance()` method that returns the single instance. + +**Example:** + +```java +// ShooterSubsystem.java +public class ShooterSubsystem extends SubsystemBase { + private static ShooterSubsystem INSTANCE; + + private ShooterSubsystem() { + // Private constructor prevents external instantiation + } + + public static ShooterSubsystem getInstance() { + if (INSTANCE == null) { + INSTANCE = new ShooterSubsystem(); + } + return INSTANCE; + } +} + +// CandleSubsystem.java +public class CandleSubsystem extends SubsystemBase { + @Override + public void periodic() { + // Access any subsystem from anywhere + if (ShooterSubsystem.getInstance().isShooterActive()) { + setColor(Color.GREEN); + } + } +} +``` + +### Pros ✅ + +1. **Global Access** + - Can access subsystems from anywhere in code + - No need to thread dependencies through constructors + - Very convenient for cross-subsystem communication + +2. **Simple Wiring** + - No constructor parameter lists + - No need to pass dependencies around + - Less boilerplate code + +3. **Single Instance Guarantee** + - Only one instance exists (matches FRC reality - one robot) + - Prevents accidental multiple instances + - Aligns with physical hardware (one drivetrain, one shooter, etc.) + +4. **Easy Cross-Subsystem Access** + - `CandleSubsystem` can easily read from any subsystem + - Perfect for status/state aggregation + - No circular dependency issues + +5. **Matches FRC Reality** + - There's only one robot + - There's only one instance of each subsystem + - Singleton enforces this at the code level + +### Cons ❌ + +1. **Hidden Dependencies** + - Dependencies are not visible in class signatures + - Hard to see what a class depends on + - Can create "spaghetti dependencies" + +2. **Testing Challenges** + - Hard to inject test doubles + - Must reset singleton state between tests + - Global state can cause test interference + +3. **Tight Coupling** + - Classes are tightly coupled to specific implementations + - Can't easily swap implementations + - Violates dependency inversion principle + +4. **Initialization Order** + - Must be careful about initialization order + - `getInstance()` might be called before subsystem is initialized + - Can cause null pointer exceptions if not careful + +5. **Thread Safety (Usually Not an Issue in FRC)** + - In multi-threaded environments, need synchronization + - FRC code runs single-threaded, so not a concern + +--- + +## RobotContainer-as-Service-Locator Pattern (Alternative Approach) + +### How It Works + +Pass `RobotContainer` itself to subsystems that need access to multiple subsystems, and add getter methods to `RobotContainer` for each subsystem. + +**Example:** + +```java +// RobotContainer.java +public class RobotContainer { + private final SwerveSubsystem m_drive = new SwerveSubsystem(); + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + private final IntakeSubsystem m_intake = new IntakeSubsystem(); + private final CandleSubsystem m_candle = new CandleSubsystem(this); // Pass 'this' + + // Getters for subsystems + public SwerveSubsystem getDrive() { return m_drive; } + public ShooterSubsystem getShooter() { return m_shooter; } + public IntakeSubsystem getIntake() { return m_intake; } + public CandleSubsystem getCandle() { return m_candle; } +} + +// CandleSubsystem.java +public class CandleSubsystem extends SubsystemBase { + private final RobotContainer container; + + public CandleSubsystem(RobotContainer container) { + this.container = container; + } + + @Override + public void periodic() { + // Access other subsystems through container + if (container.getShooter().isShooterActive()) { + setColor(Color.GREEN); + } + if (container.getIntake().hasGamePiece()) { + setColor(Color.BLUE); + } + } +} +``` + +### Pros ✅ + +1. **Single Dependency** + - Only need to pass `RobotContainer` (one parameter) + - Avoids long constructor parameter lists + - Cleaner than passing many individual subsystems + +2. **Explicit Access Point** + - Clear that you're accessing subsystems through a container + - `container.getShooter()` is more explicit than `ShooterSubsystem.getInstance()` + - Makes it obvious where subsystems come from + +3. **Testable** + - Can create a mock `RobotContainer` for testing + - Can inject test doubles through container + - Better than singleton for testing + +4. **Centralized Management** + - All subsystems are managed in one place + - Easy to see all subsystem instances + - `RobotContainer` becomes the "registry" of subsystems + +5. **No Global State** + - Avoids global singleton state + - Still allows dependency injection + - More flexible than singleton + +6. **Type Safety** + - Getter methods provide type safety + - IDE autocomplete works well + - Compiler catches errors + +### Cons ❌ + +1. **Dependency on RobotContainer** + - Creates coupling to `RobotContainer` class + - `RobotContainer` becomes a "god object" + - Can create circular dependencies if not careful + +2. **Less Explicit Dependencies** + - Not immediately clear which subsystems are actually used + - Must read code to see `container.getShooter()` calls + - Less self-documenting than direct dependency injection + +3. **Potential for Overuse** + - Easy to pass `RobotContainer` everywhere + - Can lead to tight coupling + - May violate single responsibility principle + +4. **Initialization Order** + - Must ensure `RobotContainer` is fully initialized before passing `this` + - Can cause issues if subsystems access container in constructor + - Need to be careful about initialization order + +5. **Refactoring Challenges** + - If `RobotContainer` structure changes, affects all dependent classes + - Harder to extract subsystems into separate modules + - Less modular than pure dependency injection + +--- + +## Side-by-Side Comparison + +### Example: CandleSubsystem Reading from ShooterSubsystem + +#### Dependency Injection Approach + +```java +// RobotContainer.java +private final ShooterSubsystem m_shooter = new ShooterSubsystem(); +private final CandleSubsystem m_candle = new CandleSubsystem(m_shooter); + +// CandleSubsystem.java +public class CandleSubsystem extends SubsystemBase { + private final ShooterSubsystem shooter; + + public CandleSubsystem(ShooterSubsystem shooter) { + this.shooter = shooter; + } + + @Override + public void periodic() { + if (shooter.isShooterActive()) { + setColor(Color.GREEN); + } + } +} +``` + +**Pros:** Explicit dependency, testable, clear +**Cons:** Must pass through RobotContainer, longer constructor + +#### Singleton Approach + +```java +// RobotContainer.java +// No need to pass anything - subsystems access each other directly + +// CandleSubsystem.java +public class CandleSubsystem extends SubsystemBase { + @Override + public void periodic() { + if (ShooterSubsystem.getInstance().isShooterActive()) { + setColor(Color.GREEN); + } + } +} +``` + +**Pros:** Simple, no wiring needed, easy cross-subsystem access +**Cons:** Hidden dependency, harder to test, less explicit + +#### RobotContainer-as-Service-Locator Approach + +```java +// RobotContainer.java +public class RobotContainer { + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + private final CandleSubsystem m_candle = new CandleSubsystem(this); + + public ShooterSubsystem getShooter() { return m_shooter; } +} + +// CandleSubsystem.java +public class CandleSubsystem extends SubsystemBase { + private final RobotContainer container; + + public CandleSubsystem(RobotContainer container) { + this.container = container; + } + + @Override + public void periodic() { + if (container.getShooter().isShooterActive()) { + setColor(Color.GREEN); + } + } +} +``` + +**Pros:** Single dependency, explicit access point, testable, no global state +**Cons:** Coupling to RobotContainer, less explicit about which subsystems used + +--- + +## NetworkTables Pattern (WPILib Standard) + +### How It Works + +Use WPILib's NetworkTables (a key-value store) to publish and subscribe to subsystem state. Subsystems publish their state to NetworkTables, and other subsystems read from NetworkTables. + +**Example:** + +```java +// ShooterSubsystem.java - Publisher +public class ShooterSubsystem extends SubsystemBase { + private final BooleanPublisher shooterActivePublisher = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Shooter/Active") + .publish(); + + @Override + public void periodic() { + // Publish state to NetworkTables + shooterActivePublisher.set(isShooterActive); + } +} + +// CandleSubsystem.java - Subscriber +public class CandleSubsystem extends SubsystemBase { + private final BooleanSubscriber shooterActiveSubscriber = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Shooter/Active") + .subscribe(false); // Default value if not available + + @Override + public void periodic() { + // Read state from NetworkTables + if (shooterActiveSubscriber.get()) { + setColor(Color.GREEN); + } + } +} +``` + +### Pros ✅ + +1. **WPILib Standard** + - Built into WPILib, no custom code needed + - Well-documented and supported + - Thread-safe and efficient + +2. **Decoupled Communication** + - Subsystems don't need direct references to each other + - Publisher and subscriber are completely decoupled + - No circular dependency issues + +3. **External Visibility** + - Data automatically available to Shuffleboard, AdvantageScope, etc. + - Great for debugging and monitoring + - Can be viewed from driver station + +4. **Multiple Subscribers** + - Multiple subsystems can subscribe to the same data + - No need to pass references around + - Easy to add new consumers + +5. **Persistent Across Restarts** + - NetworkTables persists data (with some limitations) + - Can survive robot code restarts + - Useful for debugging + +6. **Already in Your Codebase** + - You're already using NetworkTables for swerve states + - Consistent with existing patterns + - Team already familiar with it + +### Cons ❌ + +1. **String-Based Keys** + - Keys are strings, prone to typos + - No compile-time checking + - Refactoring is harder (find/replace strings) + +2. **Performance Overhead** + - NetworkTables has serialization overhead + - Slightly slower than direct method calls + - Usually negligible, but not zero + +3. **Type Safety** + - Less type-safe than direct method calls + - Must handle type conversions + - Runtime errors if types don't match + +4. **Default Values** + - Must provide default values for subscribers + - Need to handle "not available" cases + - Can lead to stale data if publisher stops + +5. **Not Ideal for Control** + - NetworkTables is better for state sharing than control + - Commands should still use direct subsystem access + - More suited for read-only state + +6. **Debugging Complexity** + - Harder to trace data flow + - Can't easily see who's reading/writing + - NetworkTables viewer helps but adds complexity + +### When to Use NetworkTables + +**Good for:** +- ✅ Status/state sharing (like RobotState) +- ✅ Monitoring and debugging +- ✅ Cross-subsystem read-only state +- ✅ Data that needs to be visible externally +- ✅ Decoupled communication + +**Not ideal for:** +- ❌ Control flow (commands should use direct access) +- ❌ High-frequency updates (though usually fine) +- ❌ Bidirectional communication +- ❌ When you need compile-time type safety + +--- + +## FRC-Specific Considerations + +### Why Singleton Makes Sense for FRC + +1. **Physical Reality** + - There's only one robot + - There's only one instance of each subsystem + - Singleton matches this reality + +2. **Cross-Subsystem Communication** + - Many subsystems need to read from others + - `CandleSubsystem` needs to read from multiple subsystems + - `RobotState` needs to aggregate from all subsystems + - Singleton makes this easy + +3. **Status/Aggregation Subsystems** + - `CandleSubsystem` is a "status display" subsystem + - It needs to read from many other subsystems + - Dependency injection would require many constructor parameters + - Singleton avoids this + +4. **WPILib Command Framework** + - CommandScheduler manages subsystem lifecycle + - Subsystems are registered with scheduler + - Singleton doesn't conflict with this + +### When Dependency Injection is Better + +1. **Commands** + - Commands should use dependency injection + - They're created dynamically and need explicit dependencies + - Your current approach is correct here + +2. **Testability** + - If you do extensive unit testing + - Dependency injection is easier to mock + - But many FRC teams don't do much unit testing + +3. **Multiple Implementations** + - If you might have different implementations + - But in FRC, you typically have one implementation + +--- + +## Pattern Comparison Summary + +| Aspect | Dependency Injection | Singleton | RobotContainer Locator | NetworkTables | +|--------|---------------------|-----------|----------------------|---------------| +| **Constructor Parameters** | Many (one per subsystem) | None | One (RobotContainer) | None | +| **Explicit Dependencies** | ✅ Very explicit | ❌ Hidden | ⚠️ Somewhat explicit | ❌ Hidden (string keys) | +| **Testability** | ✅ Excellent | ❌ Difficult | ✅ Good | ⚠️ Moderate | +| **Cross-Subsystem Access** | ❌ Requires passing all | ✅ Easy | ✅ Easy | ✅ Easy | +| **Global State** | ✅ None | ❌ Global | ✅ None | ⚠️ NetworkTables state | +| **Coupling** | ✅ Low | ⚠️ Medium | ❌ High (to RobotContainer) | ✅ Very Low (decoupled) | +| **Boilerplate** | ❌ More | ✅ Less | ⚠️ Medium | ⚠️ Medium (publisher/subscriber) | +| **Type Safety** | ✅ Compile-time | ✅ Compile-time | ✅ Compile-time | ❌ Runtime (string keys) | +| **External Visibility** | ❌ No | ❌ No | ❌ No | ✅ Yes (Shuffleboard, etc.) | +| **Performance** | ✅ Fastest | ✅ Fast | ✅ Fast | ⚠️ Slightly slower | +| **WPILib Standard** | ✅ Yes (pattern) | ✅ Yes (pattern) | ✅ Yes (pattern) | ✅ Yes (built-in) | + +## Hybrid Approach (Recommended) + +You can use **multiple patterns** strategically: + +1. **Subsystems: Use Singleton or RobotContainer Locator** + - Subsystems represent physical hardware (one instance) + - Easy cross-subsystem communication + - Matches FRC reality + - **RobotContainer Locator** is a good middle ground + +2. **Commands: Use Dependency Injection** + - Commands are actions, not hardware + - Created dynamically, need explicit dependencies + - Better for testing and clarity + +**Example:** + +```java +// Subsystem (Singleton) +public class ShooterSubsystem extends SubsystemBase { + private static ShooterSubsystem INSTANCE; + public static ShooterSubsystem getInstance() { ... } +} + +// Command (Dependency Injection) +public class ShootCommand extends Command { + private final ShooterSubsystem shooter; + + public ShootCommand(ShooterSubsystem shooter) { + this.shooter = shooter; + addRequirements(shooter); + } +} + +// Status Subsystem (Singleton, reads from other singletons) +public class CandleSubsystem extends SubsystemBase { + @Override + public void periodic() { + // Read from other subsystems easily + if (ShooterSubsystem.getInstance().isShooterActive()) { + setColor(Color.GREEN); + } + } +} +``` + +--- + +## Recommendation for Your Codebase + +### For CandleSubsystem Specifically + +**Three viable options:** + +#### Option 1: RobotContainer Locator (Recommended for your team) +**Best if:** You want to avoid singletons but also avoid long parameter lists + +```java +// RobotContainer.java +private final CandleSubsystem m_candle = new CandleSubsystem(this); + +// CandleSubsystem.java +public CandleSubsystem(RobotContainer container) { + this.container = container; +} +``` + +**Why this works well:** +- ✅ Single dependency (just `RobotContainer`) +- ✅ No global state (unlike singleton) +- ✅ Still testable (can mock `RobotContainer`) +- ✅ Explicit access point (`container.getShooter()`) +- ✅ Matches your current dependency injection style +- ✅ Easy to add getters as needed + +#### Option 2: Singleton Pattern +**Best if:** You want maximum simplicity and don't mind global state + +```java +// CandleSubsystem.java +@Override +public void periodic() { + if (ShooterSubsystem.getInstance().isShooterActive()) { + setColor(Color.GREEN); + } +} +``` + +**Why this works:** +- ✅ Simplest approach +- ✅ No constructor parameters +- ✅ Easy cross-subsystem access +- ✅ Matches KnightKrawlers pattern +- ❌ Global state (harder to test) +- ❌ Hidden dependencies + +#### Option 3: Dependency Injection (Current) +**Best if:** You want maximum explicitness and testability + +```java +// RobotContainer.java +private final CandleSubsystem m_candle = new CandleSubsystem(m_shooter, m_intake, m_drive); + +// CandleSubsystem.java +public CandleSubsystem(ShooterSubsystem shooter, IntakeSubsystem intake, SwerveSubsystem drive) { + this.shooter = shooter; + this.intake = intake; + this.drive = drive; +} +``` + +**Why this works:** +- ✅ Most explicit dependencies +- ✅ Best for testing +- ✅ No hidden dependencies +- ❌ Long constructor parameter list +- ❌ More boilerplate + +#### Option 4: NetworkTables (WPILib Standard) +**Best if:** You want decoupled communication and external visibility + +```java +// ShooterSubsystem.java - Publisher +private final BooleanPublisher shooterActivePublisher = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Shooter/Active") + .publish(); + +// CandleSubsystem.java - Subscriber +private final BooleanSubscriber shooterActiveSubscriber = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Shooter/Active") + .subscribe(false); +``` + +**Why this works:** +- ✅ WPILib standard, already in your codebase +- ✅ Completely decoupled subsystems +- ✅ Automatically visible in Shuffleboard/AdvantageScope +- ✅ No constructor parameters needed +- ❌ String-based keys (no compile-time checking) +- ❌ Slightly slower than direct access +- ❌ Less type-safe + +### For Other Subsystems + +**Consider Singleton** for: +- Subsystems that represent physical hardware +- Subsystems that need cross-subsystem communication +- Subsystems that are accessed from many places + +**Keep Dependency Injection** for: +- Commands (your current approach is good) +- Utility classes +- Test doubles + +--- + +## Migration Path + +If you want to adopt singletons: + +1. **Start with CandleSubsystem** - Convert it to singleton +2. **Add getInstance() to other subsystems** - Make them singletons +3. **Keep commands using DI** - Don't change command constructors +4. **Update RobotContainer** - Can still create instances there, but also support getInstance() + +**Example Migration:** + +```java +// Before (Dependency Injection) +private final CandleSubsystem m_candle = new CandleSubsystem(m_shooter, m_intake); + +// After (Singleton) +// In CandleSubsystem.periodic(), access other subsystems directly: +ShooterSubsystem.getInstance().isShooterActive() +IntakeSubsystem.getInstance().hasGamePiece() +``` + +--- + +## Conclusion + +**For FRC robot code, there are four viable approaches:** + +### NetworkTables (Best for Status Sharing) +- ✅ WPILib standard, already in your codebase +- ✅ Completely decoupled, no dependencies +- ✅ Automatically visible in Shuffleboard/AdvantageScope +- ✅ Great for monitoring and debugging +- ❌ String-based keys, less type-safe +- **Recommended if:** You want decoupled communication and external visibility, especially for read-only state + +### RobotContainer Locator (Best Middle Ground) +- ✅ Single dependency, avoids long parameter lists +- ✅ No global state, still testable +- ✅ Explicit access through getters +- ✅ Matches dependency injection philosophy +- **Recommended if:** You want to avoid singletons but need easy cross-subsystem access + +### Singleton Pattern (Simplest) +- ✅ Simplest approach, no wiring needed +- ✅ Easy cross-subsystem communication +- ❌ Global state, harder to test +- **Recommended if:** You prioritize simplicity and don't do much unit testing + +### Dependency Injection (Most Explicit) +- ✅ Most explicit, best for testing +- ✅ No hidden dependencies +- ❌ Long parameter lists for status subsystems +- **Recommended if:** You do extensive unit testing and want maximum explicitness + +**For Commands: Always use Dependency Injection** +- Commands are actions, not hardware +- Explicit dependencies are clearer +- Better for testing +- Your current approach is correct + +**Final Recommendation:** + +For `CandleSubsystem` specifically, **NetworkTables is the clear winner** because: + +1. **Dual Display Requirement**: You need the same status indicators on both: + - Physical LEDs on the robot (visible when driver can see robot) + - Driver Station display (visible when driver can't see robot, looking at camera feed) + - NetworkTables automatically makes data available to both + +2. **Single Source of Truth**: + - Publish status once to NetworkTables + - `CandleSubsystem` reads from NetworkTables to control LEDs + - Driver Station/Shuffleboard reads from NetworkTables to display status + - No duplicate logic needed + +3. **Already in Your Codebase**: + - You're already using NetworkTables for swerve states + - You're already using SmartDashboard/Shuffleboard + - Consistent with existing patterns + +4. **Perfect Use Case**: + - LED status is read-only state (perfect for NetworkTables) + - External visibility is essential (driver needs to see it) + - Completely decoupled (no constructor parameters) + +**Implementation Approach:** +- Subsystems publish their state to NetworkTables (e.g., `"RobotState/Shooter/Ready"`) +- `CandleSubsystem` subscribes to NetworkTables and controls LEDs +- Shuffleboard/Driver Station subscribes to same NetworkTables data and displays status indicators +- Single source of truth, dual display + +**Alternative:** If you don't need Driver Station display, RobotContainer Locator is a good middle ground for compile-time type safety. diff --git a/docs/auto-sim-analysis/auto-sim-analysis.md b/docs/auto-sim-analysis/auto-sim-analysis.md new file mode 100644 index 0000000..2c2c095 --- /dev/null +++ b/docs/auto-sim-analysis/auto-sim-analysis.md @@ -0,0 +1,263 @@ +# Autonomous Simulation Analysis + +## Symptom + +When switching to Autonomous mode in the simulator, the robot jumps to a position +and stops. PathPlanner's `FollowPathCommand` runs for **exactly one execute cycle**, +commands enormous speeds (7–10 m/s), then immediately finishes. TeleopSwerve (the +default command) takes over and the robot sits still. + +This happens for both `DriveTestAuto` and `EightLemonAuto`. + +Teleop and Test modes work correctly in simulation. + +--- + +## Key Files + +| File | Role | +|------|------| +| `src/main/java/frc/robot/Robot.java` | Entry point. Calls `CommandScheduler.run()` in `robotPeriodic()`, schedules auto command in `autonomousInit()`, calls `SimulationManager` in `simulationPeriodic()`. | +| `src/main/java/frc/robot/RobotContainer.java` | Creates subsystems, configures bindings. `getAutonomousCommand()` returns `EightLemonAuto` (hardcoded). Sets TeleopSwerve as default command on SwerveSubsystem. | +| `src/main/java/frc/robot/Subsystems/SwerveSubsystem.java` | Swerve drivetrain. Has `periodic()` (updates odometry), `hardResetPose()`, `startAutoAt()`, `autoDrive()`, `configurePathPlanner()`, `driveFromChassisSpeeds()`. Extends `SubsystemBase` (auto-registered with CommandScheduler). | +| `src/main/java/frc/robot/sim/SimulationManager.java` | Simulation physics. `simulationPeriodic()` integrates chassis speeds into `simPose`, writes pigeon sim state and encoder values. Consumes `pendingSimPoseReset` from SwerveSubsystem. | +| `src/main/java/frc/robot/Auto/EightLemonAuto.java` | `SequentialCommandGroup` containing `drive.startAutoAt(3.5355, 7.1300, 91.82)` then `drive.autoDrive("8FuelPath")`. | +| `src/main/java/frc/robot/Auto/DriveTestAuto.java` | `SequentialCommandGroup` containing `drive.startAutoAt(1.165, 6.000, 0.000)` then `drive.autoDrive("DriveTestPath")`. | +| `src/main/java/frc/robot/Constants.java` | Contains `SwerveConstants`, `AutoConstants` (with `ROBOT_CONFIG`, `SWERV_DRIVE_CONTROLLER`), `FieldConstants`. | + +--- + +## How the Autonomous Command is Structured + +`EightLemonAuto` is a `SequentialCommandGroup`: +1. `drive.startAutoAt(3.5355, 7.1300, 91.82)` — returns `runOnce(...)` (an InstantCommand) that calls `hardResetPose()` to set the robot's starting position and heading. +2. `drive.autoDrive("8FuelPath")` — returns `AutoBuilder.followPath(path)` which is PathPlanner's `FollowPathCommand`. + +In `autonomousInit()`, the entire SequentialCommandGroup is scheduled via `CommandScheduler.getInstance().schedule(...)`. + +### How PathPlanner is Configured + +```java +// SwerveSubsystem.configurePathPlanner() +AutoBuilder.configure( + this::getPose, // pose supplier — reads from odometry + this::resetOdometry, // pose resetter — calls hardResetPose(pose, false) + this::getChassisSpeeds, // chassis speeds supplier + (speeds, feedforwards) -> driveFromChassisSpeeds(speeds, false), // drive consumer (closed-loop) + AutoConstants.SWERV_DRIVE_CONTROLLER, // PPHolonomicDriveController with PID + AutoConstants.ROBOT_CONFIG, // RobotConfig with module locations + FieldConstants::isRedAlliance, // alliance flip supplier + this // subsystem requirement +); +``` + +### How `hardResetPose` Works + +```java +private void hardResetPose(Pose2d newPose, boolean updatePigeon) { + if (updatePigeon) { + pigeon.setYaw(newPose.getRotation().getDegrees()); + } + if (odometry != null) { + Rotation2d gyroAngle = updatePigeon ? newPose.getRotation() : getYaw(); + odometry.resetPosition(gyroAngle, getPositions(), newPose); + } + pendingSimPoseReset = newPose; // signals SimulationManager +} +``` + +### How `periodic()` Works + +```java +public void periodic() { + odometry.update(getYaw(), getPositions()); // reads pigeon + module encoders + updateOdometryWithVision("limelight-a"); + updateOdometryWithVision("limelight-b"); + field.setRobotPose(getPose()); + robotPose.set(getPose()); + // ... SmartDashboard updates +} +``` + +### How SimulationManager Works + +```java +public void simulationPeriodic() { + // 1. Consume any pending pose reset from SwerveSubsystem + Pose2d reset = swerveSubsystem.consumeSimPoseReset(); + if (reset != null) { simPose = reset; } + + // 2. Integrate chassis speeds into simPose + simPose = simPose.exp(new Twist2d(vx*dt, vy*dt, omega*dt)); + + // 3. Update pigeon sim state + pigeonSimState.setRawYaw(simPose.getRotation().getDegrees()); + + // 4. Update module encoder sim states + updateModuleEncoders(desiredStates, dt); +} +``` + +--- + +## Diagnostic Method + +We added temporary logging (still present, prefixed `[SWERVE-DEBUG]`, `[SIM-DEBUG]`, +`[DRIVE-DEBUG]`) to: + +- `hardResetPose` — logs pose and updatePigeon flag +- `driveFromChassisSpeeds` — logs vx/vy/omega and openLoop flag (first 20 cycles) +- `resetOdometry` — logs pose + stack trace +- `consumeSimPoseReset` — logs consumed pose +- `SimulationManager.simulationPeriodic()` — logs dt, desired speeds, simPose, odoPose (first 10 cycles after reset) + +--- + +## Confirmed Bugs Found and Fixed + +### Bug 1: Wrong Field Dimensions — FIXED ✅ + +| Source | Field Length | Field Width | +|--------|-------------|-------------| +| `FieldConstants` (was) | **17.548** | **8.052** | +| PathPlanner `FlippingUtil` default | 16.54 | 8.07 | +| `navgrid.json` in this project | 16.54 | 8.07 | +| 2026 official field drawings | 16.54 | 8.07 | + +**Impact**: When flipping for red alliance, the robot was placed ~1 meter away in X +from where PathPlanner expected the path to start. + +**Fix applied**: Updated `FieldConstants.FIELD_LENGTH` to `16.5410` and +`FIELD_WIDTH` to `8.0693`. + +**Result**: Position now matches PathPlanner's expectation (confirmed by diagnostic +output: `hardResetPose: pose=Pose2d(Translation2d(X: 13.00, Y: 0.94), ...)`). + +### Bug 2: EightLemonAuto Starting Rotation Mismatch — FIXED ✅ + +`EightLemonAuto.startAutoAt` used heading `-130.45°`, but the 8FuelPath's +`idealStartingState.rotation` is `91.82°`. These must match so PathPlanner can use +its pre-computed ideal trajectory. + +**Fix applied**: Changed `startAutoAt(3.5355, 7.1300, 91.82)` to match the path file. + +**Result**: After alliance-flipping, the heading is now 271.82° for red, which matches +PathPlanner's mirrored path. + +### Bug 3: Pigeon `setYaw` / `setRawYaw` Doubling in Simulation — ATTEMPTED, REVERTED + +In CTRE Phoenix 6 simulation, `pigeon.setYaw(X)` creates an internal offset and +`pigeonSimState.setRawYaw(Y)` sets the raw value. When both are used with the same +target, the reported yaw doubles. + +**Fix attempted**: Guard `pigeon.setYaw()` with `RobotBase.isReal()`. + +**Result**: The heading doubling appeared to stop in diagnostic output, but the overall +issue was not resolved. **Reverted** to avoid leaking simulation-specific branching +logic into production code. `hardResetPose` now unconditionally calls `pigeon.setYaw()`. +Any pigeon simulation quirks should be handled entirely within `SimulationManager`. + +### Bug 4: Module Order Mismatch in RobotConfig — FIXED ✅ + +`SwerveDriveKinematics` uses order FL, FR, BR, BL but `AutoConstants.ROBOT_CONFIG` had +FL, FR, BL, BR (back modules swapped). + +**Fix applied**: Changed `ROBOT_CONFIG` to match kinematics order. + +**Result**: No observable change (robot is square — swapped modules have identical +positions). Fix is correct but had no practical effect. + +### Bug 5: Pigeon Sim Timing — rawYaw Not Set Before periodic() — ATTEMPTED, REVERTED + +Hypothesis was that `periodic()` reads a stale pigeon value before `SimulationManager` +can update it. + +**Fix attempted**: Call `pigeon.getSimState().setRawYaw(heading)` inside `hardResetPose` +when in simulation. + +**Result**: No effect — `odoPose` rotation was still 0.00° at cycle 0. Either CTRE sim +state doesn't take effect immediately, or execution order differs from assumption. +**Reverted** to keep sim logic out of production code. + +--- + +## Current Status + +After five fix attempts, the core symptom is unchanged: PathPlanner commands huge speeds +for exactly one cycle, then finishes. + +### What We Know For Certain (from diagnostic output) + +1. `hardResetPose` fires correctly with pose `(13.00, 0.94, 271.82°)`. +2. `SimulationManager` consumes the pose reset and sets `simPose` correctly. +3. PathPlanner's first `execute()` commands enormous speeds: `vx=7.136 vy=7.647 omega=-10.064`. +4. PathPlanner's second cycle commands `vx=0 vy=0 omega=0` — the command has finished. +5. From cycle 2 onward, `openLoop=true` — TeleopSwerve (default command) has taken over. +6. `odoPose` rotation is 0.00° at cycle 0 despite `hardResetPose` setting it to 271.82°. + +### What We Don't Know + +1. **Why is `odoPose` 0° at cycle 0?** Our attempts to fix the pigeon value before + `periodic()` had no effect. We don't know the actual execution order — specifically + whether `periodic()` even runs between `startAutoAt` and PathPlanner's first execute. + +2. **Why does `FollowPathCommand` finish after exactly 1 cycle?** We hypothesized NaN + trajectory time but have NOT confirmed this. Could be NaN, could be a very short + time, could be another early-termination condition. + +3. **What is the actual execution order within `CommandScheduler.run()`?** When a + `SequentialCommandGroup` contains an `InstantCommand` (via `runOnce`) followed by + `FollowPathCommand`, does the scheduler run both within the same `run()` call? + If so, `periodic()` does NOT run between `startAutoAt` and PathPlanner's first + `execute()` — which would invalidate our theory about periodic() corrupting the pose. + +### Key Architectural Concern + +We want to avoid leaking simulation-specific code into production robot code. The +`hardResetPose` method should remain clean production code with no `isReal()`/ +`isSimulation()` branching. Simulation-specific pigeon/encoder handling belongs +entirely in `SimulationManager`. + +--- + +## Latest Console Output (for reference) + +``` +Selected auto mode: EightLemonAuto +[SWERVE-DEBUG] autoDrive: loaded path '8FuelPath' successfully +[SWERVE-DEBUG] hardResetPose: pose=Pose2d(Translation2d(X: 13.00, Y: 0.94), Rotation2d(Rads: 4.74, Deg: 271.82)) updatePigeon=true +[SIM-DEBUG] consumeSimPoseReset: Pose2d(Translation2d(X: 13.00, Y: 0.94), Rotation2d(Rads: 4.74, Deg: 271.82)) +[SIM-DEBUG] cycle=0 dt=0.0712 desiredVx=0.000 desiredVy=0.000 desiredOmega=0.000 simPose=Pose2d(Translation2d(X: 13.00, Y: 0.94), Rotation2d(Rads: 4.74, Deg: 271.82)) odoPose=Pose2d(Translation2d(X: 13.00, Y: 0.94), Rotation2d(Rads: 0.00, Deg: 0.00)) +[DRIVE-DEBUG] cycle=0 vx=7.136 vy=7.647 omega=-10.064 openLoop=false +[DRIVE-DEBUG] cycle=1 vx=0.000 vy=0.000 omega=0.000 openLoop=false +[SIM-DEBUG] cycle=1 dt=0.0026 desiredVx=0.000 desiredVy=0.000 desiredOmega=0.000 simPose=Pose2d(Translation2d(X: 13.00, Y: 0.94), Rotation2d(Rads: -1.54, Deg: -88.18)) odoPose=Pose2d(Translation2d(X: 13.00, Y: 0.94), Rotation2d(Rads: 0.00, Deg: 0.00)) +[DRIVE-DEBUG] cycle=2 vx=0.000 vy=-0.000 omega=0.000 openLoop=true +[SIM-DEBUG] cycle=2 dt=0.0050 desiredVx=0.000 desiredVy=0.000 desiredOmega=0.000 simPose=Pose2d(Translation2d(X: 13.00, Y: 0.94), Rotation2d(Rads: -1.54, Deg: -88.18)) odoPose=Pose2d(Translation2d(X: 13.00, Y: 0.94), Rotation2d(Rads: -1.54, Deg: -88.18)) +``` + +Note: `odoPose` rotation is 0.00° at cycles 0 and 1, then jumps to -88.18° at cycle 2. +PathPlanner has already finished by cycle 1. + +--- + +## Summary of All Fix Attempts + +| # | Issue | Status | Result | Files | +|---|-------|--------|--------|-------| +| 1 | Field dimensions (17.548 → 16.54) | ✅ Fixed | Position now correct | `Constants.java` | +| 2 | EightLemonAuto rotation (-130.45° → 91.82°) | ✅ Fixed | Heading now matches path | `EightLemonAuto.java` | +| 3 | Pigeon setYaw/setRawYaw doubling in sim | ⏪ Reverted | Appeared to help in isolation; reverted to keep sim logic out of production code | `SwerveSubsystem.java` | +| 4 | Module order mismatch in RobotConfig | ✅ Fixed | No effect (square robot) | `Constants.java` | +| 5 | Pigeon sim timing: rawYaw in hardResetPose | ⏪ Reverted | No effect; reverted to keep sim logic out of production code | `SwerveSubsystem.java` | + +## Existing Temporary Diagnostic Logging + +The following temporary logging is still in the code and should be replaced/enhanced +by the instrumentation effort: + +- `SwerveSubsystem.hardResetPose` — `[SWERVE-DEBUG]` prefix +- `SwerveSubsystem.autoDrive` — `[SWERVE-DEBUG]` prefix +- `SwerveSubsystem.driveFromChassisSpeeds` — `[DRIVE-DEBUG]` prefix (first 20 cycles) +- `SwerveSubsystem.resetOdometry` — `[SWERVE-DEBUG]` prefix + stack trace +- `SwerveSubsystem.consumeSimPoseReset` — plain println +- `SimulationManager.simulationPeriodic` — `[SIM-DEBUG]` prefix (first 10 cycles after reset) diff --git a/docs/auto-sim-analysis/auto-sim-instrument-prompt.md b/docs/auto-sim-analysis/auto-sim-instrument-prompt.md new file mode 100644 index 0000000..675ebb8 --- /dev/null +++ b/docs/auto-sim-analysis/auto-sim-instrument-prompt.md @@ -0,0 +1,281 @@ +# Instrumentation Task: Autonomous Simulation Runtime Analysis + +## Context + +Read `docs/auto-sim-analysis.md` first — it contains the full history of this +investigation, including the symptom, what we've tried, what worked, what didn't, +and what we still don't know. + +**TL;DR**: When autonomous mode starts in simulation, PathPlanner's `FollowPathCommand` +runs for exactly ONE cycle, commands huge speeds, then finishes. We've been doing +static analysis (reading code, forming hypotheses, applying fixes) for five iterations +and the core issue is unchanged. We need to switch to **runtime analysis** — instrument +the code to see exactly what happens, in what order, with what values. + +## Goal + +Add comprehensive instrumentation logging to trace the **exact runtime execution flow** +from the moment autonomous mode starts until PathPlanner finishes (which currently +takes ~1 cycle). The output should let us: + +1. **Determine the exact execution order** — which methods are called in what sequence, + within which WPILib lifecycle callback +2. **See the values at every decision point** — what does PathPlanner see when it + decides to finish? What does odometry report? What does the pigeon report? +3. **Identify the boundary between our code and 3rd-party code** — when execution + enters a library method we can't instrument (PathPlanner, WPILib, CTRE), log the + inputs going in and the outputs/state coming out + +## Instrumentation Rules + +### Easy Removal + +All instrumentation code MUST be trivial to find and remove after the analysis is +complete. Follow these rules strictly: + +1. **Every instrumentation line** (imports, field declarations, method calls) MUST be + on its own line(s) and bracketed by comments: + ```java + // --- TRACE INSTRUMENTATION --- + TraceLogger.log("SwerveSubsystem.periodic", "ENTER"); + // --- END TRACE --- + ``` + +2. **The `TraceLogger` utility class** should be in its own file + (`src/main/java/frc/robot/util/TraceLogger.java`) so it can be deleted in one step. + +3. **Do NOT modify existing lines of code.** Add trace lines *between* existing lines. + Never combine a trace call with a functional code change on the same line. + +4. After the analysis, a single `grep -r "TRACE INSTRUMENTATION" --include="*.java"` + will find every instrumented location, and the blocks between `--- TRACE + INSTRUMENTATION ---` and `--- END TRACE ---` can be deleted to restore the code + exactly to its pre-instrumentation state. + +5. **Remove the existing temporary diagnostics** (prefixed `[SWERVE-DEBUG]`, + `[SIM-DEBUG]`, `[DRIVE-DEBUG]`, and the plain `println` in `consumeSimPoseReset`) + as part of adding the new instrumentation. These are NOT bracketed with removal + markers and have been cluttering the code. Replace them with properly bracketed + trace lines. This cleanup is part of the instrumentation task. + +### Logging Format + +All instrumentation lines MUST use this format: +``` +[TRACE-nnn] LOCATION | message +``` + +Where: +- `nnn` is a monotonically increasing sequence number (use a shared static counter) +- `LOCATION` is the class and method name (e.g., `SwerveSubsystem.periodic`) +- `message` contains the relevant data + +Example: +``` +[TRACE-001] Robot.autonomousInit | ENTER +[TRACE-002] RobotContainer.getAutonomousCommand | creating EightLemonAuto +[TRACE-003] Robot.autonomousInit | scheduling command: EightLemonAuto +[TRACE-004] Robot.robotPeriodic | ENTER — calling CommandScheduler.run() +[TRACE-005] SwerveSubsystem.periodic | ENTER — getYaw()=0.00° odoPose before update=(0,0,0°) +[TRACE-006] SwerveSubsystem.periodic | after odometry.update() — odoPose=(0,0,0°) +[TRACE-007] SwerveSubsystem.periodic | EXIT +... +``` + +### Shared Sequence Counter + +Create a simple utility class for the shared counter: + +```java +// src/main/java/frc/robot/util/TraceLogger.java +public class TraceLogger { + private static int seq = 0; + public static void log(String location, String message) { + System.out.printf("[TRACE-%03d] %s | %s%n", seq++, location, message); + } + public static void reset() { seq = 0; } +} +``` + +### 3rd-Party Method Boundaries + +When execution calls a method from a 3rd-party library (PathPlanner, WPILib, +CTRE Phoenix), we CANNOT instrument the library's internal code. Instead, log +**before and after** the call with inputs and outputs: + +```java +// BEFORE calling 3rd-party method +TraceLogger.log("SwerveSubsystem.periodic", + "CALLING odometry.update() — gyroAngle=" + getYaw() + " positions=" + Arrays.toString(getPositions())); +odometry.update(getYaw(), getPositions()); +// AFTER calling 3rd-party method +TraceLogger.log("SwerveSubsystem.periodic", + "RETURNED from odometry.update() — odoPose=" + odometry.getEstimatedPosition()); +``` + +Mark these clearly so the reader knows the gap represents opaque 3rd-party execution: +``` +[TRACE-042] SwerveSubsystem.periodic | >>> ENTERING 3RD-PARTY: odometry.update(gyro=271.82°, positions=[...]) +[TRACE-043] SwerveSubsystem.periodic | <<< RETURNED FROM 3RD-PARTY: odometry.update() → odoPose=(13.00, 0.94, 0.00°) +``` + +### What to Instrument + +Instrument **every method** in the execution path from `autonomousInit()` through +the first few cycles of autonomous. Specifically: + +#### 1. `Robot.java` +- `autonomousInit()` — ENTER/EXIT, what command is created and scheduled +- `robotPeriodic()` — ENTER/EXIT (before and after `CommandScheduler.run()`) +- `simulationPeriodic()` — ENTER/EXIT (before and after `simManager.simulationPeriodic()`) + +#### 2. `RobotContainer.java` +- `getAutonomousCommand()` — what auto mode is selected, what command is returned + +#### 3. `SwerveSubsystem.java` +- `periodic()` — ENTER/EXIT, pigeon yaw before/after `odometry.update()`, odoPose + before/after +- `hardResetPose()` — all parameters, pigeon yaw before/after `pigeon.setYaw()`, + odoPose before/after `odometry.resetPosition()` +- `startAutoAt()` — the lambda execution (when does it actually run?) +- `autoDrive()` — path loading, what `AutoBuilder.followPath()` returns +- `driveFromChassisSpeeds()` — speeds and openLoop flag +- `resetOdometry()` — pose parameter (PathPlanner may call this) +- `getPose()` — what it returns (PathPlanner calls this frequently) +- `getChassisSpeeds()` — what it returns (PathPlanner calls this) +- `getYaw()` — raw pigeon value and returned value +- `configurePathPlanner()` — confirm this runs during construction + +#### 4. `SimulationManager.java` +- `simulationPeriodic()` — ENTER/EXIT, consumed reset, dt, desired speeds, + simPose before/after integration, pigeon value set, encoder values set + +#### 5. `EightLemonAuto.java` / `DriveTestAuto.java` +- Constructor — log when `addCommands` is called (this runs during `autonomousInit` + when the command is constructed) + +### Cycle and Phase Tracking + +The `TraceLogger` should track the current **phase** and **cycle number** to help +segment the output during analysis. Phases correspond to WPILib lifecycle states: + +```java +public class TraceLogger { + private static int seq = 0; + private static String currentPhase = "INIT"; + private static int cycleInPhase = 0; + + public static void log(String location, String message) { + System.out.printf("[TRACE-%03d] [%s cycle=%d] %s | %s%n", + seq++, currentPhase, cycleInPhase, location, message); + } + + public static void setPhase(String phase) { + currentPhase = phase; + cycleInPhase = 0; + log("TraceLogger", "=== PHASE CHANGE: " + phase + " ==="); + } + + public static void incrementCycle() { cycleInPhase++; } + public static int getCycleInPhase() { return cycleInPhase; } + public static String getPhase() { return currentPhase; } + public static void reset() { seq = 0; currentPhase = "INIT"; cycleInPhase = 0; } +} +``` + +Call `TraceLogger.setPhase("AUTONOMOUS")` in `Robot.autonomousInit()` (before +scheduling the command), and `TraceLogger.incrementCycle()` at the top of +`Robot.robotPeriodic()`. + +Example output with phases: +``` +[TRACE-047] [AUTONOMOUS cycle=0] Robot.robotPeriodic | ENTER +[TRACE-048] [AUTONOMOUS cycle=0] SwerveSubsystem.periodic | ENTER +... +[TRACE-071] [AUTONOMOUS cycle=0] Robot.simulationPeriodic | EXIT +[TRACE-072] [AUTONOMOUS cycle=1] Robot.robotPeriodic | ENTER +``` + +### Cycle Limiting + +**We only care about autonomous mode.** The robot starts in disabled mode and may +run several cycles before the user switches to autonomous. Comprehensive logging +should be **limited to the AUTONOMOUS phase** — specifically the first 5 cycles +after `autonomousInit()` fires. During other phases (DISABLED, TELEOP), either +don't log at all or log only phase transitions. + +Use the phase and cycle tracking to manage this: +- During `AUTONOMOUS` phase, cycles 0–4: log everything +- During `AUTONOMOUS` phase, cycle 5+: stop logging (or one summary line per cycle) +- During other phases: log only `setPhase()` transitions + +Err on the side of **over-instrumenting within those 5 autonomous cycles.** Since +the issue manifests in the very first cycle, even 5 cycles is generous. The bounded +window means even verbose logging produces a manageable amount of output. + +### Important: `getPose()` and `getChassisSpeeds()` are called by PathPlanner + +PathPlanner's `FollowPathCommand` calls `getPose()` and `getChassisSpeeds()` (via +the suppliers registered in `configurePathPlanner`). These calls happen INSIDE +PathPlanner's `execute()` method. By instrumenting `getPose()` and +`getChassisSpeeds()`, we can see what values PathPlanner is working with, even +though we can't instrument PathPlanner itself. + +Similarly, PathPlanner drives the robot by calling the drive consumer lambda +`(speeds, feedforwards) -> driveFromChassisSpeeds(speeds, false)`. By instrumenting +`driveFromChassisSpeeds`, we see PathPlanner's output. + +And PathPlanner may call `resetOdometry()` (the pose resetter registered in +`configurePathPlanner`). Instrumenting that tells us if PathPlanner is resetting +the pose. + +### Important: Replace Existing Diagnostics + +The code already contains temporary diagnostic logging (see "Existing Temporary +Diagnostic Logging" section in `auto-sim-analysis.md`). **Replace** all existing +`[SWERVE-DEBUG]`, `[SIM-DEBUG]`, `[DRIVE-DEBUG]` logging with the new unified +`[TRACE-nnn]` format. Don't have two logging systems running simultaneously. + +## Expected Output + +The console output (which will be redirected to a file) should tell a complete story. +Reading it top-to-bottom should answer: + +1. What is the exact sequence of method calls from `autonomousInit` through the + first few `robotPeriodic` cycles? +2. When does `startAutoAt`'s lambda actually execute? Before or after `periodic()`? +3. What does `pigeon.getYaw()` return at each point? Does `pigeon.setYaw()` take + effect immediately or is it deferred? +4. What does `odometry.getEstimatedPosition()` return before and after each + `update()` and `resetPosition()` call? +5. What values does PathPlanner see when it calls `getPose()` and `getChassisSpeeds()`? +6. What speeds does PathPlanner command via `driveFromChassisSpeeds()`? +7. When does `SimulationManager.simulationPeriodic()` run relative to everything else? +8. Where exactly do 3rd-party library calls happen, and what are their inputs/outputs? + +## Console Output Notes + +- The console output will be redirected to a file for analysis +- 3rd-party libraries (WPILib, PathPlanner, CTRE Phoenix) may also print to the + console — their output will be interspersed with our `[TRACE-nnn]` lines +- Our trace lines are easily filterable by the `[TRACE-` prefix +- The sequence numbers allow reconstructing the exact order even if output from + multiple sources is interleaved + +## After Instrumentation + +After adding the instrumentation: +1. Run `./gradlew simulateJava` and redirect output to a file +2. Switch to Autonomous mode in the Sim GUI +3. Capture the output +4. Analyze the trace to build a complete picture of the execution flow +5. Identify the root cause of the issue +6. Create a flow chart or sequence diagram of what actually happens at runtime + +## Design Principle + +**Do NOT add simulation-specific branching (`RobotBase.isReal()`, `isSimulation()`) +to production code.** If simulation-specific behavior is needed, it should live +entirely in `SimulationManager`. The production code (`SwerveSubsystem`, +`Robot`, `RobotContainer`, auto commands) should be identical whether running on +the real robot or in simulation. diff --git a/docs/auto-sim-analysis/auto-sim-log-analysis-prompt.md b/docs/auto-sim-analysis/auto-sim-log-analysis-prompt.md new file mode 100644 index 0000000..80bde79 --- /dev/null +++ b/docs/auto-sim-analysis/auto-sim-log-analysis-prompt.md @@ -0,0 +1,141 @@ +# Task: Analyze Autonomous Simulation Trace Log + +## Context + +We're debugging a FRC robot simulation where PathPlanner's `FollowPathCommand` +finishes after exactly one execute cycle, commanding enormous speeds (7+ m/s), +then stopping. We've already found and fixed 4 bugs (field dimensions, rotation +mismatch, module order) but the core symptom persists. We instrumented the code +with comprehensive trace logging to capture the exact runtime execution flow. +The trace output is now in `sim-output.log`. + +## Files to Read (in this order) + +1. **`docs/auto-sim-analysis.md`** — Full investigation history: symptom + description, key files and their roles, how the autonomous command is + structured, how PathPlanner is configured, how `hardResetPose`/`periodic`/ + `SimulationManager` work, all bugs found and fixed so far, what we know and + don't know, and the latest console output from before instrumentation. + +2. **`docs/auto-sim-instrument.md`** — The instrumentation specification: what + was instrumented, the logging format, trace rules, what questions the trace + should answer. + +3. **`sim-output.log`** — The actual trace output captured from running the + simulator and switching to Autonomous mode. This is the primary artifact to + analyze. + +4. **Source files** (read as needed during analysis): + - `src/main/java/frc/robot/util/TraceLogger.java` — The trace logging utility + - `src/main/java/frc/robot/Robot.java` — Entry point, lifecycle callbacks + - `src/main/java/frc/robot/RobotContainer.java` — Command creation + - `src/main/java/frc/robot/Subsystems/SwerveSubsystem.java` — Swerve drivetrain (most heavily instrumented) + - `src/main/java/frc/robot/sim/SimulationManager.java` — Simulation physics + - `src/main/java/frc/robot/Auto/EightLemonAuto.java` — The auto command being run + - `src/main/java/frc/robot/Constants.java` — Configuration constants + +## Trace Format + +Every trace line follows this format: +``` +[TRACE-nnn] [PHASE cycle=N] LOCATION | message +``` +- `nnn` = global sequence number (monotonically increasing) +- `PHASE` = `INIT`, `DISABLED`, or `AUTONOMOUS` +- `cycle=N` = cycle count within the current phase +- `LOCATION` = class.method being traced +- 3rd-party library call boundaries are marked with `>>>` (entering) and `<<<` (returning) + +The trace only emits verbose output during the first 5 autonomous cycles +(cycles 0–4). During DISABLED phase, only phase-change transitions are logged. + +## What to Analyze + +Read `auto-sim-analysis.md` and `auto-sim-instrument.md` first to understand +the full context and the questions we're trying to answer. Then analyze +`sim-output.log` to build a complete picture of what happens at runtime. + +### Specific Questions to Answer + +These come directly from the instrumentation spec (Section "Expected Output"): + +1. **What is the exact sequence of method calls** from `autonomousInit` through + the first few `robotPeriodic` cycles? + +2. **When does `startAutoAt`'s lambda actually execute?** Before or after + `periodic()`? (This determines whether odometry sees the reset pose before + PathPlanner's first execute.) + +3. **What does `pigeon.getYaw()` return at each point?** Does `pigeon.setYaw()` + take effect immediately or is it deferred? (Look at the pigeon yaw value + logged before and after `pigeon.setYaw()` in `hardResetPose`, and the yaw + value passed to `odometry.update()` in `periodic()`.) + +4. **What does `odometry.getEstimatedPosition()` return** before and after each + `update()` and `resetPosition()` call? + +5. **What values does PathPlanner see** when it calls `getPose()` and + `getChassisSpeeds()`? (Look for `SwerveSubsystem.getPose` and + `SwerveSubsystem.getChassisSpeeds` trace lines that appear BETWEEN + `periodic` EXIT and `robotPeriodic` EXIT — those are PathPlanner calling + them during command execution.) + +6. **What speeds does PathPlanner command** via `driveFromChassisSpeeds()`? + How many cycles does it command non-zero speeds before finishing? + +7. **When does `SimulationManager.simulationPeriodic()` run** relative to + everything else? (It runs AFTER `robotPeriodic`, so the pigeon sim state + it writes is not visible until the NEXT cycle's `periodic()`.) + +8. **Where exactly do 3rd-party library calls happen**, and what are their + inputs/outputs? + +### Key Mystery From Previous Analysis + +From `auto-sim-analysis.md`, Section "What We Don't Know": + +- **Why is `odoPose` rotation 0° right after `hardResetPose` set it to 271.82°?** + The `hardResetPose` calls `odometry.resetPosition()` with the correct pose, + and the trace confirms odometry reports the correct pose immediately after. + But then `periodic()` calls `odometry.update()` and the rotation resets to 0°. + WHY? Look at what yaw value `periodic()` passes to `odometry.update()` — if + the pigeon hasn't been updated yet (because `SimulationManager` hasn't run), + the gyro yaw will be stale/wrong, and the odometry estimator will "correct" + the pose based on the stale gyro reading. + +- **Why does `FollowPathCommand` finish after exactly 1 execute cycle?** + Look at what `getPose()` returns when PathPlanner calls it. If the pose + heading is 0° instead of 271.82°, PathPlanner may compute a trajectory + that's essentially instant or invalid, causing immediate termination. + +## Deliverables + +After analyzing the trace, provide: + +1. **A cycle-by-cycle narrative** — Walk through the trace output and explain + what happens at each step, especially the first 3 autonomous cycles. + +2. **Root cause identification** — Based on the trace evidence, identify why + PathPlanner finishes after one cycle. Be specific about which values are + wrong, why they're wrong, and the chain of causation. + +3. **Execution order diagram** — Show the actual method call order within a + single `robotPeriodic` cycle (e.g., `periodic()` → CommandScheduler executes + commands → `simulationPeriodic()`), noting where the timing gap causes + problems. + +4. **Proposed fix** — Based on the root cause, propose a specific fix. Remember + the design constraint: **no simulation-specific branching in production code** + (`SwerveSubsystem`, `Robot`, etc.). Simulation-specific fixes belong in + `SimulationManager`. + +## Important Notes + +- The trace sequence numbers start at 6939 for AUTONOMOUS because ~6938 + trace calls happened during the DISABLED phase (the `shouldLog()` method + suppressed their output but the counter still incremented). +- 3rd-party output (WPILib warnings, CTRE Phoenix messages) is interleaved + with trace lines — these may contain clues. +- The `openLoop=true` flag in `driveFromChassisSpeeds` indicates TeleopSwerve + (the default command) has taken over — PathPlanner uses `openLoop=false`. +- Do NOT modify any code or instrumentation. This task is analysis only. diff --git a/docs/auto-sim-analysis/auto-sim-log-analysis.md b/docs/auto-sim-analysis/auto-sim-log-analysis.md new file mode 100644 index 0000000..17b9179 --- /dev/null +++ b/docs/auto-sim-analysis/auto-sim-log-analysis.md @@ -0,0 +1,1496 @@ +# Autonomous Simulation Trace Log Analysis + +## 1. Cycle-by-Cycle Narrative + +### Pre-Autonomous: DISABLED Phase (TRACE-006 through TRACE-6938) + +The robot starts in DISABLED mode. ~6,938 trace calls occur but are suppressed +by `shouldLog()` (only phase changes are logged during DISABLED). The shared +counter increments silently, which is why AUTONOMOUS starts at TRACE-6939. + +--- + +### AUTONOMOUS Cycle 0 — `autonomousInit()` (TRACE-6939 → TRACE-6960) + +This is the critical cycle where the pose reset happens and the bug is planted. + +**TRACE-6939–6940**: Phase changes to AUTONOMOUS. `Robot.autonomousInit()` enters. + +**TRACE-6941–6948**: The EightLemonAuto command is constructed: +- `getAutonomousCommand()` selects EightLemonAuto +- The EightLemonAuto constructor calls `addCommands(...)` which creates: + 1. `drive.startAutoAt(3.5355, 7.13, 91.82)` → a `runOnce(...)` (InstantCommand) + 2. `drive.autoDrive("8FuelPath")` → a `FollowPathCommand` +- The path file loads successfully, and a FollowPathCommand is returned + +**TRACE-6949–6950**: `autonomousInit()` receives the command and calls +`CommandScheduler.getInstance().schedule(...)`. + +**KEY FINDING — TRACE-6951**: The `startAutoAt` lambda **executes immediately** +during `schedule()`. This is because WPILib's `CommandScheduler.schedule()` calls +`command.initialize()` synchronously. For a `SequentialCommandGroup`, +`initialize()` calls the first sub-command's `initialize()`. Since `startAutoAt` +returns `runOnce(...)` — which places the action in `initialize()`, not +`execute()` — the lambda fires immediately during the `schedule()` call, +**inside `autonomousInit()`**, before any `robotPeriodic()` has run. + +> **Note on `runOnce` semantics:** WPILib's `SubsystemBase.runOnce(Runnable)` +> creates an `InstantCommand` where `initialize()` runs the lambda, `execute()` +> is a no-op, and `isFinished()` always returns `true`. The action is placed in +> `initialize()` — not `execute()` — by design, so the command completes in a +> single scheduler cycle. This is the standard WPILib pattern for one-shot +> actions in a `SequentialCommandGroup` and is correct here; the bug is not in +> the command structure but in what `hardResetPose` does when the lambda runs. + +**TRACE-6952–6958**: The lambda executes `hardResetPose()`: + +| Step | What happens | Value | +|------|-------------|-------| +| **6953** | pigeonYawBefore | **0.0°** | +| **6953** | odoPoseBefore | (0.00, 0.00, 0.00°) | +| **6954** | `pigeon.setYaw(271.82)` called | — | +| **6955** | pigeonYawAfter | **0.0°** ⚠️ `setYaw` did NOT take effect | +| **6956** | `odometry.resetPosition(gyroAngle=271.82°, ...)` called | gyroAngle = `newPose.getRotation()` = 271.82° | +| **6957** | odoPoseAfter | (13.00, 0.94, **271.82°**) ✓ Correct | +| **6958** | pendingSimPoseReset set | (13.00, 0.94, 271.82°) | + +The critical observation: **`pigeon.setYaw(271.82)` has NO immediate effect** +(TRACE-6955 shows yaw still 0.0°). But `odometry.resetPosition()` was called +with `gyroAngle=271.82°` (the desired heading, not the actual pigeon reading). +The odometry is now internally calibrated to the assumption that "the gyro is +currently reading 271.82°" — but it isn't. It reads 0°. + +**TRACE-6959–6960**: Lambda exits, `autonomousInit()` exits. No `periodic()` or +`simulationPeriodic()` has run yet. + +--- + +### AUTONOMOUS Cycle 1 — First `robotPeriodic()` (TRACE-6961 → TRACE-6979) + +**TRACE-6961**: `robotPeriodic()` enters. This is the first periodic cycle after +autonomous starts. + +**TRACE-6962**: `SwerveSubsystem.periodic()` runs. +odoPose before update = **(13.00, 0.94, 271.82°)** — still correct from +`hardResetPose`. + +**TRACE-6963**: `odometry.update()` is called with: +- `yaw = Rotation2d(0.00°)` ← **THE BUG MANIFESTS HERE**. The pigeon yaw is + still 0° because `pigeon.setYaw()` hasn't taken effect. +- `positions = [0, 0, 0, 0]` — no wheel movement + +**TRACE-6964**: `odometry.update()` returns +`odoPose = (13.00, 0.94, **0.00°**)` ← **ROTATION DESTROYED**. + +Here's why: `odometry.resetPosition(gyroAngle=271.82°, ..., pose=271.82°)` +stored internally that "at reset time, gyro was 271.82°". The gyro offset is +271.82° − 271.82° = 0°. Now `update(yaw=0°)` computes: +heading = 271.82° + (0° − 271.82°) = **0°**. The estimator faithfully applied +the delta: the gyro "changed" by −271.82°, so the heading changed by −271.82°. + +**TRACE-6965–6967**: `periodic()` continues — `getPose()` returns +(13.00, 0.94, **0.00°**) twice (for `field.setRobotPose` and `robotPose.set`), +then exits. + +**TRACE-6968**: `getPose()` returns (13.00, 0.94, **0.00°**) — this is +**PathPlanner calling `getPose()` during `FollowPathCommand.initialize()`** +(happens during `CommandScheduler.run()`, after `periodic()` exits). + +**TRACE-6969**: `getChassisSpeeds()` returns (0, 0, 0) — PathPlanner reading +current speeds during initialization. + +**TRACE-6970**: `robotPeriodic()` exits. + +**TRACE-6971–6979**: `simulationPeriodic()` runs AFTER `robotPeriodic()`: +- SimulationManager consumes the pending pose reset → sets `simPose` to + (13.00, 0.94, 271.82°) +- Calls `pigeonSimState.setRawYaw(-88.18°)` (271.82° normalized to [-180, 180]) +- This rawYaw won't be visible to `getYaw()` until a future cycle due to CTRE + sim state latency + +**CRITICAL**: `simulationPeriodic()` runs **too late**. By the time it updates +the pigeon sim state, `periodic()` has already read the stale 0° yaw and +corrupted the odometry, and PathPlanner has already initialized with the wrong +heading. + +--- + +### AUTONOMOUS Cycle 2 — PathPlanner Executes and Finishes (TRACE-6980 → TRACE-6998) + +**TRACE-6981**: `periodic()` — odoPose before update = +(13.00, 0.94, **0.00°**) (corrupted last cycle). + +**TRACE-6982**: `odometry.update(yaw=0.00°, ...)` — pigeon STILL reads 0°. +CTRE sim state change from cycle 1 hasn't propagated yet. + +**TRACE-6983**: odoPose after update = (13.00, 0.94, **0.00°**) — unchanged. + +**TRACE-6987**: `getPose()` → (13.00, 0.94, **0.00°**) — **PathPlanner sees +heading 0°** during its `execute()`. + +**TRACE-6988**: `getChassisSpeeds()` → (0, 0, 0) — robot is stationary. + +**TRACE-6989**: `driveFromChassisSpeeds(vx=7.136, vy=7.647, omega=-10.064, +openLoop=false)` — **PathPlanner commands enormous speeds**. With a 271.82° +heading error, PathPlanner's PID controllers generate massive corrective outputs. +The speeds are saturated at the robot's physical limits. + +**TRACE-6990**: `driveFromChassisSpeeds(vx=0, vy=0, omega=0, openLoop=false)` — +**PathPlanner commands zero immediately after**. This is +`FollowPathCommand.end()` being called. The command finished +(`isFinished()` returned true) after a single execute cycle. The +`openLoop=false` confirms it's still PathPlanner (not TeleopSwerve). + +**TRACE-6991**: `robotPeriodic()` exits. PathPlanner is done. +FollowPathCommand ran for exactly **1 execute cycle**. + +**TRACE-6992–6998**: SimulationManager runs, sets pigeon to -88.18° again. +Still no visible effect on getYaw(). + +--- + +### AUTONOMOUS Cycle 3 — TeleopSwerve Takes Over (TRACE-6999 → TRACE-7014) + +**TRACE-7001**: `odometry.update(yaw=183.64°, ...)` — **NOW the pigeon +responds**, but with the wrong value! The pigeon reads 183.64° because of the +doubling bug: +- `pigeon.setYaw(271.82)` created offset = +271.82° +- `pigeonSimState.setRawYaw(-88.18)` set rawYaw = −88.18° +- Reported yaw = −88.18° + 271.82° = **183.64°** (should be 271.82° ≡ −88.18°) + +**TRACE-7002**: odoPose = (13.00, 0.94, **−176.36°**) — the sudden 183.64° jump +from the stale 0° baseline. + +**TRACE-7006**: `driveFromChassisSpeeds(vx=0, vy=0, omega=0, openLoop=true)` — +`openLoop=true` confirms **TeleopSwerve is now the active command**. PathPlanner +is gone. The robot sits still. + +--- + +### AUTONOMOUS Cycle 4 — Steady State (TRACE-7015 → TRACE-7030) + +Pigeon reads 183.64° (unchanged — no robot motion), odoPose is −176.36°. +TeleopSwerve continues commanding zero. The robot is stuck. + +--- + +## 2. Root Cause Identification + +There are **two interacting bugs**, with the first being the primary cause of the +instant termination: + +### Primary Bug: `hardResetPose` passes the wrong `gyroAngle` to `odometry.resetPosition()` + +In `SwerveSubsystem.java` line 140: + +```java +Rotation2d gyroAngle = updatePigeon ? newPose.getRotation() : getYaw(); +``` + +When `updatePigeon=true`, the code passes `newPose.getRotation()` (the +**desired** heading, 271.82°) instead of `getYaw()` (the **actual** pigeon +reading, 0°) as the `gyroAngle` parameter. + +`SwerveDrivePoseEstimator.resetPosition(gyroAngle, positions, pose)` records the +gyroAngle as the baseline for future delta calculations. It computes internally: + +``` +gyroOffset = pose.rotation − gyroAngle +``` + +When gyroAngle equals the pose rotation, offset = 0°. + +Then `update(currentYaw, ...)` computes: + +``` +heading = pose.rotation + (currentYaw − storedGyroAngle) +``` + +With storedGyroAngle=271.82° and currentYaw=0°: + +``` +heading = 271.82° + (0° − 271.82°) = 0° +``` + +**The odometry interprets the 271.82° difference between the stored baseline and +the actual pigeon as the robot having rotated −271.82° since the reset**, and +"helpfully" adjusts the heading to 0°. + +#### Chain of Causation + +1. `pigeon.setYaw(271.82)` does not take immediate effect in CTRE simulation → + `getYaw()` returns 0° +2. `odometry.resetPosition(gyroAngle=271.82°, ...)` stores 271.82° as baseline, + even though the actual gyro reads 0° +3. First `periodic()` calls `odometry.update(yaw=0°)` → heading snaps from + 271.82° to **0°** +4. PathPlanner's `FollowPathCommand` initializes with `getPose()` returning + heading **0°** instead of **271.82°** +5. With 271.82° heading error, PathPlanner computes a trajectory that is either + instant or has a vanishingly short duration +6. FollowPathCommand finishes after 1 execute cycle; TeleopSwerve takes over + +### Secondary Bug: Pigeon yaw doubling in simulation + +Even if the primary bug were fixed, the simulation-specific yaw doubling would +cause problems during path following: + +- `pigeon.setYaw(271.82)` creates an internal offset of +271.82° +- `SimulationManager` calls `pigeonSimState.setRawYaw(-88.18°)` +- Reported yaw = −88.18° + 271.82° = **183.64°**, instead of the correct + −88.18° (≡ 271.82°) + +This would corrupt the heading by ~88° once the sim state propagates (cycle 3+), +causing path-following errors. + +--- + +## 3. Execution Order Diagram + +Within a single robot loop iteration (one call to `TimedRobot`'s main loop), the +actual execution order is: + +``` +╔══════════════════════════════════════════════════════════════════════╗ +║ AUTONOMOUS CYCLE 1 ║ +║ (first robotPeriodic after autonomousInit) ║ +╠══════════════════════════════════════════════════════════════════════╣ +║ ║ +║ 1. robotPeriodic() ENTERS ║ +║ │ ║ +║ └─▶ 2. CommandScheduler.run() ║ +║ │ ║ +║ ├─▶ 3. SwerveSubsystem.periodic() ║ +║ │ │ ║ +║ │ ├─▶ getYaw() → 0.00° ← STALE! pigeon.setYaw ║ +║ │ │ hasn't taken effect ║ +║ │ │ ║ +║ │ ├─▶ odometry.update(yaw=0°, positions=[0,0,0,0]) ║ +║ │ │ └── odoPose: 271.82° → 0.00° ✗ CORRUPTED ║ +║ │ │ ║ +║ │ └── EXIT ║ +║ │ ║ +║ └─▶ 4. SequentialCommandGroup.execute() ║ +║ │ ║ +║ ├── InstantCommand.execute() (no-op, already ran) ║ +║ ├── InstantCommand.isFinished() → true ║ +║ ├── Advance to FollowPathCommand ║ +║ │ ║ +║ └─▶ FollowPathCommand.initialize() ║ +║ ├── getPose() → (13.00, 0.94, 0.00°) ✗ ║ +║ └── getChassisSpeeds() → (0, 0, 0) ║ +║ (Trajectory computed with WRONG heading) ║ +║ ║ +║ 5. robotPeriodic() EXITS ║ +║ ║ +║ 6. simulationPeriodic() ║ +║ │ ║ +║ └─▶ SimulationManager.simulationPeriodic() ║ +║ ├── consumes pendingSimPoseReset ║ +║ │ └── simPose = (13.00, 0.94, 271.82°) ║ +║ ├── pigeonSimState.setRawYaw(-88.18°) ║ +║ │ └── ⚠ TOO LATE! periodic() already read 0° ║ +║ └── EXIT ║ +║ ║ +╠══════════════════════════════════════════════════════════════════════╣ +║ AUTONOMOUS CYCLE 2 ║ +╠══════════════════════════════════════════════════════════════════════╣ +║ ║ +║ 1. robotPeriodic() ENTERS ║ +║ └─▶ CommandScheduler.run() ║ +║ ├─▶ periodic() — yaw STILL 0° (sim state lag) ║ +║ │ └── odoPose remains (13.00, 0.94, 0°) ║ +║ │ ║ +║ └─▶ FollowPathCommand.execute() ║ +║ ├── getPose() → (13.00, 0.94, 0°) ✗ ║ +║ ├── getChassisSpeeds() → (0, 0, 0) ║ +║ ├── driveFromChassisSpeeds(7.1, 7.6, -10.1) ← !! ║ +║ ├── isFinished() → TRUE (trajectory complete) ║ +║ └── end() → driveFromChassisSpeeds(0, 0, 0) ║ +║ ║ +║ 2. simulationPeriodic() — setRawYaw(-88.18) again ║ +║ ║ +╠══════════════════════════════════════════════════════════════════════╣ +║ AUTONOMOUS CYCLE 3+ ║ +╠══════════════════════════════════════════════════════════════════════╣ +║ ║ +║ TeleopSwerve (default command) now active. ║ +║ Pigeon yaw finally changes to 183.64° (doubled). ║ +║ Robot sits still. PathPlanner is gone. ║ +║ ║ +╚══════════════════════════════════════════════════════════════════════╝ +``` + +**The core timing problem**: `periodic()` runs BEFORE `CommandScheduler` +executes the PathPlanner command, and `simulationPeriodic()` runs AFTER both. So +the pigeon sim state written by `SimulationManager` is never visible until at +least 1 cycle later (plus CTRE's own sim state propagation delay, adding +potentially 1 more cycle). + +--- + +## 4. Proposed Fix + +### Fix A (Production Code): Always use actual gyro reading in `odometry.resetPosition()` + +In `SwerveSubsystem.hardResetPose` (line 140): + +**Before:** +```java +Rotation2d gyroAngle = updatePigeon ? newPose.getRotation() : getYaw(); +``` + +**After:** +```java +Rotation2d gyroAngle = getYaw(); +``` + +**Why this works**: Instead of telling the estimator "the gyro currently reads +271.82°" (a lie — it reads 0°), we tell it the truth: "the gyro reads 0° and the +robot is at 271.82°." The estimator computes: + +``` +offset = 271.82° − 0° = 271.82° +``` + +When the next `update(yaw=0°)` arrives, it correctly calculates: + +``` +heading = 0° + 271.82° = 271.82° ✓ +``` + +**Why this is safe on real hardware**: On a real Pigeon2, `setYaw()` may or may +not take immediate effect. If it does, `getYaw()` returns 271.82° → offset = 0° +→ same result as current code. If it doesn't, `getYaw()` returns the old value → +offset compensates → correct result. The fix is **more correct** than the current +code in all cases. + +**No simulation-specific branching**: This is a pure logic fix that removes an +incorrect assumption (that `setYaw` takes immediate effect). It improves +production code correctness. + +### Fix B (SimulationManager): Eliminate pigeon yaw doubling + +In `SimulationManager.simulationPeriodic()`, when consuming a pose reset, also +**zero out the pigeon offset** by writing the raw yaw that produces the correct +reported yaw. Currently `pigeon.setYaw()` creates an offset, and then +`setRawYaw()` sets a raw value — the two add up (doubling). + +The cleanest approach: when SimulationManager sets the pigeon sim state after a +pose reset, it should also clear the offset created by `pigeon.setYaw()`. Add +this after consuming the reset: + +```java +if (reset != null) { + this.simPose = reset; + // Clear the offset created by hardResetPose's pigeon.setYaw() call. + // In CTRE simulation, setYaw() creates an internal offset and setRawYaw() + // sets the raw value; reported yaw = rawYaw + offset. To prevent doubling, + // we set rawYaw to 0 first (so setYaw's offset targets 0), then overwrite + // rawYaw with the correct heading. + pigeonSimState.setRawYaw(0); + swerveSubsystem.getPigeon().setYaw(0); // clears the offset to 0 + pigeonSimState.setRawYaw(simPose.getRotation().getDegrees()); // real value +} +``` + +**Alternative simpler approach**: Since Fix A makes odometry robust against stale +gyro readings, the doubling is less catastrophic — the offset will compensate. +However, the reported heading will still be wrong (183.64° instead of 271.82°), +which would cause PathPlanner to compute incorrect corrections during path +following. So Fix B is recommended for correct ongoing simulation. + +### Fix Priority + +**Fix A alone solves the instant-termination bug.** PathPlanner will initialize +with the correct heading and compute a valid trajectory. Even with the pigeon +doubling (Fix B not applied), the odometry offset would partially compensate +during the first couple cycles while the pigeon value is still stale (0°), giving +PathPlanner correct poses initially. + +Fix B is needed for correct long-term path following in simulation. + +Both fixes respect the design constraint: **Fix A improves production code +without any simulation branching; Fix B modifies only `SimulationManager`.** + +--- + +## Answers to Specific Analysis Questions + +### Q1: What is the exact sequence of method calls from `autonomousInit` through the first few `robotPeriodic` cycles? + +See the cycle-by-cycle narrative above. The key sequence is: + +1. `autonomousInit()` → `getAutonomousCommand()` → `EightLemonAuto()` constructor + → `startAutoAt()` + `autoDrive()` → `schedule()` → lambda fires → + `hardResetPose()` → odometry set to 271.82° +2. First `robotPeriodic()` → `periodic()` → `odometry.update(yaw=0°)` → heading + snaps to 0° → PathPlanner `initialize()` sees 0° +3. Second `robotPeriodic()` → `periodic()` → still 0° → PathPlanner `execute()` + → commands 7+ m/s → `isFinished()` → true → `end()` +4. Third `robotPeriodic()` → TeleopSwerve active → robot stationary + +### Q2: When does `startAutoAt`'s lambda actually execute? + +**During `autonomousInit()`**, specifically inside `CommandScheduler.schedule()`. +It fires BEFORE any `robotPeriodic()` call. The `schedule()` call triggers +`SequentialCommandGroup.initialize()` → `InstantCommand.initialize()` → +lambda runs. This was confirmed by the lambda executing at TRACE-6951, between +the schedule call (6950) and `autonomousInit EXIT` (6960). + +### Q3: What does `pigeon.getYaw()` return at each point? + +| When | pigeon.getYaw() | Why | +|------|-----------------|-----| +| Before `pigeon.setYaw(271.82)` | 0.0° | Initial value | +| Immediately after `pigeon.setYaw(271.82)` | **0.0°** | setYaw is deferred in CTRE sim | +| Cycle 1 `periodic()` | **0.0°** | Still deferred | +| Cycle 2 `periodic()` | **0.0°** | Still deferred (1-cycle CTRE lag) | +| Cycle 3 `periodic()` | **183.64°** | Finally visible, but DOUBLED | + +`pigeon.setYaw()` does NOT take immediate effect. There is a minimum 2-cycle +delay before the pigeon reports any change. When it does report, the value is +wrong due to offset doubling (183.64° instead of 271.82°). + +### Q4: What does `odometry.getEstimatedPosition()` return before and after each `update()` and `resetPosition()` call? + +| Event | odoPose | +|-------|---------| +| Before `resetPosition()` | (0.00, 0.00, 0.00°) | +| After `resetPosition(gyro=271.82°, pose=271.82°)` | (13.00, 0.94, **271.82°**) ✓ | +| Cycle 1: before `update()` | (13.00, 0.94, **271.82°**) ✓ | +| Cycle 1: after `update(yaw=0°)` | (13.00, 0.94, **0.00°**) ✗ | +| Cycle 2: after `update(yaw=0°)` | (13.00, 0.94, **0.00°**) ✗ | +| Cycle 3: after `update(yaw=183.64°)` | (13.00, 0.94, **−176.36°**) ✗ | + +### Q5: What values does PathPlanner see when it calls `getPose()` and `getChassisSpeeds()`? + +- **Cycle 1** (initialize): `getPose()` = (13.00, 0.94, **0.00°**), + `getChassisSpeeds()` = (0, 0, 0) +- **Cycle 2** (execute): `getPose()` = (13.00, 0.94, **0.00°**), + `getChassisSpeeds()` = (0, 0, 0) + +PathPlanner never sees the correct 271.82° heading. + +### Q6: What speeds does PathPlanner command via `driveFromChassisSpeeds()`? + +- **Cycle 2**: vx=7.136, vy=7.647, omega=−10.064 (openLoop=false) — one + enormous burst +- **Cycle 2**: vx=0, vy=0, omega=0 (openLoop=false) — `end()` zeros + +PathPlanner commands non-zero speeds for exactly **1 execute cycle** before +finishing. From cycle 3 onward, `openLoop=true` indicates TeleopSwerve. + +### Q7: When does `SimulationManager.simulationPeriodic()` run relative to everything else? + +It runs **AFTER** `robotPeriodic()` (which contains `CommandScheduler.run()`). +The WPILib `TimedRobot` lifecycle order is: + +``` +robotPeriodic() → autonomousPeriodic() → simulationPeriodic() +``` + +This means pigeon sim state written by SimulationManager is not visible until +the NEXT cycle's `periodic()` — and even then, CTRE's sim state may add an +additional cycle of latency. + +### Q8: Where exactly do 3rd-party library calls happen, and what are their inputs/outputs? + +| 3rd-party call | Location | Inputs | Output | +|---------------|----------|--------|--------| +| `pigeon.setYaw(271.82)` | hardResetPose, cycle 0 | 271.82° | No immediate effect (yaw stays 0°) | +| `odometry.resetPosition(...)` | hardResetPose, cycle 0 | gyro=271.82°, pose=271.82° | odoPose = (13, 0.94, 271.82°) | +| `odometry.update(...)` | periodic, cycle 1 | yaw=0°, pos=[0,0,0,0] | odoPose = (13, 0.94, **0°**) | +| `odometry.update(...)` | periodic, cycle 2 | yaw=0°, pos=[0,0,0,0] | odoPose = (13, 0.94, 0°) | +| `pigeonSimState.setRawYaw(...)` | simPeriodic, cycle 1 | -88.18° | Written to sim state | +| `odometry.update(...)` | periodic, cycle 3 | yaw=183.64°, pos=[varied] | odoPose = (13, 0.94, −176.36°) | + +--- + +## Key Mystery Resolved + +From `auto-sim-analysis.md`, Section "What We Don't Know": + +> **Why is `odoPose` rotation 0° right after `hardResetPose` set it to 271.82°?** + +**Answer**: Because `hardResetPose` passes `newPose.getRotation()` (271.82°) as +the `gyroAngle` to `odometry.resetPosition()`, but the pigeon's actual yaw is +0°. On the very next `periodic()` call, `odometry.update(yaw=0°)` computes a +−271.82° heading delta and snaps the rotation to 0°. + +> **Why does `FollowPathCommand` finish after exactly 1 execute cycle?** + +**Answer**: PathPlanner initializes with heading 0° instead of 271.82°. It +executes once, commands enormous speeds (7+ m/s, 10+ rad/s), then `isFinished()` +returns true. The robot **does not actually move** — the translation remains +(13.00, 0.94) throughout. The path is NOT completed; PathPlanner terminates +prematurely. + +We cannot determine the exact internal mechanism without instrumenting +PathPlanner itself. The most likely explanation is that +`path.generateTrajectory()`, called during `initialize()` with the wildly wrong +starting heading (0° vs 271.82°), produces a **degenerate trajectory with zero +or near-zero total time**. PathPlanner's `isFinished()` is typically +`timer.hasElapsed(trajectory.getTotalTimeSeconds())` — if the total time is ≈ 0, +this returns true on the first check. The enormous speed command in the single +`execute()` cycle is PathPlanner's computed output for that degenerate +trajectory, but it is immediately overwritten by `end()` zeroing the motors +before `SimulationManager` can integrate any motion. + +--- + +## 5. Follow-up Analysis: `sim-output2.log` (with `TracedCommand` instrumentation) + +### 5.1 What the New Log Confirmed + +After adding `TracedCommand` wrappers around both `startAutoAt` and +`autoDrive[8FuelPath]`, the second simulation run (`sim-output2.log`) provided +definitive lifecycle evidence: + +1. **`startAutoAt` lifecycle** — works as expected: + - `initialize()` fires during `autonomousInit()` (cycle 0), executing the + `hardResetPose` lambda + - `execute()` is a no-op (cycle 1) + - `isFinished()` = true (cycle 1) + - `end(interrupted=false)` (cycle 1) + +2. **`autoDrive[8FuelPath]` lifecycle** — confirmed: **exactly ONE `execute()` + cycle, then `isFinished()=true`**: + - `initialize()` fires in cycle 1 (same `CommandScheduler.run()` where + `startAutoAt` finishes) + - `execute()` fires in cycle 2, commanding vx=-7.417, vy=7.375, omega=-2.292 + - `isFinished()` = **true** in cycle 2 + - `end(interrupted=false)` in cycle 2, zeroing speeds to (0, 0, 0) + +3. **Robot final position**: The robot **stays at the path START** (13.00, 0.94). + It does NOT move. The enormous speed command persists for a single 20ms + cycle, but `end()` zeros the motors before `SimulationManager.simulationPeriodic()` + can integrate any motion. The `desiredVx`/`desiredVy`/`desiredOmega` seen by + SimulationManager are all 0.000. + +### 5.2 What Was Ruled Out + +#### Alliance flip mismatch — RULED OUT + +PathPlanner 2026.1.2's `FlippingUtil` defaults to `FieldSymmetry.kRotational` +(confirmed by reading the library source from the Gradle cache JAR). The +kRotational flip is: +- Position: `(fieldSizeX − x, fieldSizeY − y)` +- Rotation: `rotation − 180°` (equivalent to `rotation + 180°`) + +This is **identical** to the team's `FieldConstants.flipForAlliance()`: +- Position: `(FIELD_LENGTH − x, FIELD_WIDTH − y)` +- Rotation: `rotation + 180°` + +There is no double-flip or coordinate mismatch between where `startAutoAt` +places the robot and where PathPlanner expects the path to start. + +#### Heading corruption at PathPlanner initialization — NON-DETERMINISTIC + +**Important**: The pigeon `setYaw()` latency is **non-deterministic** between +simulation runs. In `sim-output.log`, the pigeon yaw was still 0° in cycles 1 +and 2, corrupting the heading. In `sim-output2.log`, the pigeon yaw offset +**takes effect by cycle 1**. This means +`odometry.update(yaw=271.82°)` sees the same yaw that `resetPosition()` stored, +so the heading is **preserved as -88.18°** (≡ 271.82°): + +``` +[TRACE-7160] odometry.update(yaw=Rotation2d(Deg: 271.82), ...) → odoPose=(..., Deg: -88.18) +``` + +PathPlanner initializes in cycle 1 with `getPose()` returning +**(13.00, 0.94, -88.18°)** — the **correct** heading. Velocity is (0, 0, 0). +The flipped path's `idealStartingState` is also rotation=-88.18°, velocity=0. +**Both match**, so PathPlanner uses its pre-computed ideal trajectory. + +**Yet PathPlanner STILL finishes after 1 execute cycle.** This means the +heading corruption identified in the initial analysis (Section 2) is NOT the +only cause of the instant termination. Something else is wrong. + +#### Pigeon yaw doubling — confirmed but happens AFTER auto finishes + +The yaw corruption (271.82° → 183.64°) first appears in cycle 3: +``` +[TRACE-7210] odometry.update(yaw=Rotation2d(Deg: 183.64), ...) → odoPose=(..., Deg: -176.36) +``` +By this point, `autoDrive` has already finished (cycle 2). The doubling is a +real bug that would affect longer-running paths, but it is not responsible for +the immediate termination observed here. + +### 5.3 PathPlanner `isFinished()` — Source Code Analysis + +Reading `FollowPathCommand` source from the PathPlanner 2026.1.2 JAR: + +```java +// FollowPathCommand.java (3rd-party — com.pathplanner.lib.commands) +@Override +public boolean isFinished() { + double totalTime = trajectory.getTotalTimeSeconds(); + return timer.hasElapsed(totalTime) || !Double.isFinite(totalTime); +} +``` + +Two conditions cause termination: +1. **`timer.hasElapsed(totalTime)`** — the internal timer (started in + `initialize()`) has exceeded the trajectory's total duration +2. **`!Double.isFinite(totalTime)`** — the trajectory's total time is `NaN` or + `Infinity` + +Both `trajectory` and `timer` are **private fields** of `FollowPathCommand` +(a 3rd-party class). We cannot access them from our `TracedCommand` wrapper. + +To determine which condition triggers, we added elapsed-time tracking to +`TracedCommand`: it records `Timer.getFPGATimestamp()` at `initialize()` and +logs the elapsed time at each `isFinished()` call. If `isFinished()=true` with +elapsed time ≈ 0.02s, then either `totalTime ≤ 0.02` or `totalTime` is +non-finite. + +### 5.4 Reverse-Engineering the Target State + +PathPlanner's `FollowPathCommand.execute()` works as follows: + +```java +double currentTime = timer.get(); // elapsed since initialize +var targetState = trajectory.sample(currentTime); // "where should robot be at t?" +ChassisSpeeds targetSpeeds = controller.calculateRobotRelativeSpeeds(currentPose, targetState); +output.accept(targetSpeeds, targetState.feedforwards); // → driveFromChassisSpeeds +``` + +`trajectory.sample(t)` returns the trajectory state at time `t`. If +`t >= totalTime`, it returns the **end state** (final destination). The +controller then computes PID feedback = `kP × (target − current)` with kP=5.0. + +From the trace output `driveFromChassisSpeeds(vx=-7.417, vy=7.375, ...)`, we +can reverse-engineer the target position by converting robot-relative speeds +back to field-relative, then dividing by kP: + +| | Robot (current) | Implied target | PID feedback (kP=5.0) | +|---|---|---|---| +| X | 13.00 m | ~14.43 m | 5.0 × 1.43 ≈ 7.15 | +| Y | 0.94 m | ~2.47 m | 5.0 × 1.53 ≈ 7.65 | + +The implied target **(14.43, 2.47)** matches the **end point** of the flipped +8FuelPath. The path file `8FuelPath.path` defines: +- Blue start: (3.5355, 7.1300) → flipped red: **(13.00, 0.94)** +- Blue end: (2.1083, 5.6006) → flipped red: **(14.43, 2.47)** + +This confirms `trajectory.sample()` returned the end state, meaning the timer +had already exceeded the trajectory's total time after just ~20ms. + +> **Note on `trajectory.sample()` timing**: The `timer.get()` value of ~0.02s +> represents actual elapsed FPGA time between `initialize()` (cycle 1) and +> `execute()` (cycle 2). This is real wall-clock time, not a hardcoded value. +> (The project's `SimulationManager` similarly uses actual elapsed time via +> `Timer.getFPGATimestamp()` rather than assuming a fixed 0.02s per cycle.) + +### 5.5 Path File vs. Trajectory — What Can and Cannot Be Read From `8FuelPath.path` + +The path file (`src/main/deploy/pathplanner/paths/8FuelPath.path`) contains: + +| Data | Available? | Value | +|------|-----------|-------| +| Start position (anchor) | ✅ Yes | (3.5355, 7.1300) | +| End position (anchor) | ✅ Yes | (2.1083, 5.6006) | +| Bézier control points | ✅ Yes | nextControl/prevControl for each waypoint | +| Constraints (max vel/accel) | ✅ Yes | maxVelocity=3.0 m/s, maxAcceleration=3.0 m/s² | +| Ideal starting state | ✅ Yes | velocity=0, rotation=91.82° | +| Goal end state | ✅ Yes | velocity=0, rotation=65.82° | +| **Trajectory total time** | ❌ **No** | Computed at runtime by PathPlanner | +| **Trajectory states** | ❌ **No** | Computed at runtime by PathPlanner | + +The trajectory (time-parameterized sequence of poses, velocities, and +accelerations) is **generated at runtime** by PathPlanner using the path +geometry + constraints + `RobotConfig`. The method +`PathPlannerPath.generateTrajectory(startingSpeeds, startingRotation, config)` +is public and can be called directly to inspect the generated trajectory. + +### 5.6 Trajectory Trace Instrumentation Added + +To answer the remaining mystery, we added trace output in `autoDrive()` that +calls `PathPlannerPath.generateTrajectory()` with the same inputs PathPlanner +would use (flipped path, ideal starting state, `ROBOT_CONFIG`). This logs: +- `totalTime` and whether it is finite +- Number of trajectory states +- Sampled positions at 0%, 25%, 50%, 75%, and 100% of the trajectory + +We also added elapsed-time tracking to `TracedCommand.isFinished()` to log how +much real time has passed since `initialize()` when `isFinished()` returns true. + +**These traces will appear in the next simulation run.** The trajectory trace +in `autoDrive()` logs the actual `totalTime` value and `Double.isFinite(totalTime)` +at path construction time — so we will see the exact value (e.g. `totalTime=NaN +isFinite=false` or `totalTime=0.003s isFinite=true`) immediately in the log, +without needing to wait for the command to run. + +### 5.7 Remaining Open Question (answered in Section 6) + +**Why does the pre-computed ideal trajectory appear to have near-zero total +time?** The 8FuelPath covers ~2.1 meters (computed from the two anchor points) +with maxVelocity=3.0 m/s and maxAcceleration=3.0 m/s². A normal trajectory +should take approximately 1.7 seconds. The trajectory generation uses the +project's `AutoConstants.ROBOT_CONFIG` (mass=52kg, MOI=6.8, NEO Vortex motors, +6.75:1 gearing, 0.0508m wheel radius). These values appear reasonable. + +**This question is definitively answered in Section 6 below.** + +--- + +## 6. Definitive Root Cause: `ROBOT_CONFIG` Parameter Mismatch + +### 6.1 Background: DC Motor Physics + +This section explains the physics concepts needed to understand the bug. + +#### Key Units + +| Unit | Name | Measures | +|------|------|----------| +| **Nm** | Newton-meters | Torque (rotational force) | +| **A** | Amps | Electrical current | +| **V** | Volts | Electrical potential | +| **Ω** | Ohms | Electrical resistance | +| **rad/s** | Radians per second | Rotational speed | +| **m/s** | Meters per second | Linear speed | + +**Ohm's Law**: `V = I × R` (volts = current × resistance), or equivalently +`I = V / R`. + +**12V** is the nominal voltage produced by the robot's battery. + +#### Motor Terminology + +| Term | Definition | +|------|-----------| +| **Stall torque** | The **maximum** torque a motor can produce. It occurs when the shaft is held still **by a load** (0 RPM) — the motor is pushing as hard as it can but something is preventing it from spinning. Think of pedaling a bike uphill: you push hardest on the pedals when going nearly zero speed. As you pedal faster, you can push with less force. "Stall" means "the shaft wants to spin but can't because the load is too heavy." For our geared NEO Vortex: **24.3 Nm**. | +| **Stall current** | The current the motor draws when stalled (shaft held still by a load). This is the **maximum** current the motor ever draws — because there is no back-EMF to resist it (see below). For our NEO Vortex: **211 A**. | +| **Free speed** | The maximum speed the motor reaches when spinning with NO load (nothing attached to the shaft). The motor is spinning as fast as it can but producing zero useful torque. For the raw NEO Vortex motor, REV Robotics publishes a free speed of ~6784 RPM (≈710 rad/s). After our 6.75:1 gear reduction: **710 / 6.75 = 105.2 rad/s** at the output shaft. Converting to robot speed: **105.2 × 0.0508 m (wheel radius) = 5.35 m/s**. | +| **Free current** | The current drawn at free speed — just enough to overcome internal friction. This is the **minimum** current. Published by REV Robotics for the NEO Vortex: **3.6 A**. | + +> **Where do these numbers come from?** The raw motor specs (stall torque, +> stall current, free speed, free current) are published by the motor +> manufacturer (REV Robotics for the NEO Vortex). The geared values are +> calculated by applying our 6.75:1 gear ratio. The robot speed is calculated +> from the geared motor speed × wheel radius. The motor resistance is derived +> from Ohm's Law: `R = 12V / 211A = 0.057 Ω` (it is not typically published +> directly by the manufacturer). + +#### Back-EMF: Why Faster Motors Draw Less Current + +**EMF** stands for **Electromotive Force** (not electromagnetic field). It is +measured in volts. + +When a motor spins, its spinning magnets act as a **generator**, producing a +voltage that **opposes** the applied battery voltage. This opposing voltage is +called **back-EMF**. The faster the motor spins, the more back-EMF it +generates. + +**The current flowing through the motor** depends on the difference between +the battery voltage and the back-EMF: + +``` + Battery Voltage − Back-EMF +Motor Current = ───────────────────────────────── + Motor Resistance +``` + +Or in symbols: `I = (V − back_EMF) / R` + +- **At stall** (0 speed): back-EMF = 0V → current = 12V / 0.057Ω = **211A** (maximum) +- **At free speed** (105.2 rad/s): back-EMF = 12V − 3.6A × 0.057Ω = **11.795V** + → current = (12V − 11.795V) / 0.057Ω = **3.6A** (minimum) +- **At any speed in between**: back-EMF is proportional to speed, so current + is somewhere between 3.6A and 211A + +**This is counter-intuitive**: slower speeds draw MORE current, not less. +A stalled motor is basically a short circuit across the battery. + +#### Kv and Kt: The Two Motor Constants + +**Kv (velocity constant)** relates motor speed to back-EMF voltage: + +``` +back-EMF = motor_speed / Kv +``` + +Measured in **rad/s per volt**: "how many rad/s does the motor spin per volt +of back-EMF?" We can derive Kv from the free-speed conditions. At free speed, +we know the current (3.6A, published by REV), the voltage (12V, battery), and +the geared speed (710 rad/s published by REV ÷ 6.75 gear ratio = 105.2 rad/s): + +``` +At free speed: back-EMF = batteryVoltage − freeCurrent × resistance + = 12V − 3.6A × 0.057Ω = 11.795V + +Since back-EMF = motor_speed / Kv: + Kv = motor_speed / back-EMF + = 105.2 / 11.795 + = 8.92 rad/s/V +``` + +Substituting back into the current formula: + +``` + 12V − (motor_speed / 8.92) +Motor Current = ────────────────────────────────── + 0.057 Ω +``` + +**Kt (torque constant)** relates current to torque: + +``` +torque = Kt × current +``` + +Measured in **Nm per amp**: "how much torque does the motor produce per amp +of current?" Derived from stall conditions (maximum torque at maximum current): + +``` +Kt = stallTorque / stallCurrent = 24.3 Nm / 211 A = 0.1152 Nm/A +``` + +#### `getCurrent()` and `getTorque()` — Real WPILib Methods + +These are actual methods on the `DCMotor` class in WPILib. They implement +the physics formulas described above: + +| Method | What it computes | Formula | +|--------|-----------------|---------| +| `getCurrent(speed, voltage)` | "At `speed` rad/s with `voltage` applied, how many amps?" | `I = (V − speed/Kv) / R` | +| `getTorque(current)` | "At `current` amps, how much torque?" | `torque = Kt × current` | + +#### Speed vs. Current Table for Our Motor + +| Motor speed | Robot speed | Current at 12V | Torque | +|---|---|---|---| +| 0 rad/s (**stalled**) | 0 m/s | **211 A** | 24.3 Nm | +| 59.1 rad/s | **3.0 m/s** (our maxSpeed) | **94.6 A** | 10.9 Nm | +| 86.7 rad/s | 4.41 m/s | **40 A** (our current limit) | 4.6 Nm | +| 105.2 rad/s (**free**) | 5.35 m/s | **3.6 A** | 0.4 Nm | + +### 6.2 What is `ModuleConfig`? + +`ModuleConfig` is PathPlanner's representation of one swerve drive module — +the physical properties of the wheel + motor + gearbox combination. Our code +creates it in `Constants.java`: + +```java +public static final ModuleConfig MODULE_CONFIG = new ModuleConfig( + SwerveConstants.wheelDiameter / 2, // wheelRadius = 0.0508 m (2 inches) + SwerveConstants.maxSpeed, // maxDriveVelocityMPS = 3.0 m/s + 1.2, // wheelCOF (coefficient of friction) + DCMotor.getNeoVortex(1).withReduction(6.75), // drive motor with gearbox + SwerveConstants.driveContinuousCurrentLimit, // driveCurrentLimit = 40 A + 1); // 1 motor per module +``` + +PathPlanner uses `ModuleConfig` to answer physics questions like "how much +force can this module exert on the carpet?" during trajectory generation. + +### 6.3 What is `torqueLoss`? + +**`torqueLoss`** represents the torque consumed by friction and inefficiency — +the torque the motor must produce just to keep the wheels spinning, with +nothing left over for acceleration or deceleration. + +PathPlanner computes it inside the `ModuleConfig` constructor: + +```java +// How fast do the wheels spin at maxDriveVelocityMPS? +maxDriveVelocityRadPerSec = maxDriveVelocityMPS / wheelRadiusMeters; +// = 3.0 / 0.0508 = 59.05 rad/s + +// How much current does the motor draw at that speed, at 12V? +maxSpeedCurrentDraw = driveMotor.getCurrent(59.05, 12.0); +// = 94.63 A + +// Clamp to the current limit: +clampedCurrent = Math.min(94.63, 40.0); +// = 40.0 A ← THIS IS THE PROBLEM + +// torqueLoss = the torque at the clamped current: +torqueLoss = driveMotor.getTorque(40.0); +// = 0.1152 × 40 = 4.607 Nm +``` + +### 6.4 What is a Trajectory "State"? + +A PathPlanner **trajectory** is a time-ordered list of **states**. Each state +is a snapshot answering: "At time T seconds, where should the robot be and how +fast should it be moving?" + +| State field | Meaning | +|---|---| +| `timeSeconds` | When this state occurs (seconds from start) | +| `pose` | Where the robot should be (x, y, heading) | +| `linearVelocity` | How fast the robot should be moving (m/s) | +| `heading` | Direction of travel | +| `fieldSpeeds` | Velocity broken into vx, vy, omega components | + +For example, a healthy 8FuelPath trajectory has 12 states: + +``` +state[ 0] t=0.000s pose=(3.54, 7.13) vel=0.000 m/s ← start (stopped) +state[ 1] t=0.369s pose=(3.41, 6.98) vel=1.081 m/s ← accelerating +... +state[ 5] t=0.826s pose=(2.88, 6.38) vel=2.380 m/s ← peak speed +... +state[11] t=1.690s pose=(2.11, 5.60) vel=0.000 m/s ← end (stopped) +``` + +PathPlanner generates these states by simulating the robot's motor physics: +"Given the motor's torque capability, friction, and the robot's mass, how +quickly can the robot accelerate from rest, cruise, and decelerate to a stop?" + +### 6.5 The Bug: Zero Available Torque + +During trajectory generation, PathPlanner runs a **forward acceleration pass** +over the states. For each state, it computes how fast the module can be going, +using this logic from `PathPlannerTrajectory.forwardAccelPass()`: + +```java +// What current does the motor draw at the previous state's speed? +double currentDraw = Math.min( + driveMotor.getCurrent(lastVelRadPerSec, 12.0), // physics current + driveCurrentLimit); // our 40A limit + +// How much torque is available for acceleration? +double availableTorque = driveMotor.getTorque(currentDraw) - torqueLoss; +``` + +Starting from state[0] (robot at rest, velocity = 0): + +``` +Step 1: lastVel = 0 m/s (robot is stopped) + +Step 2: lastVelRadPerSec = 0 / 0.0508 = 0 rad/s + +Step 3: getCurrent(0, 12.0) = 211 A + ↑ At 0 rad/s the motor draws stall current (I = 12V / 0.057Ω = 211A) + +Step 4: min(211, 40) = 40 A + ↑ Clamped to our driveCurrentLimit (40A) + +Step 5: getTorque(40) = Kt × 40 = 0.1152 × 40 = 4.607 Nm + ↑ The motor's torque output at 40A + +Step 6: availableTorque = 4.607 − 4.607 = 0.000 Nm ← ZERO! + ───── ───── + │ └── torqueLoss (computed in Section 6.3: + │ the clamped current at maxSpeed was + │ ALSO 40A → same torque: 4.607 Nm) + │ + └── torque at stall, clamped to 40A (Step 5) +``` + +**Summary**: The stall current (211A) is clamped to our 40A limit. The +max-speed current (94.6A) was ALSO clamped to the same 40A limit when +`torqueLoss` was computed (Section 6.3). Same clamped current → same torque → +**zero available torque → zero acceleration → velocity stays at 0 forever.** + +With zero velocity at every state, the time between states becomes +**infinite** — the robot can never reach the next position. PathPlanner +computes `time = distance / velocity = distance / 0 = Infinity`. Since +`Infinity` is not a usable number, PathPlanner's code skips the time +assignment, leaving `timeSeconds` at its default value of **0.0** for every +state. The result: `totalTime = 0.0 seconds`, and PathPlanner's `isFinished()` +check (`timer.hasElapsed(0.0)`) returns `true` immediately. + +### 6.6 Why This is Even Worse Than "Can't Reach 3 m/s" + +You asked: *"Doesn't PathPlanner recognize that it can NOT go 3 m/s at 40A?"* + +It's actually **worse** than that. PathPlanner doesn't just fail to reach +3 m/s — it concludes the robot **cannot accelerate at all, from any speed**. + +Here's why: at EVERY speed from 0 to 3 m/s, the motor draws MORE than 40A at +12V (see the table in Section 6.1). So at every speed, the current is clamped +to 40A, producing the same torque (4.607 Nm) — which equals `torqueLoss`. +Available torque is zero at every single speed. + +PathPlanner doesn't explicitly "realize" this or print a warning. It just +computes the physics: zero available torque → zero acceleration → all +velocities remain at zero → the trajectory has no duration. The result is a +trajectory where the robot is at 12 distinct positions but with 0 velocity +and 0 time at each one — a physically impossible "teleportation" that +`FollowPathCommand` finishes instantly. + +### 6.7 The Real Meaning of `maxDriveVelocityMPS` + +PathPlanner's `ModuleConfig` documentation says: + +> **maxDriveVelocityMPS**: "The max speed that the drive motor can reach +> while actually driving the robot at full output." + +This is a **physical parameter** — the maximum speed the motor/gearbox/wheel +combination can physically achieve. It is NOT a software speed limit. + +Our motor's physical maximum speed (at the wheel) is **5.35 m/s** — calculated +from the manufacturer's published free speed (710 rad/s) through our 6.75:1 +gearbox (→ 105.2 rad/s) times our wheel radius (0.0508 m). We set +`maxDriveVelocityMPS` to **3.0 m/s**, which is our desired software speed +limit, not the physical maximum. + +The software speed limit belongs in the **path file's constraints** +(`maxVelocity: 3.0` in `8FuelPath.path`), which PathPlanner applies separately +during trajectory generation to cap the actual planned speed. + +### 6.8 How We Found This: JUnit Tests + +This root cause was discovered by writing JUnit tests that call +`PathPlannerPath.generateTrajectory()` directly, bypassing the full simulation. +The test file is `src/test/java/frc/robot/auto/TrajectoryGenerationTest.java`. + +**Key test results:** + +1. **`testRobotConfigValues`** — PASSED. Dumped all config values and confirmed + `torqueLoss (4.607) < stallTorque (24.300)`. The config LOOKS sane at first + glance, because the overall motor CAN produce more torque than `torqueLoss`. + But the current limit clamps the operating point to exactly `torqueLoss`. + +2. **`test8FuelPath_pathPointsAreDistinct`** — PASSED. The path has 12 points + spanning 2.09 meters. The path geometry is fine. + +3. **`testDriveTestPath_noFlip`** — FAILED. Even the simplest straight-line + path produces `totalTime=0.0` and zero velocities at every state. This + proved the bug is in the config, not in path geometry or flipping. + +4. **`test8FuelPath_noFlip`**, **`_flipped`**, **`_mirrored`**, + **`_mirroredThenFlipped`** — ALL FAILED. Every variant produces + `totalTime=0.0`. This proved the bug is independent of path transformation. + +5. **`testRootCause_currentLimitCausesZeroAcceleration`** — The definitive + test. It: + - Reproduced PathPlanner's `torqueLoss` calculation step by step + - Proved `clampedCurrent == clampedStallCurrent == 40A` → `availableTorque = 0` + - **Fix A** (set maxSpeed to 5.35): `totalTime = 1.69s` ✅ + - **Fix B** (set currentLimit to 120A): `totalTime = 1.72s` ✅ + +### 6.9 The Fix + +#### Fix (in `Constants.java`): Set `maxDriveVelocityMPS` to the motor's physical maximum + +**Before:** +```java +public static final ModuleConfig MODULE_CONFIG = new ModuleConfig( + SwerveConstants.wheelDiameter / 2, + SwerveConstants.maxSpeed, // 3.0 m/s ← SOFTWARE limit, not physical + 1.2, + DCMotor.getNeoVortex(1).withReduction(SwerveConstants.driveGearRatio), + SwerveConstants.driveContinuousCurrentLimit, + 1); +``` + +**After:** +```java +// maxDriveVelocityMPS must be the PHYSICAL max speed of the motor+gearbox+wheel, +// NOT the software speed limit. The software limit comes from the path file's +// maxVelocity constraint. Using the motor's theoretical free speed: +double physicalMaxSpeed = DCMotor.getNeoVortex(1) + .withReduction(SwerveConstants.driveGearRatio) + .freeSpeedRadPerSec * (SwerveConstants.wheelDiameter / 2); // = 5.35 m/s + +public static final ModuleConfig MODULE_CONFIG = new ModuleConfig( + SwerveConstants.wheelDiameter / 2, + physicalMaxSpeed, // 5.35 m/s ← PHYSICAL max + 1.2, + DCMotor.getNeoVortex(1).withReduction(SwerveConstants.driveGearRatio), + SwerveConstants.driveContinuousCurrentLimit, + 1); +``` + +**Why this works:** +- At 5.35 m/s (free speed), the motor draws only 3.6A → `torqueLoss = getTorque(3.6) = 0.41 Nm` +- At stall (0 m/s), current is clamped to 40A → `getTorque(40) = 4.607 Nm` +- `availableTorque = 4.607 − 0.41 = 4.20 Nm` → robot accelerates normally +- Trajectory generates correctly: **totalTime = 1.69 seconds**, 12 states + with velocities ramping up to 2.38 m/s and back down to 0 + +**No other files need to change.** The path files' `maxVelocity: 3.0` +constraint still limits the robot to 3 m/s during path following. + +### 6.10 Relationship to Earlier Findings + +The `ROBOT_CONFIG` parameter mismatch is the **primary root cause** of the +instant PathPlanner termination. It affects ALL paths, ALL transformations +(flip, mirror, no-op), and ALL starting conditions. + +The bugs identified in Sections 2–4 (pigeon yaw latency, odometry heading +corruption, pigeon doubling) are **real but secondary**: + +| Bug | Still real? | Impact with Config fixed | +|-----|-----------|--------------------------| +| `hardResetPose` passes wrong `gyroAngle` | Yes | Would cause 1-2 cycles of wrong heading, but PathPlanner would recover since the trajectory is now 1.7s long | +| Pigeon yaw doubling in SimulationManager | Yes | Would cause ongoing heading error during path following in simulation | +| `pigeon.setYaw()` latency in CTRE sim | Yes | Would cause temporary heading glitch, compensated by Fix A from Section 4 | + +**All three bugs should still be fixed**, but they would not cause instant +termination once the config is corrected. + +--- + +## 7. Final Summary + +### The Bug + +PathPlanner's `FollowPathCommand` finished after exactly one `execute()` cycle, +commanding enormous speeds (~7 m/s, ~10 rad/s) for a single 20ms frame before +zeroing the motors. The robot never moved. + +### Root Cause + +In `Constants.java`, `ModuleConfig.maxDriveVelocityMPS` was set to **3.0 m/s** +(a software speed limit) instead of the motor's physical maximum of **5.35 m/s**. +At 3.0 m/s, the NEO Vortex motor draws 94.6A — well above our 40A current +limit. PathPlanner's trajectory generator clamped both the "max-speed current" +and the "stall current" to the same 40A, producing identical torque values. The +result: `availableTorque = 0 Nm` → zero acceleration → zero velocity at every +state → `totalTime = 0.0s` → `isFinished()` returns true immediately. + +### The Fix + +Changed `ModuleConfig`'s `maxDriveVelocityMPS` from `SwerveConstants.maxSpeed` +(3.0 m/s, the teleop software limit) to the motor's theoretical free speed at +the wheel (~5.35 m/s, computed from manufacturer specs). This is a one-line +change in `Constants.java`. The teleop speed limit (`maxSpeed = 3`) is +unchanged, and path files still enforce their own `maxVelocity` constraints. + +### How We Found It + +1. **Trace logging** (`sim-output.log`, `sim-output2.log`) revealed the + one-cycle termination and showed that even with correct heading, + PathPlanner still finished immediately +2. **Trajectory tracing** (`sim-output3.log`) showed `totalTime = 0.0s` for + all generated trajectories, ruling out heading/flipping issues +3. **JUnit tests** (`TrajectoryGenerationTest.java`) called + `PathPlannerPath.generateTrajectory()` directly, proving that ALL paths + produced `totalTime = 0.0` with the original config, and that changing + `maxDriveVelocityMPS` to the physical free speed fixed it + +### Verification + +The fix was verified in three ways: + +1. **JUnit tests** — all 8 tests pass, including one that proves the old + config produces `totalTime = 0.0` and the new config produces + `totalTime = 1.69s` with proper acceleration/deceleration profiles +2. **Simulation run** (`sim-output4-fixed.log`) — `autoDrive` now runs across + multiple cycles with smoothly increasing velocities (0.015 → 0.032 → + 0.099 m/s over the first 3 execute cycles), `isFinished() = false` at + each check, and the trajectory has `totalTime = 1.78s` (13 states) +3. **Visual confirmation** — the robot moves along the path in the simulator + +--- + +## 8. Remaining Issues to Fix + +The following bugs were identified during this investigation. They are **not** +responsible for the instant termination (now fixed), but they will cause +problems during longer autonomous paths in simulation. + +> **Branch context**: The `MODULE_CONFIG` fix (Section 6.9) and +> `TrajectoryGenerationTest` unit tests were applied to the +> `jmm-sim-test-support` branch. All other changes below — including the +> `hardResetPose` infrastructure, SimulationManager pose sync, field +> dimensions, EightLemonAuto coordinates, and trace instrumentation — exist +> **only** on the `jmm-auto-mode-debug` branch and have NOT been merged to +> `jmm-sim-test-support`. + +### 8.1 `startAutoAt` passes wrong `gyroAngle` to `odometry.resetPosition()` + +**Affects**: Both branches (different code, same underlying bug) + +**Bug**: When resetting the robot's pose, the code passes +`newPose.getRotation()` (the **desired** heading) as `gyroAngle` to +`odometry.resetPosition()` instead of `getYaw()` (the **actual** pigeon +reading). Because `pigeon.setYaw()` does not take immediate effect (especially +in simulation), the stored gyro baseline is wrong. On the next `periodic()`, +`odometry.update()` computes a large heading delta and corrupts the rotation. + +On `jmm-sim-test-support`, this code is in `startAutoAt()` directly: +```java +pigeon.setYaw(startPose2d.getRotation().getDegrees()); +odometry.resetPosition(startPose2d.getRotation(), getPositions(), startPose2d); +// ^^^^^^^^^^^^^^^^^^^^^^^^^ should be getYaw() +``` + +On `jmm-auto-mode-debug`, the same bug exists inside `hardResetPose()`: +```java +Rotation2d gyroAngle = updatePigeon ? newPose.getRotation() : getYaw(); +// ^^^^^^^^^^^^^^^^^^^^^ should be getYaw() +``` + +**Impact**: 1-2 cycles of incorrect heading at the start of autonomous. With +the config fix applied, PathPlanner's trajectory is long enough (~1.7s) to +recover, but the initial heading error causes unnecessary corrective maneuvers. + +**Fix**: Always use `getYaw()` as the gyroAngle: +```java +Rotation2d gyroAngle = getYaw(); // actual pigeon reading, not desired heading +``` +This is correct on both real hardware and in simulation. + +### 8.2 SimulationManager does not sync pose after `startAutoAt` + +**Affects**: `jmm-sim-test-support` branch + +**Bug**: On `jmm-sim-test-support`, `SimulationManager` has no mechanism to +detect when `startAutoAt` (or `zeroGyro`) resets the robot's pose. Its internal +`simPose` stays at `(0, 0, 0°)` after `startAutoAt` places the robot at, say, +`(13.0, 0.94, 271.82°)`. On the next `simulationPeriodic()` call, +`pigeonSimState.setRawYaw(simPose.rotation)` overwrites the pigeon with 0° +(from the stale `simPose`), undoing the `pigeon.setYaw()` call in `startAutoAt`. + +On `jmm-auto-mode-debug`, this was partially addressed by adding: +- `pendingSimPoseReset` field on `SwerveSubsystem` +- `consumeSimPoseReset()` method +- `SimulationManager` consuming the reset each cycle + +However, the pigeon yaw **doubling** bug (8.3) still exists on that branch. + +**Fix**: Port the `consumeSimPoseReset` infrastructure from +`jmm-auto-mode-debug` to `jmm-sim-test-support`, then also fix the doubling +(8.3). + +### 8.3 Pigeon yaw doubling in SimulationManager + +**Affects**: `jmm-auto-mode-debug` branch (on `jmm-sim-test-support`, the sim +doesn't sync pose at all — see 8.2 — so doubling doesn't occur, but a +different and worse bug occurs instead) + +**Bug**: `pigeon.setYaw(X)` creates an internal offset of +X, and then +`pigeonSimState.setRawYaw(X)` sets a raw value. The reported yaw = rawYaw + +offset = X + X = **2X**. For example, a 180° heading becomes 360° (≡ 0°). + +**Impact**: Ongoing heading error during path following in simulation. The +odometry heading will be wrong by the original setYaw value for the entire +autonomous period. + +**Fix**: When `SimulationManager` consumes a pose reset, clear the pigeon +offset before setting the raw yaw: +```java +pigeonSimState.setRawYaw(0); +swerveSubsystem.getPigeon().setYaw(0); // clears the offset +pigeonSimState.setRawYaw(desiredYawDeg); // set the actual value +``` + +### 8.4 `pigeon.setYaw()` latency in CTRE simulation + +**Affects**: Both branches + +**Characteristic**: `pigeon.setYaw()` does not take immediate effect in CTRE +simulation. The yaw value remains stale for 1-2 cycles before the new value +appears. This is a CTRE library behavior, not our bug. + +**Impact**: Temporary heading glitch during the first 1-2 cycles after a pose +reset. Fix 8.1 makes the system robust against this latency, so no additional +code change is needed specifically for this issue. + +### 8.5 `ROBOT_CONFIG` module order may not match `SwerveDriveKinematics` + +**Affects**: `jmm-sim-test-support` branch + +**Bug**: On `jmm-sim-test-support`, `ROBOT_CONFIG` is constructed with module +order `FL, FR, BL, BR`. On `jmm-auto-mode-debug`, this was changed to +`FL, FR, BR, BL` with a comment: "Module order must match +SwerveDriveKinematics: FL, FR, BR, BL." If the orders don't match, PathPlanner +will associate module physics with the wrong physical wheel positions. + +**Fix**: Verify the `swerveKinematics` construction order in `Constants.java` +and ensure `ROBOT_CONFIG` uses the same order. + +### 8.6 Field dimensions do not match 2026 official values + +**Affects**: `jmm-sim-test-support` branch + +**Bug**: `FieldConstants.FIELD_LENGTH` = 17.548 m and `FIELD_WIDTH` = 8.052 m. +On `jmm-auto-mode-debug`, these were updated to the 2026 official dimensions: +`FIELD_LENGTH` = 16.54 m and `FIELD_WIDTH` = 8.07 m with the comment: "Must +match PathPlanner's FlippingUtil defaults so alliance flipping is consistent." + +**Impact**: Alliance flipping (`flipForAlliance`) computes mirrored positions +using `(FIELD_LENGTH − x, FIELD_WIDTH − y)`. If these constants don't match +PathPlanner's internal field size, the flipped path start won't match the +flipped `startAutoAt` position, causing a position mismatch at the start of +autonomous. + +**Fix**: Update `FIELD_LENGTH` and `FIELD_WIDTH` to match the 2026 official +field dimensions and PathPlanner's `FlippingUtil` defaults. + +### 8.7 `EightLemonAuto` coordinates and heading don't match path file + +**Affects**: `jmm-sim-test-support` branch + +**Bug**: `EightLemonAuto` calls `startAutoAt(3.53, 7.13, -130.45)` but the +`8FuelPath.path` file defines: +- First anchor: `(3.5355, 7.1300)` +- `idealStartingState.rotation`: `91.82°` + +On `jmm-auto-mode-debug`, this was corrected to +`startAutoAt(3.5355, 7.1300, 91.82)`. + +**Impact**: The robot starts at a slightly wrong position and a completely +wrong heading (-130.45° vs 91.82°), causing PathPlanner to compute corrective +maneuvers at the start of the path. + +**Fix**: Update `EightLemonAuto` coordinates and heading to match the path +file's first anchor and `idealStartingState.rotation`. + +### 8.8 Centralized pose reset infrastructure not on `jmm-sim-test-support` + +**Affects**: `jmm-sim-test-support` branch (architecture improvement) + +On `jmm-auto-mode-debug`, all pose resets (`startAutoAt`, `zeroGyro`, +`resetOdometry`) were refactored to flow through a centralized +`hardResetPose()` method that handles pigeon, odometry, and sim notification +in one place. This prevents future bugs where one code path forgets to notify +the simulation. + +On `jmm-sim-test-support`, each reset path (`startAutoAt`, `zeroGyro`, +`resetOdometry`) independently calls `pigeon.setYaw()` and/or +`odometry.resetPosition()` with no sim notification. + +**Fix**: Port `hardResetPose()`, `hardResetHeading()`, and +`consumeSimPoseReset()` from `jmm-auto-mode-debug` to `jmm-sim-test-support`, +then fix the gyroAngle bug (8.1) in the centralized method. + +### 8.9 Remove trace instrumentation (`jmm-auto-mode-debug` only) + +**Affects**: `jmm-auto-mode-debug` branch only (not on `jmm-sim-test-support`) + +**Files**: `SwerveSubsystem.java`, `RobotContainer.java`, `Robot.java`, +`SimulationManager.java`, `DriveTestAuto.java`, `EightLemonAuto.java`, +`TracedCommand.java`, `TraceLogger.java` + +**Task**: The trace logging and `TracedCommand` wrappers were added for +debugging. They should be removed (or gated behind a debug flag) before +competition. All instrumented sections are delimited with +`// --- TRACE INSTRUMENTATION ---` and `// --- END TRACE ---` comments. + +### 8.10 Restore `getAutonomousCommand()` to use selected auto mode (`jmm-auto-mode-debug` only) + +**Affects**: `jmm-auto-mode-debug` branch only (not on `jmm-sim-test-support`) + +**File**: `RobotContainer.java` + +**Task**: The `getAutonomousCommand()` method was temporarily modified to always +return `DriveTestAuto` for testing. Restore it to use the `autoModeChooser` +selection: +```java +// Remove this line: +selected = AutoConstants.AutoMode.DriveTestAuto; // FOR TESTING +``` + +### Priority + +| Issue | Branch(es) | Priority | Difficulty | +|-------|-----------|----------|-----------| +| 8.7 Fix EightLemonAuto coordinates/heading | sim-test-support | **High** (wrong start pose) | Trivial | +| 8.6 Fix field dimensions | sim-test-support | **High** (wrong alliance flip) | Trivial | +| 8.5 Fix ROBOT_CONFIG module order | sim-test-support | **High** (verify & fix) | Trivial | +| 8.1 Fix gyroAngle in pose reset | Both | **High** (affects auto accuracy) | Easy | +| 8.8 Port centralized pose reset | sim-test-support | **High** (prerequisite for 8.2/8.3 fixes) | Medium | +| 8.2 SimulationManager pose sync | sim-test-support | **Medium** (sim-only) | Easy (after 8.8) | +| 8.3 Fix pigeon yaw doubling | debug (after 8.8 on sim-test) | **Medium** (sim-only) | Easy | +| 8.10 Restore auto mode selection | debug only | **Medium** (debug cleanup) | Trivial | +| 8.9 Remove trace instrumentation | debug only | **Medium** (debug cleanup) | Easy but tedious | +| 8.4 CTRE pigeon latency | Both | **Low** (mitigated by 8.1) | N/A (library behavior) | + +## 9. Odometry Reset Diagnostic Test Results + +### 9.1 Test Overview + +Two tests were created to investigate issues 8.1 and 8.2: + +| Test | Type | Location | Branch(es) | Purpose | +|------|------|----------|------------|---------| +| `OdometryResetTest.java` | JUnit | `src/test/java/frc/robot/sim/` | `jmm-auto-mode-debug` only | Code-as-documentation: demonstrates the math bug using pure WPILib `SwerveDrivePoseEstimator` with no hardware dependencies. Does **not** test actual robot code. | +| `OdometryResetTestCommand.java` | DiagnosticTest (simulator) | `src/main/java/frc/robot/test/` | Both branches | Runs in the full simulator with real pigeon latency, SimulationManager, and SwerveSubsystem. Exercises the actual buggy code path. | + +The JUnit test was moved to the debug branch because it serves only as +documentation — it cannot detect regressions in robot code since it doesn't +call any robot code. The `OdometryResetTestCommand` is on both branches as a +genuine diagnostic tool. + +### 9.2 OdometryResetTestCommand Design + +The test runs multiple trials (default 10), each with two phases: + +**Phase 1 — Reset & Observe** (10 cycles): +- Resets the robot pose to `(5.0, 4.0, 90°)` using the same buggy code as + `startAutoAt()`: `pigeon.setYaw(desired)` + `odometry.resetPosition(desired, ...)` +- Observes whether odometry heading is corrupted on subsequent cycles +- Measures pigeon latency (how many cycles before pigeon catches up) + +**Phase 2 — Closed-Loop L-Path Drive**: +- Resets to a *different* heading (`0°`) to trigger genuine pigeon latency + (pigeon must transition from 90° to 0°) +- Drives a 1m forward leg + 1m right-turn leg using proportional control +- Records lateral deviation from ideal path and final position error + +### 9.3 Results (10-Trial Run) + +From `docs/logs/odo-reset-test.log` (run on `jmm-sim-test-support` branch): + +``` +Trial | Corrupted | MaxHdgErr | PigeonLat | MaxLatDev | FinalPosErr | FinalHdgErr +------+-----------+-----------+-----------+-----------+-------------+------------ + 1 | 1/10 | 90.00° | YES | 0.0473m | 0.0464m | 0.90° + 2 | 0/10 | 0.00° | YES | 0.0470m | 0.0473m | 0.92° + 3 | 0/10 | 0.00° | YES | 0.0469m | 0.0467m | 0.90° + 4 | 0/10 | 0.00° | YES | 0.0468m | 0.0469m | 0.91° + 5 | 0/10 | 0.00° | YES | 0.0474m | 0.0466m | 0.90° + 6 | 0/10 | 0.00° | YES | 0.0472m | 0.0467m | 0.91° + 7 | 0/10 | 0.00° | YES | 0.0472m | 0.0468m | 0.91° + 8 | 0/10 | 0.00° | YES | 0.0474m | 0.0467m | 0.91° + 9 | 0/10 | 0.00° | YES | 0.0471m | 0.0471m | 0.92° + 10 | 0/10 | 0.00° | YES | 0.0474m | 0.0468m | 0.91° +``` + +**Aggregate statistics:** +- Trials with heading corruption: **1/10** (10%) — first trial only +- Pigeon latency present: **10/10** (100%) — `pigeon.setYaw()` never instant +- Average max lateral deviation: **0.0472m** (~4.7cm) +- Worst final position error: **0.0473m** (~4.7cm) +- Worst final heading error: **0.92°** + +### 9.4 Key Findings + +1. **The heading corruption bug (8.1) is real but appears only on the first + trial.** After Trial 1, the pigeon has already settled at 90° from the + Phase 2→Phase 1 transition (−89° → 90°). The CTRE sim processes the + `setYaw()` within the inter-trial gap, so Trials 2–10 show zero corruption. + Trial 1 is unique because the pigeon starts at 0° (never been set before). + +2. **Pigeon latency is 100% consistent.** Every Phase 2 reset shows + `pigeonYaw after=90°` when the target was `0°`, confirming that + `pigeon.setYaw()` does not take effect within the same cycle in CTRE + simulation. + +3. **Despite pigeon latency, the SimulationManager corrects it within 1 cycle.** + By the first drive sample (`L1 C 0`), the pigeon and odometry heading are + already at the correct value (0°). This means `SimulationManager.simulationPeriodic()` + overwrites the pigeon with the correct heading from `simPose` before the + drive loop reads it. + +4. **The ~0.047m lateral deviation is controller tracking error, not bug-related.** + It appears consistently across all 10 trials (including those with zero + heading corruption) and is caused by the proportional-only controller's + overshoot during the 90° turn in Leg 2. The deviation is identical whether + or not heading corruption occurred. + +5. **Practical impact in simulation: negligible.** The bug causes a 1-cycle + heading glitch that is immediately corrected. Longer drive distances would + not reveal additional problems, as the pigeon catches up within one cycle + and the closed-loop controller handles the remainder. + +### 9.5 Conclusion + +The `odometry.resetPosition()` bug (8.1) is **mathematically real** but has +**negligible practical impact in the CTRE simulator** because: +- `SimulationManager` overwrites the pigeon each cycle, masking the latency +- The bug manifests as a single corrupted cycle that self-corrects + +On **real hardware**, the impact may differ — pigeon latency could be shorter +(no sim frame delay) or longer (CAN bus congestion). The fix (use `getYaw()` +instead of desired heading) is still recommended as it eliminates the bug +regardless of latency behavior. diff --git a/docs/code-improvements.md b/docs/code-improvements.md index 4fc918f..49b9605 100644 --- a/docs/code-improvements.md +++ b/docs/code-improvements.md @@ -1778,6 +1778,195 @@ Remove the `cppDependencies` section (including `binaryPlatforms`) from `WPILibN --- +## 18. Add a `RobotState` Class as the Single Source of Truth for Robot Pose (Architecture) + +### What +Extract pose estimation, sensor fusion, and robot state tracking from `SwerveSubsystem` into a dedicated `RobotState` class. This class would be the single source of truth for "where is the robot?" and "what is the robot doing?" — consolidating the odometry, gyro management, and vision fusion logic that is currently scattered across `SwerveSubsystem`, `SimulationManager`, and various commands. + +**Note**: This is a **plain Java class** (either a singleton or an injected dependency), **not** a WPILib `Subsystem`. See the rationale below. + +### Why + +This recommendation emerged directly from debugging the autonomous pose reset issues documented in `docs/auto-sim-log-analysis.md`. The investigation revealed that `SwerveSubsystem` currently mixes three distinct responsibilities: + +1. **Motor control** — commanding swerve modules to drive +2. **Pose estimation** — fusing gyro, encoders, and vision into a robot pose +3. **Pose management** — resetting and synchronizing pose across subsystems (pigeon, odometry, simulation) + +This mixing caused multiple bugs (documented in detail in [`docs/auto-sim-analysis/auto-sim-log-analysis.md`](auto-sim-analysis/auto-sim-log-analysis.md), Section 8 — "Remaining Issues"): +- **Issue 8.1**: `startAutoAt()` passed the wrong `gyroAngle` to `odometry.resetPosition()` because pose reset logic was interleaved with drive setup code +- **Issue 8.2**: `SimulationManager` couldn't detect pose resets because the reset happened inside `SwerveSubsystem` without notification +- **Issue 8.3**: `pigeon.setYaw()` caused heading doubling in simulation because the pigeon and odometry were managed by different code paths with different assumptions +- **`drive()` bug**: Field-centric conversion used `getYaw()` (raw pigeon) instead of `getPose().getRotation()` (fused estimate), creating inconsistency + +A `RobotState` class eliminates these issues by providing a single, controlled entry point for all pose operations. + +### Reference Implementations from Elite FRC Teams + +Two of the most architecturally sophisticated FRC teams have published code using this exact pattern: + +#### FRC 6328 — Mechanical Advantage + +**Repository**: [Mechanical-Advantage/RobotCode2025Public](https://github.com/Mechanical-Advantage/RobotCode2025Public) (also [2026](https://github.com/Mechanical-Advantage/RobotCode2026Public), [2024](https://github.com/Mechanical-Advantage/RobotCode2024Public)) + +**File**: `src/main/java/org/littletonrobotics/frc2025/RobotState.java` (~471 lines) + +**Key design decisions**: +- **Singleton pattern**: `RobotState.getInstance()` — accessible from anywhere without dependency injection +- **Owns the pose estimator**: Implements its own Kalman filter (does NOT use WPILib's `SwerveDrivePoseEstimator`), maintaining both `odometryPose` and `estimatedPose` +- **Receives observations, not raw sensor values**: The Drive subsystem calls `addOdometryObservation(wheelPositions, gyroAngle, timestamp)` — note that the Drive subsystem reads the sensors, but `RobotState` does all the math +- **Manages gyro offset**: Tracks a `gyroOffset` field so that `resetPose()` correctly handles the gyro-to-field rotation mapping +- **Vision fusion**: `addVisionObservation()` and `addTxTyObservation()` process vision data with configurable standard deviations +- **Game piece tracking**: Also tracks coral and algae positions (game-specific state) +- **Has its own `periodic()`**: Called from the main robot loop for logging and LED state updates +- **NOT a Subsystem**: Plain Java class — no `requires()`, no command scheduling conflicts + +**Data flow**: +``` +Drive subsystem → addOdometryObservation() → RobotState (computes pose) +Vision subsystem → addVisionObservation() → RobotState (fuses vision) +Any command/subsystem ← getEstimatedPose() ← RobotState (reads pose) +``` + +#### FRC 254 — The Cheesy Poofs + +**Repository**: [Team254/FRC-2025-Public](https://github.com/Team254/FRC-2025-Public) (also [2024](https://github.com/Team254/FRC-2024-Public), [2023](https://github.com/Team254/FRC-2023-Public), and back to [2019](https://github.com/Team254/FRC-2019-Public)) + +**File**: `src/main/java/com/team254/frc2025/RobotState.java` (~509 lines) + +**Key design decisions**: +- **Dependency injection**: `RobotState` is created in `RobotContainer` and passed to subsystems that need it — more testable than a singleton +- **Thread-safe**: Uses `AtomicReference<>` for all mutable state because their odometry runs on a high-frequency thread separate from the main robot loop +- **Time-interpolatable pose buffer**: `ConcurrentTimeInterpolatableBuffer` stores historical poses for latency compensation when applying vision corrections +- **Comprehensive velocity tracking**: Tracks measured, desired, and fused chassis speeds in both robot-relative and field-relative frames +- **Mechanism state**: Also tracks elevator height, wrist angle, intake rotations, etc. — a true "robot state" beyond just pose +- **Trajectory tracking**: Stores current trajectory target and actual pose for diagnostics +- **Pose prediction**: `getPredictedFieldToRobot(lookaheadTimeS)` extrapolates future pose based on current velocity +- **NOT a Subsystem**: Plain Java class — no scheduling conflicts, thread-safe, accessible from anywhere + +**Data flow**: +``` +DriveIOHardware → addOdometryMeasurement(timestamp, pose) → RobotState +DriveIOHardware → addDriveMotionMeasurements(...) → RobotState (speeds, IMU data) +Vision subsystem → updateMegatagEstimate() → RobotState → SwerveDrivePoseEstimator +Any command/subsystem ← getLatestFieldToRobot() ← RobotState +``` + +### Why NOT a Subsystem? + +Both 6328 and 254 deliberately chose NOT to make `RobotState` a WPILib `Subsystem`. The reasons are: + +1. **No scheduling conflicts**: A `Subsystem` can only be "owned" by one `Command` at a time via `requires()`. If `RobotState` were a subsystem, only one command could read the robot's pose at a time — which is nonsensical since multiple systems (drive, vision, autonomous, LED controller) all need pose simultaneously +2. **No default command needed**: `RobotState` doesn't need a default command — it processes data when data arrives, not on a fixed schedule driven by command allocation +3. **Thread safety**: `RobotState` may need to be accessed from multiple threads (odometry thread, vision processing thread, main robot loop). `Subsystem` isn't designed for this +4. **Simplicity**: A plain class with well-defined methods is simpler and more predictable than the command scheduling framework + +### Proposed Design for Our Codebase + +```java +/** + * Single source of truth for robot pose and state. + * + * This is a plain Java class (NOT a Subsystem) that centralizes all + * pose estimation, sensor fusion, and state tracking. Subsystems feed + * sensor data IN, and commands/subsystems read state OUT. + */ +public class RobotState { + private static RobotState instance; + + private final SwerveDrivePoseEstimator odometry; + private Rotation2d gyroOffset; + + public static RobotState getInstance() { + if (instance == null) instance = new RobotState(); + return instance; + } + + // ---- Data IN (called by subsystems) ---- + + /** Called by SwerveSubsystem.periodic() with raw sensor readings */ + public void addOdometryObservation(Rotation2d gyroAngle, + SwerveModulePosition[] positions) { ... } + + /** Called by vision processing code */ + public void addVisionObservation(Pose2d visionPose, double timestamp, + Matrix stdDevs) { ... } + + /** Reset pose (e.g., at auto start). Correctly handles gyro offset. */ + public void resetPose(Rotation2d actualGyroReading, + SwerveModulePosition[] positions, + Pose2d newPose) { ... } + + // ---- Data OUT (read by commands/subsystems) ---- + + /** The fused robot pose — THE single source of truth */ + public Pose2d getEstimatedPose() { ... } + + /** Heading from the fused pose — use this for field-centric driving */ + public Rotation2d getRotation() { + return getEstimatedPose().getRotation(); + } + + /** Current chassis speeds */ + public ChassisSpeeds getRobotVelocity() { ... } +} +``` + +**What moves OUT of `SwerveSubsystem`**: +- `SwerveDrivePoseEstimator odometry` → `RobotState` +- `getPose()` → delegates to `RobotState.getInstance().getEstimatedPose()` +- `getYaw()` — only used internally by `RobotState` for odometry input +- `resetOdometry()` → delegates to `RobotState.getInstance().resetPose()` +- `startAutoAt()` pose reset logic → delegates to `RobotState.getInstance().resetPose()` +- `updateOdometryWithVision()` → delegates to `RobotState.getInstance().addVisionObservation()` +- Vision Limelight processing → could stay in `SwerveSubsystem` but calls `RobotState` for fusion + +**What stays IN `SwerveSubsystem`**: +- Swerve module management and motor commands +- `drive()`, `driveFromChassisSpeeds()` — motor control +- Pigeon2 hardware object — but `SwerveSubsystem.periodic()` passes `pigeon.getYaw()` to `RobotState` +- SmartDashboard module-specific telemetry + +### Incremental Migration Path + +This change doesn't need to happen all at once. A practical migration path: + +1. **Phase 1** (minimal, fixes current bugs): Create `RobotState` with just `resetPose()` that correctly handles gyro offset. Have `startAutoAt()` and `resetOdometry()` delegate to it. This alone fixes issues 8.1 and 8.3. + +2. **Phase 2** (move odometry): Move `SwerveDrivePoseEstimator` into `RobotState`. `SwerveSubsystem.periodic()` calls `robotState.addOdometryObservation()` instead of `odometry.update()`. + +3. **Phase 3** (move vision): Move `updateOdometryWithVision()` logic into `RobotState.addVisionObservation()`. + +4. **Phase 4** (clean up): Remove `getPose()` from `SwerveSubsystem` (or make it delegate). All callers use `RobotState.getInstance().getEstimatedPose()`. + +### Where +- **New file**: `src/main/java/frc/robot/RobotState.java` +- **Modified file**: `src/main/java/frc/robot/Subsystems/SwerveSubsystem.java` — extract pose estimation logic +- **Modified file**: `src/main/java/frc/robot/sim/SimulationManager.java` — read pose from `RobotState` instead of `SwerveSubsystem` +- **Modified files**: Any command/subsystem that currently calls `swerveSubsystem.getPose()` — redirect to `RobotState` + +### Impact +- **High value**: Eliminates an entire class of pose-related bugs by centralizing state management +- **Medium risk**: Significant refactoring of `SwerveSubsystem`, but can be done incrementally (see migration path above) +- **Improves testability**: `RobotState` can be unit tested independently of hardware +- **Follows proven patterns**: Both 6328 and 254 — arguably the two most successful software teams in FRC history — have used this pattern for multiple seasons +- **Mid-to-late season timing**: This is best done during an off-week or post-season, not right before a competition + +### Decision Points +- **Singleton vs. dependency injection?** Singleton (like 6328) is simpler; dependency injection (like 254) is more testable. Recommend singleton for our team's experience level. +- **Phase 1 only, or full migration?** Phase 1 alone fixes the current bugs with minimal risk. Full migration is the architecturally correct solution but requires more effort. +- **When to implement?** Phase 1 can be done now. Phases 2-4 are best for off-season or a non-competition week. + +### Status +- [ ] Pending team review +- [ ] Approved (Phase 1 only) +- [ ] Approved (Full migration) +- [ ] Rejected +- [ ] In progress +- [ ] Implemented + +--- + ## Future Recommendations _Additional code improvement recommendations will be added here as they are identified._ diff --git a/docs/debugging-methodology.md b/docs/debugging-methodology.md new file mode 100644 index 0000000..b857c8b --- /dev/null +++ b/docs/debugging-methodology.md @@ -0,0 +1,614 @@ +# Swerve Drive Fault Isolation Methodology + +## Philosophy + +The key principle is **isolation**: systematically eliminate variables until you've narrowed the problem to a single root cause. You have two symptoms — treat them as potentially related but investigate independently. At each step, you're asking: *"Does this test eliminate a category of causes?"* + +The methodology has three phases: +1. **Observe & Instrument** — gather data before changing anything +2. **Isolate** — eliminate categories of causes using targeted tests +3. **Confirm** — verify the root cause by fixing it and demonstrating the symptom disappears + +### One Rule Above All + +> **Change ONE variable at a time, test, and record the result before changing the next variable.** + +If you change PID values AND angle offsets AND swap a motor at the same time and the problem goes away, you have no idea which change fixed it — and you've learned nothing. + +--- + +## Problem Statements + +1. **Primary Issue**: The robot fails to drive straight — it drifts/pulls to one side when attempting to drive straight forward +2. **Secondary Issue**: One specific angle motor (only one of the four modules) jitters back and forth slightly when the drive base is not being commanded to move (robot at rest) + +## Possible Root Cause Categories + +| Category | Likelihood | Examples | +|---|---|---| +| Our code (especially the 2024→2026 REVLib migration) | **HIGH** | Dropped motor config settings, wrong angleOffset, bad conversion factors, PID values, missing/incorrect inversion flags | +| Electronics (wiring, misconfiguration, hardware failure) | MODERATE | Loose CAN bus connection, bad motor controller, encoder failure, wiring swap | +| Mechanical | MODERATE | Uneven wheel wear, binding module, bent frame, weight distribution | +| Vendor code (REVLib, Phoenix6) | LOW | API behavior change between versions, default value change | +| WPILib code | LOW | Change in `SwerveModuleState.optimize()`, kinematics math | + +--- + +## Phase 1: Observe & Instrument (Do This First, Change Nothing) + +Before touching any code or hardware, collect baseline data. The codebase already publishes useful telemetry to SmartDashboard and NetworkTables — use it. + +### Test 1.1: Record Baseline Telemetry (At Rest) + +**Goal**: Characterize the jittering module's behavior compared to the other three. + +**Procedure**: +1. Power on the robot, connect to SmartDashboard/AdvantageScope +2. **Do NOT touch the joysticks** — let the robot sit idle with the robot **enabled** in teleop mode +3. Record for 30+ seconds and capture for ALL four modules: + - `Mod X Cancoder` (absolute encoder angle) + - `Mod X Integrated` (integrated encoder angle) + - `Mod X Velocity` (drive velocity — should be ~0) +4. Identify which module is jittering. Note its module number (0–3). + +**What you're looking for**: + +| Observation | Suggests | +|---|---| +| Integrated encoder oscillates but Cancoder is stable | The PID is hunting — likely a **code/tuning** issue | +| Both encoders oscillate together | The wheel is **physically moving** — could be mechanical or electrical | +| Cancoder and Integrated disagree significantly | Bad `angleOffset` calibration or failed `resetToAbsolute()` | +| Drive velocity is non-zero at rest | Something is commanding movement — check TeleopSwerve deadband or default command | + +**Record these values in the Data Recording Template at the bottom of this document.** + +### Test 1.2: Record Baseline Telemetry (Driving Straight) + +**Goal**: Characterize the drift direction and magnitude. + +**Procedure**: +1. Place the robot on a flat, open surface (competition carpet if possible) +2. Zero the gyro +3. Push the forward-only joystick axis gently (~30% power) and hold straight for 3–5 seconds +4. Record: + - Pigeon Yaw (did the heading change? Which direction?) + - All four module angles (are all four pointing the same direction?) + - All four module velocities (are they all the same speed?) + - Desired Swerve States vs actual Swerve States (are they matching?) + +**What you're looking for**: + +| Observation | Suggests | +|---|---| +| One module angle is offset from the others | Bad `angleOffset`, bad `resetToAbsolute()`, or bad CANcoder | +| One module velocity differs significantly from others | Mechanical (wheel friction), electrical (wiring), or motor issue | +| All modules match but robot still drifts | Gyro issue (field-oriented mode using bad heading), or mechanical (weight distribution, floor friction) | +| Desired states ≠ Actual states for one module | That module's control loop isn't tracking — PID tuning, encoder, or motor issue | + +### Test 1.3: Record Startup Calibration Values + +**Goal**: Verify that `resetToAbsolute()` is correctly calibrating each module at startup. + +**Procedure**: +1. Physically point all four wheels straight forward (use a straight-edge against the frame) +2. Power cycle the robot +3. Immediately after boot, before enabling, record for each module: + - `Mod X Cancoder` value + - `Mod X Integrated` value + - The `angleOffset` from Constants.java (or Preferences if overridden) +4. Verify the math: `Integrated` should equal `Cancoder − angleOffset` + +**What you're looking for**: + +| Observation | Suggests | +|---|---| +| Math checks out for all modules | `resetToAbsolute()` is working correctly | +| One module has wrong Integrated value | CAN bus timing issue — CANcoder may not have sent data before `resetToAbsolute()` ran | +| Values change between power cycles | CANcoder update frequency too low (currently 1 Hz) — module may read stale/zero data | + +--- + +## Phase 2: Isolate (Systematic Fault Tree) + +Use the telemetry data from Phase 1 to decide which branch of the fault tree to follow. The tree below is structured as a series of **binary isolation tests** — each test eliminates one category. + +### Fault Tree: Robot Drifts When Driving Straight + +``` +Robot Drifts When Driving Straight +├── A. Are all 4 module ANGLES correct when commanding "straight"? +│ ├── NO → Go to Branch A (Angle Problem) +│ └── YES ↓ +├── B. Are all 4 module SPEEDS equal when commanding "straight"? +│ ├── NO → Go to Branch B (Speed Problem) +│ └── YES ↓ +├── C. Does it drift in BOTH robot-centric AND field-oriented modes? +│ ├── Only field-oriented → Go to Branch C (Gyro/Heading Problem) +│ └── Both → Go to Branch D (Mechanical/Physical Problem) +``` + +### Fault Tree: One Module Jitters at Rest + +``` +One Module Jitters at Rest +├── E. Does it jitter with the robot code DISABLED (just powered on)? +│ ├── YES → Electrical/mechanical issue (not code) +│ └── NO ↓ +├── F. Does it jitter if you set angleKP to 0 (disable angle PID)? +│ ├── NO → PID is causing it → Go to Branch F (PID/Tuning) +│ └── YES → Something else is commanding movement → Go to Branch G +├── H. Does the SAME physical module jitter if you SWAP module numbers? +│ ├── Jitter follows the PHYSICAL module → Hardware issue with that module +│ └── Jitter follows the MODULE NUMBER → Software/config issue for that slot +``` + +--- + +### Branch A: One or More Module Angles Are Wrong + +#### Test A1: Verify Angle Offsets + +**Procedure**: +1. Physically align all four wheels to point **perfectly straight forward** (use a straight-edge) +2. Read the CANcoder values for each module from SmartDashboard +3. Compare to the `angleOffset` values in `Constants.java`: + +| Module | CAN IDs (drive, angle, encoder) | angleOffset (Constants.java) | CANcoder Reading (wheels straight) | Match? | +|---|---|---|---|---| +| 0 - Front Left | 6, 5, 7 | 31.46° | ___ | ___ | +| 1 - Front Right | 9, 8, 10 | 49.57° | ___ | ___ | +| 2 - Back Right | 12, 11, 13 | 33.13° | ___ | ___ | +| 3 - Back Left | 15, 14, 16 | 8.52° | ___ | ___ | + +4. **The CANcoder reading when wheels are straight should equal the `angleOffset`**. If they don't match, you've found your problem. + +**CRITICAL CHECK — Preferences Override**: The code reads `angleOffset` from `Preferences` first (stored on the roboRIO, survives code deploys). If someone previously ran `saveModuleOffsets()` or manually set a Preference, it will **silently override** the values in `Constants.java`: + +```java +// From SwerveModule.java constructor: +double storedOffset = + Preferences.getDouble(angleOffsetPreferenceKey, moduleConstants.angleOffset()); +angleOffset = Rotation2d.fromDegrees(normalizeDegrees(storedOffset)); +``` + +To check: Open the Preferences viewer in SmartDashboard/Shuffleboard and look for keys like `Swerve/Module0/AngleOffsetDegrees` through `Swerve/Module3/AngleOffsetDegrees`. If they exist, those values are being used instead of Constants.java. + +**Fix**: Either delete the Preferences entries to use Constants.java defaults, or use `saveModuleOffsets()` with all wheels physically pointed straight to write correct values. + +#### Test A2: Verify `resetToAbsolute()` Succeeds at Startup + +The CANcoder is configured to update at only 1 Hz. There is a 1-second `Timer.delay()` before `resetToAbsolute()` runs. If the CANcoder hasn't sent its first reading within that window, the integrated encoder will be calibrated to a stale or zero value. + +**Procedure**: +1. Power cycle the robot +2. Immediately check `Mod X Integrated` vs `Mod X Cancoder` for each module +3. Power cycle again and re-check — are the values consistent? + +If values are inconsistent between power cycles, the 1-second delay may not be enough. Try temporarily increasing the CANcoder update frequency (e.g., to 10 Hz) or increasing the delay to 2 seconds. + +#### Test A3: Verify Module Location / Kinematics Consistency + +Your kinematics setup and ModuleData locations use **swapped variable names** for X and Y: + +```java +// swerveKinematics uses (halfTrackWidth, halfWheelBase): +new Translation2d(halfTrackWidth, halfWheelBase), // Front left + +// But FRONT_LEFT (used in ModuleData) uses (halfWheelBase, halfTrackWidth): +public static final Translation2d FRONT_LEFT = new Translation2d(halfWheelBase, halfTrackWidth); +``` + +In WPILib's coordinate system: `Translation2d(x, y)` where +X = forward, +Y = left. The first argument should be the **forward/backward** distance (halfWheelBase) and the second should be the **left/right** distance (halfTrackWidth). + +Since `halfTrackWidth` and `halfWheelBase` are **both the same value** (27/2 inches), this doesn't cause a numerical bug today. But the `swerveKinematics` definition has them **backwards** compared to the `ModuleData` locations — and the kinematics is what actually controls the wheel calculations. Verify which is correct for your physical robot. + +#### Test A4: Verify Module Ordering + +Confirm that the module order in `swerveKinematics` matches the module order in `moduleData`: + +| Index | swerveKinematics position | moduleData label | Physical location | +|---|---|---|---| +| 0 | (+X, +Y) = Front Left | Front Left | ✓ or ✗? | +| 1 | (+X, -Y) = Front Right | Front Right | ✓ or ✗? | +| 2 | (-X, -Y) = Back Right | Back Right | ✓ or ✗? | +| 3 | (-X, +Y) = Back Left | Back Left | ✓ or ✗? | + +If the CAN IDs in `moduleData` don't match the physical module at the kinematics position, the robot will drive incorrectly. + +--- + +### Branch B: One or More Module Speeds Are Wrong + +#### Test B1: Free-Spin Speed Test + +**Procedure**: +1. Lift the robot so all wheels are off the ground +2. Command straight forward at ~50% speed +3. Visually observe: Are all four wheels spinning at the same speed and same direction? +4. Check telemetry: Are all four velocity readings similar? + +| Observation | Suggests | +|---|---| +| One wheel spins backwards | `driveInvert` is wrong for that module, or motor phase wiring is swapped | +| One wheel spins noticeably slower | Mechanical drag, motor issue, or different gear ratio on that module | +| Speeds match on the bench but not on the ground | Mechanical issue (wheel diameter difference, tire wear, weight distribution) | + +#### Test B2: Open Loop vs Closed Loop + +Your `drive()` method currently always uses **open loop** (percent output): + +```java +// In SwerveSubsystem.drive(): +driveFromChassisSpeeds(desiredSpeeds, true); // true = open loop +``` + +In open loop mode, motor variations, friction differences, and battery voltage sag cause speed differences between modules — this is a **known source of drift** for swerve drives. + +**Procedure**: +1. Temporarily change `true` to `false` in the `drive()` method to use closed-loop velocity control +2. Drive straight and observe — does drift improve? + +| Result | Conclusion | +|---|---| +| Drift significantly improves | The drift was caused by motor/friction variation. Open loop can't compensate. Consider using closed-loop for competition. | +| Drift doesn't change | The root cause is elsewhere (angle error, kinematics, gyro, mechanical) | + +**Note**: Closed-loop control requires properly tuned `driveKP/KI/KD` and feedforward values (`driveKS`, `driveKV`, `driveKA`). The current values are marked `//to calculate` and `//to tune`, so they may need characterization first. + +--- + +### Branch C: Gyro / Heading Problem (Field-Oriented Only) + +#### Test C1: Robot-Centric vs Field-Oriented + +This is a **quick binary test** — do it early. + +**Procedure**: +1. Switch to robot-centric mode (hold the robot-centric button) +2. Drive straight forward +3. **Does it still drift?** + +| Result | Conclusion | +|---|---| +| **Still drifts** | The gyro is NOT the problem. Go to Branches A, B, or D. | +| **Drift disappears** | The gyro heading is wrong, causing field-oriented math to rotate the chassis speed vector. Go to Test C2. | + +#### Test C2: Gyro Drift Check + +**Procedure**: +1. Place the robot still on a flat surface, enable and zero the gyro +2. Watch `Pigeon Yaw` on SmartDashboard for 60 seconds without touching the robot +3. Note any drift in the yaw reading + +| Result | Conclusion | +|---|---| +| Yaw stays stable (< 0.5° drift in 60s) | Pigeon is fine — heading problem is likely from initialization (`zeroGyro()` alliance logic) | +| Yaw drifts noticeably | Pigeon may be faulty, poorly mounted (vibrations), or needs recalibration | + +#### Test C3: Verify Gyro Zero/Alliance Logic + +Check the `zeroGyro()` method — it sets yaw to 180° for Red alliance: + +```java +public void zeroGyro() { + if (FieldConstants.isRedAlliance()){ + pigeon.setYaw(180); + } else { + pigeon.setYaw(0); + } +} +``` + +If the alliance isn't set correctly (e.g., in practice mode with no FMS), the gyro could initialize to the wrong heading. Verify what `DriverStation.getAlliance()` returns during your testing. + +--- + +### Branch D: Mechanical / Physical + +#### Test D1: Wheel Inspection + +Inspect all four modules for: + +| Check | Module 0 (FL) | Module 1 (FR) | Module 2 (BR) | Module 3 (BL) | +|---|---|---|---|---| +| Same tire type? | | | | | +| Even tire wear? | | | | | +| Same wheel diameter (calipers)? | | | | | +| Wheel makes ground contact? | | | | | +| Spins freely by hand (no binding)? | | | | | +| Module rotates freely (no binding)? | | | | | +| All bolts tight? | | | | | + +#### Test D2: Physical Module Swap Test + +This is the **definitive test** to separate hardware from software for module-specific issues. + +**Procedure**: +1. Pick the jittering module and one healthy module +2. Physically swap their positions on the robot +3. Update the CAN IDs in `Constants.java` `moduleData` to match the new physical positions +4. Deploy and test + +| Result | Conclusion | +|---|---| +| Jitter follows the **physical module** to its new position | **Hardware issue** with that module (motor, encoder, wiring, mechanical) | +| Jitter stays at the **original position** (now with a different physical module) | **Software/config issue** for that module slot (angleOffset, CAN ID, Preferences entry) | + +--- + +### Branch E: Jitter with Robot Disabled + +#### Test E1: Disabled Jitter Check + +**Procedure**: +1. Power on the robot but do NOT enable +2. Watch the suspected jittering module — does it jitter? + +| Result | Conclusion | +|---|---| +| Jitters while disabled | NOT a code issue. The motor controller is doing something on its own — check for electrical noise, bad wiring, or a faulty motor controller. | +| Only jitters when enabled | Code is causing the jitter. Continue to Branches F and G. | + +--- + +### Branch F: PID / Tuning Causing Jitter + +#### Test F1: Disable Angle PID + +**Procedure**: +1. Temporarily set `angleKP = 0.0` in Constants.java (disables angle position control) +2. Deploy and enable — does the module still jitter? + +| Result | Conclusion | +|---|---| +| Jitter stops | The PID is causing oscillation. Either `angleKP` is too high for that module, or the angle setpoint is constantly changing. | +| Jitter continues | Something else is commanding the motor. Check for competing commands or direct motor `.set()` calls. | + +**Note**: With `angleKP = 0`, the modules won't hold their angle — only use this as a diagnostic test, not during driving. + +#### Test F2: Log Angle Error for the Jittering Module + +Add temporary logging to see what the PID is doing: + +```java +// Temporary debug logging in SwerveModule.setAngle(): +private void setAngle(SwerveModuleState desiredState){ + Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.SwerveConstants.maxSpeed * 0.01)) + ? lastAngle : desiredState.angle; + + // TEMPORARY DEBUG: Log angle error for this module + double error = angle.getDegrees() - getAngle().getDegrees(); + SmartDashboard.putNumber("Mod " + moduleNumber + " Angle Error", error); + SmartDashboard.putNumber("Mod " + moduleNumber + " Angle Target", angle.getDegrees()); + + angleController.setReference(angle.getDegrees(), ControlType.kPosition); + lastAngle = angle; +} +``` + +**What you're looking for**: + +| Observation | Suggests | +|---|---| +| Error oscillates rapidly around zero (e.g., +0.5° / -0.5°) | Classic PID oscillation — `angleKP` is too aggressive, or there's mechanical backlash | +| Error is consistently non-zero (e.g., always +3°) | The module can't reach its target — possible mechanical binding, wrong conversion factor, or encoder issue | +| Target angle itself is changing rapidly | Something upstream is sending rapidly changing commands — check joystick deadband, check if the low-speed threshold is working | + +#### Test F3: Verify the Low-Speed Angle Lock + +The `setAngle()` method has a threshold that should prevent angle changes when the robot is nearly stopped: + +```java +Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.SwerveConstants.maxSpeed * 0.01)) + ? lastAngle : desiredState.angle; +``` + +This means: if speed ≤ 1% of max (≤ 0.03 m/s), hold the last angle instead of accepting new angle commands. This should prevent jitter at rest. + +**Verify**: Add logging to confirm this threshold is actually triggering when the robot is at rest. If `desiredState.speedMetersPerSecond` is slightly above the threshold (even 0.04 m/s due to joystick noise), the lock won't engage. + +--- + +### Branch G: Something Commands Movement at Rest + +#### Test G1: Verify Joystick Deadband + +The deadband is set to 0.1 (10%): + +```java +public static final double inputDeadband = .1; +``` + +**Procedure**: +1. Add temporary logging in `TeleopSwerve.execute()`: + +```java +SmartDashboard.putNumber("Raw X", m_translationSupplier.getAsDouble()); +SmartDashboard.putNumber("Raw Y", m_strafeSupplier.getAsDouble()); +SmartDashboard.putNumber("Raw Rot", m_rotationSupplier.getAsDouble()); +SmartDashboard.putNumber("Filtered X", xVal); +SmartDashboard.putNumber("Filtered Y", yVal); +SmartDashboard.putNumber("Filtered Rot", rotationVal); +``` + +2. Release the joystick completely and observe: + - Are the raw values exactly 0? (Most joysticks have slight offset) + - Are the filtered values 0 after deadband? (They should be) + +| Observation | Suggests | +|---|---| +| Raw values are non-zero but filtered values are 0 | Deadband is working correctly — not the cause | +| Filtered values are non-zero at rest | Joystick offset exceeds deadband — increase deadband or recalibrate joystick | +| All values are 0 but module still jitters | The jitter source is not TeleopSwerve — check for competing commands | + +#### Test G2: Check for Competing Commands + +Verify that no other command is accidentally controlling the swerve subsystem: + +1. In `RobotContainer.java`, check what is set as the default command for `SwerveSubsystem` +2. Check if `AutoAlign` or any other command could be scheduled during teleop +3. Look for any button bindings that trigger swerve-related commands without requiring the swerve subsystem (which would bypass the command scheduler's conflict resolution) + +--- + +## Phase 3: Confirm + +Once you've identified a suspected root cause: + +1. **Document the hypothesis**: Write it down clearly (e.g., "We believe Module 2 jitters because its `angleOffset` stored in Preferences is incorrect") +2. **Predict the outcome**: Before making the fix, write down what you expect to see (e.g., "After correcting the offset, Module 2's angle error should stay within ±0.5° at rest") +3. **Make ONE change** to fix it +4. **Re-run Tests 1.1 and 1.2** to verify the symptom is gone +5. **Revert the change** and verify the symptom **returns** — this confirms causation, not just correlation +6. **Re-apply the fix** permanently + +--- + +## Automated Diagnostic Test Framework + +The `jmm-test-support` branch includes an automated diagnostic test framework that can run many of the tests in this methodology **programmatically** — eliminating joystick variability and producing repeatable, quantitative results. + +### How to Use the Framework + +1. **Switch to Test mode** on the Driver Station (not Teleop or Auto) +2. Open SmartDashboard — a **Test Selector** dropdown appears under `DiagnosticTests/` +3. Select a test, configure its parameters, and press **Start** +4. Results appear in the console output and on SmartDashboard under `DiagnosticTests/[TestName]/Results/` + +### Available Automated Tests + +| Test | Framework Command | Replaces Manual Test | What It Does | +|---|---|---|---| +| **Swerve Alignment Test** | `SwerveAlignmentTestCommand` | **A1** (Verify Angle Offsets) | Commands all 4 modules to the same angle(s) and measures how accurately each module reaches the target. Tests at 0°, 90°, 180°, 270° to detect angle-dependent errors. Reports per-module error, cross-module spread, and integrated-vs-absolute encoder comparison. | +| **Swerve Straight Line Test** | `SwerveStraightLineTestCommand` | **1.2, B1, B2** (Driving Straight, Speed Test, Open/Closed Loop) | Bypasses the joystick and feeds exact ChassisSpeeds through `driveFromChassisSpeeds()`. Records per-module angles and velocities at regular intervals, plus gyro heading drift. Supports toggling between open-loop and closed-loop control. | +| **Swerve Angle Drift Test** | `SwerveAngleDriftTestCommand` | **F2** (Log Angle Error) | Cycles a single module between a target angle and zero N times, comparing relative (integrated) to absolute (CANcoder) encoder at each stop. Quantifies encoder drift over many cycles. | +| **LED State Test** | `LedStateTestCommand` | *(N/A — LED diagnostics)* | Tests CandleSubsystem LED states independently. | + +### When to Use Automated Tests vs Manual Tests + +| Use Automated Tests When... | Use Manual Tests When... | +|---|---| +| You need **repeatable**, **quantitative** data | You need to observe **physical behavior** (binding, noise, vibration) | +| You want to **eliminate joystick variability** | You need to test **joystick-specific** behavior (deadband, input scaling) | +| You want to **compare before/after** a code change | You need to check **disabled behavior** (Test E1) | +| You want to **test specific modules** in isolation | You need to **physically swap modules** (Test D2) | +| You want to test **open-loop vs closed-loop** with identical inputs | You need to inspect **mechanical components** (Test D1) | + +### Recommended Automated Testing Workflow + +Run these tests in order when debugging drift/jitter: + +1. **Swerve Alignment Test** (multi-angle mode) — 2 minutes + - If any module shows error > 2°: bad angleOffset or failed calibration → fix before proceeding + - If cross-module spread > 5° at any angle: modules disagree → calibration issue +2. **Swerve Angle Drift Test** on the jittering module — 1 minute + - If total drift > 5° over 10 cycles: encoder drift issue + - If drift is minimal: jitter is likely PID tuning, not encoder drift +3. **Swerve Straight Line Test** (open-loop) — 5 seconds + settle time + - Check module angle errors (should all be ≈ 0°) + - Check velocity spread between modules + - Check gyro heading drift +4. **Swerve Straight Line Test** (closed-loop) — 5 seconds + settle time + - Compare velocity spread to open-loop result + - If velocity spread drops significantly: motor variation (use closed-loop for competition) + +--- + +## Recommended Testing Order (Priority Queue) + +Based on the symptoms described, the code review, and available automated tests, here is the recommended order of investigation — starting with the highest-probability, lowest-effort tests: + +| Priority | Test | Time | Why This First | +|---|---|---|---| +| **1** | **C1: Robot-centric vs field-oriented** | 30 sec | Instantly eliminates or implicates the gyro. Almost zero effort. | +| **2** | **E1: Disabled jitter check** | 30 sec | Instantly tells you if the jitter is code or electrical. | +| **3** | 🤖 **Swerve Alignment Test** (automated) | 2 min | Replaces manual A1. Checks all 4 modules at 4 angles, reports per-module errors. Most common cause of drift. Also checks Preferences override. | +| **4** | **1.3: Startup calibration values** | 5 min | Verifies `resetToAbsolute()` succeeds. With 1 Hz CANcoder updates, there's a race condition at startup. | +| **5** | 🤖 **Swerve Angle Drift Test** (automated, on jittering module) | 1 min | Replaces manual F2. Quantifies encoder drift and directly diagnoses jitter root cause. | +| **6** | 🤖 **Swerve Straight Line Test — Open Loop** (automated) | 10 sec | Replaces manual B1 + 1.2. Repeatable straight-line test with exact inputs. Reports per-module angles, velocities, and gyro drift. | +| **7** | 🤖 **Swerve Straight Line Test — Closed Loop** (automated) | 10 sec | Replaces manual B2. Compare to open-loop result. If drift disappears, cause is motor variation. | +| **8** | **G1: Verify joystick deadband** | 10 min | Rules out joystick noise as a jitter source. (Must be manual — involves joystick hardware.) | +| **9** | **A3/A4: Verify kinematics & module ordering** | 15 min | Cross-check that CAN IDs, physical positions, and kinematics array are all consistent. | +| **10** | **D2: Physical module swap** | 30 min | Definitive hardware vs software test. Higher effort, so do it after ruling out easier causes. | + +--- + +## Data Recording Template + +Use this template for EVERY test. Fill it out before and after each test. Keep all completed templates together as a log. + +``` +═══════════════════════════════════════════════════════ +Test ID: [e.g., A1] +Test Name: [e.g., Verify Angle Offsets] +Date/Time: _______________ +Tester(s): _______________ + +HYPOTHESIS: +"If _________ then we expect to see _________" + +SETUP: +- Robot state: [disabled / enabled-teleop / enabled-auto] +- Wheels: [on ground / elevated] +- Code changes: [none / describe changes] + +RAW DATA: +Module 0 (FL): _______________ +Module 1 (FR): _______________ +Module 2 (BR): _______________ +Module 3 (BL): _______________ +Pigeon Yaw: _______________ +Other: _______________ + +RESULT: +[Describe what you observed] + +CONCLUSION: +- Eliminates: _______________ +- Implicates: _______________ + +NEXT STEP: +[Which test to run next based on this result] +═══════════════════════════════════════════════════════ +``` + +--- + +## Code-Specific Areas of Suspicion + +Based on reviewing the 2026 codebase, these specific code areas warrant scrutiny during the investigation. These are not conclusions — they are starting points guided by the code review. + +### 1. Preferences Override for Angle Offsets +**File**: `SwerveModule.java`, constructor +**Risk**: A previously-saved bad offset in Preferences silently overrides `Constants.java`. +**Action**: Check Preferences for `Swerve/ModuleX/AngleOffsetDegrees` keys. + +### 2. CANcoder Update Frequency vs Startup Timing +**File**: `SwerveModule.java`, constructor +**Risk**: CANcoder is set to 1 Hz updates. `resetToAbsolute()` runs after a 1-second delay. If the first CANcoder reading hasn't arrived, calibration uses stale data. +**Action**: Test 1.3 will reveal this. Consider increasing update frequency to 10+ Hz during startup, then reducing after calibration. + +### 3. Open-Loop Drive Control +**File**: `SwerveSubsystem.java`, `drive()` method +**Risk**: Open-loop control (percent output) cannot compensate for motor-to-motor variation, friction differences, or battery sag. This is a known source of drift. +**Action**: Test B2 will reveal this. Consider switching to closed-loop for competition driving. + +### 4. Kinematics Variable Naming Inconsistency +**File**: `Constants.java`, `swerveKinematics` vs `FRONT_LEFT` etc. +**Risk**: `swerveKinematics` uses `(halfTrackWidth, halfWheelBase)` while `ModuleData` locations use `(halfWheelBase, halfTrackWidth)` — X and Y are swapped. Currently harmless because both values are equal (27/2 inches), but indicates confusion about the coordinate system. +**Action**: Verify which is correct and make consistent. Would become a bug if the robot weren't square. + +### 5. Custom `optimize()` Method +**File**: `SwerveModule.java` +**Risk**: The custom `optimize()` method replaced the 2024 codebase's `OnboardModuleState.optimize()`. Subtle differences in angle wrapping or optimization logic could cause modules to occasionally choose the wrong rotation direction. +**Action**: Compare behavior of the custom `optimize()` against WPILib's built-in `SwerveModuleState.optimize()` for edge cases (angles near ±180°, angles near ±90°). + +### 6. Angle PID Tuning +**File**: `Constants.java` +**Risk**: `angleKP = 0.01` is quite low. If it's too low for the mechanical load, the module may not reach its target angle accurately, causing drift. If it's borderline for one module but OK for others (due to friction differences), it could explain why only one module jitters. +**Action**: Test F2 (log angle error) will reveal this. + +### 7. Missing Open-Loop Ramp Rate +**File**: `SwerveModule.java`, `configDriveMotor()` +**Risk**: `Constants.java` defines `openLoopRamp = 0.25` and `closedLoopRamp = 0.0`, but neither appears to be applied in the `SparkFlexConfig` for the drive motor. If the 2024 code applied ramp rates and 2026 doesn't, motor response characteristics changed. +**Action**: Check the 2024 `SwerveModule.java` to see if ramp rates were applied there. diff --git a/docs/diagnostic-tests/diagnostic-test-elastic-gui.md b/docs/diagnostic-tests/diagnostic-test-elastic-gui.md new file mode 100644 index 0000000..b7b01a1 --- /dev/null +++ b/docs/diagnostic-tests/diagnostic-test-elastic-gui.md @@ -0,0 +1,539 @@ +# Dashboard Interaction Design: Elastic GUI Compatibility + +> Extracted from [diagnostic-testing-framework-design.md](diagnostic-testing-framework-design.md) to keep that document focused on the core framework design. + +## Problem Statement + +The diagnostic test framework requires users to **select**, **execute**, and **cancel** tests through a dashboard GUI. The current implementation uses two SmartDashboard mechanisms: + +1. **`SendableChooser`** (dropdown) — for test selection +2. **`SmartDashboard.putBoolean()`** (boolean value) — for the Start/Cancel trigger + +This works in the **WPILib Sim GUI**, where booleans render as an interactive dropdown (`false`/`true`) — switching the value to `true` acts like a button click. However, in the **Elastic dashboard**, raw boolean NetworkTables entries render as **read-only status indicators** (red circle when false, green when true) with no mechanism for the user to change the value. The `SendableChooser` dropdown works correctly in both dashboards. + +The result is that tests **cannot be started or cancelled** from the Elastic GUI, only from the Sim GUI. + +## Root Cause + +Elastic and Sim GUI handle NetworkTables value types differently: + +| NT Value Type | Sim GUI Rendering | Elastic Rendering | +|---|---|---| +| Raw Boolean | Interactive dropdown (false/true) | Read-only status indicator (red/green) | +| `SendableChooser` | Interactive dropdown | Interactive dropdown ✓ | +| `Command` (Sendable) | Interactive button | Interactive button ✓ | + +The key insight is that Elastic **does** support interactive widgets — but only for recognized `Sendable` types (like `SendableChooser` and `Command`), not for raw boolean values. + +## How WPILib Command Buttons Work + +When a `Command` is published via `SmartDashboard.putData("key", command)`, WPILib's `Command.initSendable()` publishes a NetworkTables subtable with: +- **`.type`** = `"Command"` (tells the dashboard which widget to render) +- **`.name`** property (read-only string — the command's name) +- **`running`** property (read-write boolean): getter returns `isScheduled()`, setter calls `schedule()` when set to `true` and `cancel()` when set to `false` + +Dashboards that recognize the `"Command"` Sendable type (Shuffleboard, Elastic) render this as an **interactive toggle button**: click to schedule the command, click again to cancel it. This is the standard WPILib pattern for testing commands from dashboards. + +## Current Architecture Issues + +Beyond the Elastic compatibility problem, the current design has an additional concern: + +**New instance creation on every execution**: The current `DiagnosticTestManager.startSelectedTest()` calls `selectedTest.createTest(robotContainer)` to create a fresh `Command` instance each time a test is run. While this works, it's unnecessary — the test commands already fully reset their state in `initialize()` and read fresh parameters from SmartDashboard each run. Persistent instances that are reused across multiple runs would be cleaner. + +## Options Evaluated + +### Option 1: Custom SendableButton Class + +Create a class implementing `Sendable` that mimics the Command Sendable protocol (`SmartDashboardType = "Command"`, `running` boolean property) but instead of scheduling itself, calls back to the manager to start/cancel tests. + +| Pros | Cons | +|------|------| +| Minimal change to existing architecture | Fragile: mimics undocumented internal protocol | +| Decouples button from Command lifecycle | Maintenance risk if WPILib/Elastic changes protocol | +| | Doesn't address instance-per-execution concern | +| | Reinvents what Command already provides | + +**Verdict**: Unnecessarily complex. If we're going to use the Command Sendable protocol anyway, we should just use an actual Command. + +### Option 2: Publish Selected Test Command Directly as a Button + +When the user selects a test from the chooser, publish that test's `Command` instance via `SmartDashboard.putData("DiagnosticTests/StartTest", selectedTestCommand)`. Elastic renders it as a clickable button. + +| Pros | Cons | +|------|------| +| Native WPILib pattern, well-supported | Must re-call `putData()` with different Sendable when selection changes | +| Elastic has built-in Command widget | Re-publishing may cause `SendableBuilder` rebinding issues | +| Tests are directly the button | Manager loses some control over lifecycle tracking | +| Simple conceptual model | | + +**Verdict**: Elegant in theory, but re-publishing different Sendables to the same NetworkTables key is not a well-tested pattern in WPILib and could cause subtle bugs with stale property bindings. + +### Option 3: Proxy Command Pattern ⭐ (Recommended) + +Create a **single persistent** `TestRunnerCommand` published **once** via `SmartDashboard.putData()`. Elastic renders it as a button. When clicked: +- `initialize()` → reads the chooser, gets the selected persistent test instance, schedules it via `CommandScheduler` +- `execute()` → monitors the inner test, updates status display +- `isFinished()` → returns `true` when the inner test completes +- `end(interrupted)` → if interrupted (button clicked again), cancels the inner test + +Combined with **persistent test instances** created once and reused across runs. + +| Pros | Cons | +|------|------| +| Published once — no Sendable rebinding issues | Proxy + actual test = two commands scheduled simultaneously | +| Native Command button in Elastic | Slightly more indirection | +| Eliminates create-new-instance-each-time pattern | | +| Preserves select → configure parameters → run workflow | | +| Manager still handles status display | | +| Works in Sim GUI, Elastic, and Shuffleboard | | + +**How it works in practice:** + +1. User selects "Swerve Angle Drift Test" from the chooser dropdown (works in Elastic ✓) +2. Manager detects selection change, calls `initializeParameters()` on the persistent test instance +3. User adjusts parameters on the dashboard +4. User clicks the "Start Test" button (Command widget in Elastic ✓) +5. `TestRunnerCommand.initialize()` fires → reads chooser → schedules the persistent `SwerveAngleDriftTestCommand` → button label changes to **"Cancel Test"** +6. Both the proxy and the actual test are scheduled (no subsystem conflict — the proxy has no subsystem requirements) +7. Button shows "Cancel Test" label in running state in Elastic +8. Test completes naturally → proxy detects it → `isFinished()` returns `true` → button label changes back to **"Start Test"** → button resets +9. **OR** user clicks "Cancel Test" button → proxy is cancelled → `end(true)` cancels the inner test → button label changes back to **"Start Test"** + +**Why two commands can coexist:** The `TestRunnerCommand` does not call `addRequirements()` for any subsystem. The actual test commands require their respective subsystems (e.g., `SwerveSubsystem`). Since they don't share subsystem requirements, the `CommandScheduler` runs them independently without conflict. + +### Option 4: Replace Boolean with a SendableChooser for Action + +Replace the boolean trigger with a `SendableChooser` offering "Idle" / "Start" / "Cancel" options. + +| Pros | Cons | +|------|------| +| Minimal code change | Very clunky UX (selecting "Start" from a dropdown) | +| SendableChooser dropdown works in Elastic | Semantically wrong — an action selector, not a value selector | +| | Need to detect and reset after selection | +| | Error-prone (what if user selects "Start" twice?) | + +**Verdict**: A hack. Works technically but provides poor UX. + +### Option 5: Publish ALL Tests as Individual Command Buttons + +Publish every test as a separate Command button on the dashboard: `SmartDashboard.putData("Test: Swerve Drift", testA)`, `SmartDashboard.putData("Test: LED State", testB)`, etc. + +| Pros | Cons | +|------|------| +| Simplest mental model | Loses the select → configure → run workflow | +| Each test is its own button | Dashboard clutter with many buttons | +| No chooser needed | Hard to show per-test parameters (which section belongs to which?) | +| | No centralized status tracking | +| | Adding tests = more dashboard clutter | + +**Verdict**: Works for a small number of tests but doesn't scale and loses the framework's centralized management benefits. + +## Recommended Approach: Option 3 (Proxy Command + Persistent Instances) + +### Why This Option + +1. **Solves the Elastic issue** using native WPILib Command button rendering +2. **Eliminates instance-per-execution** — tests are created once and reused +3. **Preserves the existing workflow** (select → configure parameters → run) +4. **Publishes one Command once** to SmartDashboard — no rebinding concerns +5. **Works across all dashboards** (Sim GUI, Elastic, Shuffleboard) + +### Persistent Instances Are Already Supported + +The existing test commands already support reuse without modification: +- **State reset in `initialize()`**: All tests fully reset their state machine, counters, and results arrays in `initialize()` (e.g., `currentCycle = 0`, `currentState = MOVING_TO_TARGET`, `testResults = new TestCycleResult[...]`) +- **Parameters read in `initialize()`**: All tests read fresh values from SmartDashboard each run via `TestDashboard.getParam*()` calls +- **Subsystem requirements set once**: `addRequirements()` is called in the constructor and persists across runs + +### Required Changes + +| Component | Change | +|-----------|--------| +| **New: `TestRunnerCommand`** | Simple proxy Command in `frc.lib.test`. Published once as a button. Delegates scheduling to the selected test. Has no subsystem requirements. | +| **`DiagnosticTestManager`** | Create all test instances once in constructor (stored in `Map`). Replace `putBoolean(KEY_START_CANCEL_TEST)` with `putData()` for the proxy command. Remove boolean polling from `periodic()`. Keep all status monitoring and display logic. | +| **`DiagnosticTestRegistry`** | No changes needed — factory method still used, but instances are cached by the manager. | +| **Individual test commands** | No changes needed. | + +### Updated SmartDashboard Layout + +``` +DiagnosticTests/ + ├── TestSelector/ (SendableChooser - dropdown, unchanged) + ├── StartTest/ (Command button - replaces Boolean Start-Cancel) + ├── CurrentTest/ (String - name of running test or "None") + ├── CurrentTest Description/ (String - description of the selected test) + ├── CurrentTest Status/ (String - Idle, Running, Complete, Cancelled, Error) + └── Message/ (String - status messages and error information) +``` + +### TestRunnerCommand Sketch + +```java +/** + * Proxy command that bridges the Elastic dashboard button to test execution. + * + * Published once via SmartDashboard.putData(), this command appears as a + * clickable button in Elastic. When clicked, it schedules the currently + * selected test from the chooser. When clicked again (or the test completes), + * the button resets. + * + * This command has no subsystem requirements, so it can run concurrently + * with the actual test command without scheduling conflicts. + */ +public class TestRunnerCommand extends Command { + private static final String LABEL_START = "Start Test"; + private static final String LABEL_CANCEL = "Cancel Test"; + + private final Supplier selectedTestSupplier; + private Command activeTest; + + @Override + public void initialize() { + activeTest = selectedTestSupplier.get(); + if (activeTest != null) { + CommandScheduler.getInstance().schedule(activeTest); + setName(LABEL_CANCEL); // Button label → "Cancel Test" + } + } + + @Override + public boolean isFinished() { + return activeTest == null + || !CommandScheduler.getInstance().isScheduled(activeTest); + } + + @Override + public void end(boolean interrupted) { + if (interrupted && activeTest != null) { + activeTest.cancel(); + } + activeTest = null; + setName(LABEL_START); // Button label → "Start Test" + } +} +``` + +### Dynamic Button Label + +The `TestRunnerCommand` changes its button label between **"Start Test"** and **"Cancel Test"** by calling `setName()` in `initialize()` and `end()`. This works because WPILib's `Command.initSendable()` registers the `.name` property with a getter: + +```java +builder.addStringProperty(".name", this::getName, null); +``` + +The `SendableBuilder` periodically polls `getName()` and publishes the current value to NetworkTables. When `setName("Cancel Test")` is called, the next poll picks up the change and pushes it to the dashboard. This provides clear feedback to the operator about what action the button will perform. + +**Verification note:** While the mechanism is sound (the `.name` value in NetworkTables *will* update), whether Elastic specifically re-renders the button label text in real-time is an Elastic implementation detail that should be confirmed during end-to-end testing. If Elastic caches the label on widget creation, the `CurrentTest Status` string ("Running" / "Idle") still provides the operator with state information. The `running` boolean state (which controls button toggle appearance) is unaffected — that always updates correctly. + +### Sim GUI Compatibility + +This approach maintains full Sim GUI compatibility: +- The `SendableChooser` still appears as a dropdown in Sim GUI +- The `Command` button appears as an interactive widget in Sim GUI (Commands are rendered as toggleable entries) +- The dynamic button label (`setName()`) is reflected in SimGUI's command widget display +- All status strings continue to update normally + +## Implementation Design: Option 3 Details + +This section provides the detailed design needed to implement Option 3 (Proxy Command + Persistent Instances). It covers concrete class designs, specific changes to existing code, interaction flows, and edge case handling. + +### TestRunnerCommand — Detailed Design + +**Location:** `frc.lib.test.TestRunnerCommand` (new file) + +The `TestRunnerCommand` is a thin proxy `Command` that bridges the Elastic dashboard button to test execution. It is published **once** via `SmartDashboard.putData()` during `DiagnosticTestManager` construction, and Elastic renders it as a native clickable toggle button. + +**Key design properties:** +- **No subsystem requirements** — never calls `addRequirements()`, so it coexists with any test command without scheduling conflicts +- **Delegates to a supplier** — receives a `Supplier` from the manager that returns the currently selected persistent test instance +- **Stateless between runs** — holds a reference to the inner test only while active; clears it in `end()` +- **Dynamic button label** — calls `setName("Cancel Test")` in `initialize()` and `setName("Start Test")` in `end()`, so the dashboard button label reflects the current action. This works because WPILib's `Command.initSendable()` registers `.name` with a getter (`this::getName`) that the `SendableBuilder` polls periodically — name changes propagate to NetworkTables automatically. + +**Full class design:** + +```java +package frc.lib.test; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import java.util.function.Supplier; + +/** + * Proxy command that bridges the Elastic dashboard button to test execution. + * + * Published once via SmartDashboard.putData(), this command appears as a + * clickable button in Elastic (and SimGUI). When clicked, it schedules + * the currently selected test. When clicked again (or the test completes), + * the button resets. + * + * This command has no subsystem requirements, so it runs concurrently + * with the actual test command without scheduling conflicts. + */ +public class TestRunnerCommand extends Command { + + private final Supplier selectedTestSupplier; + private Command runningTest; + + /** + * Creates a new TestRunnerCommand. + * + * @param selectedTestSupplier Supplier that returns the persistent test + * instance to run. Returns null if no test is selected or available. + * Called once per button click (in initialize()). + */ + private static final String LABEL_START = "Start Test"; + private static final String LABEL_CANCEL = "Cancel Test"; + + public TestRunnerCommand(Supplier selectedTestSupplier) { + this.selectedTestSupplier = selectedTestSupplier; + setName(LABEL_START); // Initial button label shown in Elastic + } + + @Override + public void initialize() { + runningTest = selectedTestSupplier.get(); + if (runningTest != null) { + CommandScheduler.getInstance().schedule(runningTest); + setName(LABEL_CANCEL); // Button label changes to "Cancel Test" + } + } + + @Override + public void execute() { + // Nothing — just waiting for the inner test to finish + } + + @Override + public boolean isFinished() { + // Finished when: no test was selected, or inner test is done + return runningTest == null + || !CommandScheduler.getInstance().isScheduled(runningTest); + } + + @Override + public void end(boolean interrupted) { + if (interrupted && runningTest != null + && CommandScheduler.getInstance().isScheduled(runningTest)) { + runningTest.cancel(); + } + runningTest = null; + setName(LABEL_START); // Button label reverts to "Start Test" + } +} +``` + +**Lifecycle summary:** + +| Event | What Happens | +|-------|-------------| +| User clicks "Start Test" button | Elastic sets `running=true` → CommandScheduler schedules the proxy → `initialize()` fires → supplier returns selected test → proxy schedules it → `setName("Cancel Test")` → button label updates | +| Test runs | Both proxy and inner test are scheduled concurrently. Proxy's `execute()` is a no-op. Proxy's `isFinished()` polls whether inner test is still scheduled. Button shows "Cancel Test". | +| Test completes naturally | Inner test's `isFinished()` returns true → scheduler ends it → proxy detects it's gone → proxy's `isFinished()` returns true → proxy ends → `setName("Start Test")` → button label and state reset | +| User clicks "Cancel Test" button | Elastic sets `running=false` → scheduler cancels the proxy → proxy's `end(true)` fires → proxy cancels inner test → inner test's `end(true)` fires → `setName("Start Test")` → button label resets | +| No test selected | Supplier returns null → `isFinished()` returns true immediately → proxy ends → `setName("Start Test")` → button resets | + +### Persistent Test Instance Management + +The `DiagnosticTestManager` creates all test instances **once** in its constructor and stores them in a map. When the user clicks "Start Test", the supplier returns the existing instance — no new instance is created. + +**Instance cache:** +```java +private final Map testInstances = new HashMap<>(); + +// In constructor: +for (DiagnosticTestRegistry entry : DiagnosticTestRegistry.values()) { + try { + Command test = entry.createTest(robotContainer); + testInstances.put(entry, test); + } catch (Exception e) { + System.err.println("Failed to create test: " + entry.getDisplayName() + + ": " + e.getMessage()); + // Test won't be available, but other tests still work + } +} +``` + +**Why reuse works:** All existing test commands already support reuse without modification: +- **State reset in `initialize()`**: Tests fully reset state machines, counters, and result arrays (e.g., `currentCycle = 0`, `currentState = MOVING_TO_TARGET`, `testResults = new TestCycleResult[...]`) +- **Parameters read in `initialize()`**: Tests read fresh values from SmartDashboard via `TestDashboard.getParam*()` each run +- **Subsystem requirements set once**: `addRequirements()` is called in the constructor and persists across runs +- **No constructor-only state**: All mutable test state is reset in `initialize()`, not in the constructor + +**Parameter initialization change:** When the user selects a test from the chooser, the manager calls `initializeParameters()` directly on the persistent instance — no throwaway instance needed: +```java +Command testCommand = testInstances.get(selectedTest); +if (testCommand instanceof DiagnosticTest) { + DiagnosticTest diagnosticTest = (DiagnosticTest) testCommand; + diagnosticTest.initializeParameters(); + SmartDashboard.putString(KEY_DESCRIPTION, diagnosticTest.getTestDescription()); +} +``` + +**Instance lifetime:** Instances exist for the duration of a single test mode session (`testInit()` → `testExit()`). When `testExit()` is called, the manager is garbage collected along with all instances. Fresh instances are created on the next `testInit()`. + +### DiagnosticTestManager — Required Changes + +**Summary of changes:** + +| Category | Action | Details | +|----------|--------|---------| +| **Add** | `testInstances` field | `Map` — persistent instance cache | +| **Add** | `runTestCommand` field | `TestRunnerCommand` — proxy published as dashboard button | +| **Add** | `getSelectedTestInstance()` method | Supplier method for the proxy — returns selected persistent instance, sets `activeTest` | +| **Rename** | `KEY_START_CANCEL_TEST` | → `KEY_START_TEST` (`"DiagnosticTests/StartTest"`) | +| **Remove** | `handleStartCancelButton()` | Button handling moved to proxy | +| **Remove** | `startSelectedTest()` | Test scheduling moved to proxy | +| **Remove** | `cancelActiveTest()` | Test cancellation moved to proxy | +| **Modify** | Constructor | Create instance cache and proxy command | +| **Modify** | `initializeDashboard()` | Replace `putBoolean` with `putData` for proxy command | +| **Modify** | `periodic()` | Remove boolean polling block | +| **Modify** | `updateTestSelection()` | Use persistent instances for parameter init (no throwaway instance) | +| **Modify** | `cleanup()` | Cancel proxy, remove boolean cleanup | + +**No changes needed** to `updateTestStatus()` — it already monitors `activeTest` and detects scheduled/unscheduled/finished transitions. The existing logic correctly identifies COMPLETE (not scheduled + isFinished) vs CANCELLED (not scheduled + not finished). + +**`getSelectedTestInstance()` — new supplier method:** + +This method is called by the proxy's `initialize()` when the user clicks the button. It replaces the functionality of the current `startSelectedTest()`: + +```java +/** + * Supplier method for TestRunnerCommand. Returns the currently selected + * persistent test instance, or null if no valid test is available. + * + * Also updates manager state (activeTest, currentStatus, display) to + * begin tracking the test — equivalent to the old startSelectedTest(). + */ +private Command getSelectedTestInstance() { + DiagnosticTestRegistry selected = testChooser.getSelected(); + if (selected == null) { + SmartDashboard.putString(KEY_MESSAGE, + "No test selected. Select a test from the dropdown."); + return null; + } + + Command test = testInstances.get(selected); + if (test == null) { + SmartDashboard.putString(KEY_MESSAGE, + "Test not available: " + selected.getDisplayName()); + return null; + } + + // Update manager tracking state (replaces startSelectedTest() logic) + activeTest = test; + suppressedSelectionWarning = null; + currentStatus = TestStatus.RUNNING; + SmartDashboard.putString(KEY_CURRENT_TEST, selected.getDisplayName()); + SmartDashboard.putString(KEY_TEST_STATUS, currentStatus.toString()); + SmartDashboard.putString(KEY_MESSAGE, + "Test running: " + selected.getDisplayName()); + System.out.println("Started test: " + selected.getDisplayName()); + + return test; +} +``` + +**Why `activeTest` is set inside the supplier:** +The supplier runs inside `TestRunnerCommand.initialize()`, which is called by the CommandScheduler during `robotPeriodic()`. By the time `testPeriodic()` calls `manager.periodic()`, `activeTest` is already set and the inner test is scheduled. The existing `updateTestStatus()` logic then monitors the test normally — no race condition or one-cycle gap. + +### Interaction Flows + +**Flow 1: Start a test** +``` +1. User selects "Swerve Angle Drift Test" from chooser dropdown +2. manager.periodic() → updateTestSelection() detects selection change +3. Manager calls initializeParameters() on persistent instance +4. Parameters appear in SmartDashboard; user adjusts if desired +5. User clicks "Start Test" button in Elastic +6. Elastic sets running=true → CommandScheduler schedules proxy +7. proxy.initialize() → calls getSelectedTestInstance() +8. Supplier returns persistent test, sets activeTest + status=RUNNING +9. Proxy schedules the inner test via CommandScheduler +10. Proxy calls setName("Cancel Test") → button label updates in Elastic +11. Both proxy and test running concurrently +12. manager.periodic() → updateTestStatus() confirms RUNNING status +``` + +**Flow 2: Test completes naturally** +``` +1. Test's isFinished() returns true → CommandScheduler ends it → test.end(false) +2. Proxy's isFinished() detects inner test gone → returns true +3. CommandScheduler ends the proxy → proxy.end(false) + → clears runningTest → setName("Start Test") +4. Button label reverts to "Start Test" and button resets in Elastic +5. manager.periodic() → updateTestStatus() + → activeTest not scheduled + isFinished=true → status=COMPLETE + → Message: "Test completed: ..." +``` + +**Flow 3: User cancels a running test** +``` +1. User clicks "Cancel Test" button while test is running +2. Elastic sets running=false → CommandScheduler cancels the proxy +3. proxy.end(true) → cancels inner test → test.end(true) → setName("Start Test") +4. Button label reverts to "Start Test" and button resets in Elastic +5. manager.periodic() → updateTestStatus() + → activeTest not scheduled + not finished → status=CANCELLED + → Message: "Test cancelled unexpectedly: ..." +``` + +Note: In Flow 3, the manager's `updateTestStatus()` sees the test was cancelled "unexpectedly" (not via our old button). This is accurate — from the manager's perspective, the cancellation came from outside (the proxy). The status message could say "Test cancelled: ..." instead. This is a minor wording difference that can be adjusted if desired. + +**Flow 4: Selection change during a running test** +``` +1. User changes chooser while a test is running +2. manager.periodic() → updateTestSelection() + → Detects selection change + isTestRunning()=true + → Prints warning, keeps "Current Test" showing running test name + → Does NOT initialize parameters for new selection + → lastSelectedTest NOT updated (preserves change detection) +3. Test completes (or is cancelled) → status updates normally +4. Next manager.periodic() → updateTestSelection() + → Detects selectedTest != lastSelectedTest (still different) + → isTestRunning()=false now + → Initializes parameters for the newly selected test +``` + +### Edge Cases and Error Handling + +| Edge Case | Behavior | +|-----------|----------| +| **Button clicked, no test selected** | Supplier returns null → proxy `isFinished()` returns true immediately → button resets → message: "No test selected" | +| **Button clicked, test instance failed to create** | Instance not in map → supplier returns null → same as above → message: "Test not available" | +| **Rapid double-click** | First click schedules proxy → second click cancels proxy → proxy cancels inner test. Test may have run for only one 20ms cycle. Acceptable behavior — same as cancelling. | +| **`testExit()` while test is running** | `cleanup()` cancels proxy and `activeTest` → both end cleanly | +| **Test throws exception during execution** | CommandScheduler catches it and ends the test. `updateTestStatus()` detects the test is no longer scheduled and sets appropriate status. | +| **Same test re-run** | Supplier returns same persistent instance → CommandScheduler schedules it again (allowed after previous run ended) → test's `initialize()` resets all state → runs fresh | +| **All test instances fail to create** | `testInstances` map is empty → any button click → supplier returns null → button resets with error message. Chooser still shows test names. | + +## Implementation Steps + +Ordered steps to implement Option 3. Each step results in a compilable, testable state. + +**Step 1: Create `TestRunnerCommand`** +- New file: `src/main/java/frc/lib/test/TestRunnerCommand.java` +- Implements the proxy command as designed above +- No changes to existing files in this step + +**Step 2: Modify `DiagnosticTestManager`** +- Add `testInstances` map field and populate in constructor +- Add `runTestCommand` field, create in constructor with `this::getSelectedTestInstance` +- Add `getSelectedTestInstance()` method (new supplier) +- `initializeDashboard()`: Replace `putBoolean(KEY_START_CANCEL_TEST, false)` with `putData(KEY_START_TEST, runTestCommand)` +- `periodic()`: Remove the boolean polling block (lines that read `KEY_START_CANCEL_TEST` and call `handleStartCancelButton()`) +- `updateTestSelection()`: Replace throwaway-instance try/catch block with direct call to `testInstances.get(selectedTest)` and null check +- `cleanup()`: Cancel `runTestCommand`, remove `putBoolean` cleanup +- Rename constant: `KEY_START_CANCEL_TEST` → `KEY_START_TEST = DASHBOARD_PREFIX + "StartTest"` +- Delete methods: `handleStartCancelButton()`, `startSelectedTest()`, `cancelActiveTest()` + +**Step 3: Verify no external references to the boolean key** +- The `KEY_START_CANCEL_TEST` boolean is only referenced inside `DiagnosticTestManager` — no changes needed to test commands, `DiagnosticTestRegistry`, `Robot.java`, or `TestDashboard` + +**Step 4: Test end-to-end** +- Verify in SimGUI: chooser dropdown works, "Start Test" Command button appears, test starts/stops correctly +- Verify button label changes to "Cancel Test" while a test is running, and reverts to "Start Test" after completion or cancellation +- Verify selection change triggers parameter initialization +- Verify cancel (click button during running test) +- Verify test completion updates status to "Complete" +- Verify re-running the same test works (persistent instance reuse) +- If Elastic is available: verify the button label updates dynamically in the Elastic Command widget \ No newline at end of file diff --git a/docs/diagnostic-tests/diagnostic-testing-framework-design.md b/docs/diagnostic-tests/diagnostic-testing-framework-design.md new file mode 100644 index 0000000..8873311 --- /dev/null +++ b/docs/diagnostic-tests/diagnostic-testing-framework-design.md @@ -0,0 +1,1101 @@ +# Diagnostic Testing Framework Design Proposal + +## Executive Summary + +This proposal outlines a framework for managing and executing diagnostic tests for the FRC robot. The design leverages WPILib's test mode to maintain clear separation between production and test code, provides a centralized registry for test discovery, and integrates with SmartDashboard for user-friendly test selection and execution. + +## Research Summary: FRC Best Practices + +Based on WPILib documentation and FRC community practices: + +1. **Test Mode Architecture**: WPILib provides built-in `testInit()`, `testPeriodic()`, and `testEnd()` methods specifically for system verification. This provides natural separation from production code. + +2. **Command-Based Testing**: Since FRC uses command-based architecture, diagnostic tests should be implemented as Commands. This allows: + - Automatic lifecycle management via CommandScheduler + - Easy integration with SmartDashboard using `SmartDashboard.putData()` + - Consistent patterns with production code + +3. **Dashboard Integration**: Two recommended approaches: + - **Command Buttons**: Use `SmartDashboard.putData()` to expose commands as buttons (easiest method per WPILib docs) + - **Dropdown Selection**: Use `SendableChooser` for test selection (more scalable for many tests) + +4. **Code Organization**: + - Framework code in `frc.lib.test` package (reusable infrastructure) + - Test implementations in `frc.robot.test` package (test-specific code) + - Production code never imports test classes + - Tests depend on framework (one-way dependency) + +## Proposed Architecture + +### 1. Core Components + +#### `DiagnosticTest` Interface +- **Purpose**: Defines a common contract for all diagnostic tests +- **Design**: Interface (not abstract class) - tests already extend `Command` +- **Methods**: + - `String getTestName()` - Display name for dashboard + - `String getTestDescription()` - Optional description (default implementation provided) + - `void initializeParameters()` - Initialize SmartDashboard parameters (optional, default empty) + - `DiagnosticTestResult getResult()` - Get test result (optional, default returns null) +- **Rationale**: Allows tests to remain Commands while providing metadata and result reporting + +#### `DiagnosticTestResult` Class +- **Purpose**: Standardized result reporting for tests +- **Fields**: + - `TestStatus status` - Test completion status (SUCCESS, FAILED, CANCELLED, ERROR) + - `String message` - Human-readable result message + - `Map metrics` - Key-value pairs for test metrics (supports Double, Integer, Boolean, String) + - `List warnings` - List of warnings encountered +- **Convenience Methods**: + - `boolean passed()` - Returns true if status == SUCCESS (for backward compatibility) +- **Benefits**: Consistent result format, supports multiple metric types, distinguishes between failure and cancellation + +#### `TestParameterHelper` Utility Class +- **Purpose**: Simplifies reading parameters from SmartDashboard +- **Methods**: + - `static double getDouble(String key, double defaultValue)` + - `static int getInt(String key, int defaultValue)` + - `static boolean getBoolean(String key, boolean defaultValue)` + - `static void putDouble(String key, double defaultValue)` + - `static void putInt(String key, int defaultValue)` + - `static void putBoolean(String key, boolean defaultValue)` +- **Benefits**: Reduces boilerplate, handles parameter initialization automatically + +#### `DiagnosticTestRegistry` +- **Purpose**: Central registry of available diagnostic tests +- **Design Options**: + - **Option A (Recommended)**: Enum-based registry + - Compile-time registration + - Type-safe + - Easy to see all tests in one place + - Each enum value has factory method to create test instance + - **Option B**: Dynamic registration + - Runtime discovery via reflection + - More flexible but less type-safe + - Harder to debug +- **Recommendation**: Start with Enum-based, can migrate to dynamic later if needed +- **Provides**: Test metadata, factory methods, test discovery + +#### `DiagnosticTestManager` +- **Purpose**: Manages test lifecycle and SmartDashboard integration +- **Responsibilities**: + - Initialize dashboard UI in `testInit()` + - Initialize parameters for selected test + - Poll dashboard for test selection/start requests in `testPeriodic()` + - Read parameters from SmartDashboard when creating test + - Coordinate test execution via CommandScheduler + - Update status displays + - Display test results in SmartDashboard + - Cleanup in `testEnd()` +- **Lifetime**: Created in `testInit()`, used in `testPeriodic()`, cleaned up in `testEnd()` +- **Dependencies**: Only depends on `RobotContainer` (to access subsystems), no production code imports test classes +- **Result Display**: Automatically reads `getResult()` from completed tests and displays in SmartDashboard + +### 2. Integration Points + +#### Robot Class (`testInit`, `testPeriodic`, `testEnd`) +```java +private DiagnosticTestManager testManager; + +@Override +public void testInit() { + CommandScheduler.getInstance().cancelAll(); + testManager = new DiagnosticTestManager(m_robotContainer); +} + +@Override +public void testPeriodic() { + if (testManager != null) { + testManager.periodic(); + } +} + +@Override +public void testEnd() { + if (testManager != null) { + testManager.cleanup(); + testManager = null; + } +} +``` + +#### SmartDashboard Layout +``` +DiagnosticTests/ + ├── TestSelector/ (SendableChooser - dropdown with all available tests) + ├── StartTest/ (Boolean - button that toggles to start selected test) + ├── CurrentTest/ (String - name of currently running test) + ├── TestStatus/ (String - status: Idle, Running, Complete, Error, Cancelled) + ├── Parameters/ + │ └── [TestName]/ + │ ├── ModuleNumber/ (example parameter) + │ ├── TestAngle/ (example parameter) + │ └── NumberOfCycles/ (example parameter) + └── Results/ + ├── LastTest/ (String - name of last completed test) + ├── Passed/ (Boolean - did last test pass?) + ├── Message/ (String - result message) + ├── Metrics/ (Table - key-value pairs of numeric results) + └── Warnings/ (String array - list of warnings) +``` + +**User Flow**: +1. User selects test from dropdown +2. Test parameters appear in `Parameters/[TestName]/` section +3. User configures parameters as needed +4. User clicks "StartTest" button +5. Manager detects button press, reads parameters, creates test instance, schedules it +6. Status updates automatically as test runs +7. Test completes and results appear in `Results/` section +8. User can run test again with different parameters + +### 3. Design Principles + +1. **Separation of Concerns**: + - Framework code in `frc.lib.test` package (reusable infrastructure) + - Test implementations in `frc.robot.test` package (test-specific code) + - Production code never imports test classes + - Test code can access production subsystems (one-way dependency) + - Tests depend on framework (one-way dependency: tests → framework) + +2. **Extensibility**: + - Easy to add new tests by implementing `DiagnosticTest` interface + - Registry pattern allows dynamic or static test registration + - No changes to production code when adding tests + +3. **User Experience**: + - Clear test selection via dropdown + - One-click test execution + - Real-time status updates + - Test results visible in SmartDashboard + +4. **Maintainability**: + - Each test is self-contained + - Common utilities in `SwerveModuleTestUtils` pattern + - Clear naming conventions + +## Example Code Structure + +### DiagnosticTest Interface +```java +public interface DiagnosticTest { + String getTestName(); + default String getTestDescription() { + return "No description available"; + } + default void initializeParameters() { + // Override to set up SmartDashboard parameters + } + default DiagnosticTestResult getResult() { + return null; // Override to return test results + } +} +``` + +### TestStatus Enum +```java +public enum TestStatus { + SUCCESS, // Test completed and passed + FAILED, // Test completed but failed (e.g., drift exceeded threshold) + CANCELLED, // Test was interrupted/cancelled before completion + ERROR // Test encountered an exception or error +} +``` + +### MetricValue Class +```java +/** + * Wrapper class for test metric values that preserves type information. + * Supports Double, Integer, Boolean, and String types. + */ +public class MetricValue { + private final Object value; + private final MetricType type; // enum: DOUBLE, INT, BOOLEAN, STRING + + // Factory methods + public static MetricValue of(double value) { ... } + public static MetricValue of(int value) { ... } + public static MetricValue of(boolean value) { ... } + public static MetricValue of(String value) { ... } + + // Type-safe getters (throw exception if wrong type) + public double getDouble() { ... } + public int getInt() { ... } + public boolean getBoolean() { ... } + public String getString() { ... } + + // Type checking + public MetricType getType() { ... } + public boolean isDouble() { ... } + // ... similar for other types +} +``` + +### DiagnosticTestResult Class +```java +public class DiagnosticTestResult { + private final TestStatus status; + private final String message; + private final Map metrics; + private final List warnings; + + // Constructor and getters + public TestStatus getStatus() { ... } + public boolean passed() { return status == TestStatus.SUCCESS; } // Convenience method + public String getMessage() { ... } + public Map getMetrics() { ... } + public List getWarnings() { ... } + + // Factory methods + public static DiagnosticTestResult pass(String message) { ... } + public static DiagnosticTestResult fail(String message) { ... } + public static DiagnosticTestResult cancelled(String message) { ... } + public static DiagnosticTestResult error(String message) { ... } + public static DiagnosticTestResult passWithMetrics(String message, Map metrics) { ... } + + // Builder for complex results + public static Builder builder() { ... } +} +``` + +### TestParameterHelper Utility +```java +public class TestParameterHelper { + // Initialize parameter with default value (creates SmartDashboard entry) + public static void putDouble(String key, double defaultValue) { + SmartDashboard.putNumber(key, defaultValue); + } + + // Read parameter from SmartDashboard + public static double getDouble(String key, double defaultValue) { + return SmartDashboard.getNumber(key, defaultValue); + } + + // Similar methods for int, boolean, String +} +``` + +### DiagnosticTestRegistry (Enum-based) +```java +public enum DiagnosticTestRegistry { + SWERVE_ANGLE_DRIFT("Swerve Angle Drift Test", + "Tests encoder drift by rotating module...") { + @Override + public DiagnosticTest createTest(RobotContainer robotContainer) { + // Parameters are read from SmartDashboard in test's initialize() + return new SwerveAngleDriftTestCommand(robotContainer.getSwerveSubsystem()); + } + }; + + // ... enum implementation +} +``` + +### SwerveAngleDriftTestCommand Integration (Simplified) +```java +public class SwerveAngleDriftTestCommand extends Command implements DiagnosticTest { + private static final String PARAM_PREFIX = "DiagnosticTests/Parameters/Swerve Angle Drift Test/"; + private DiagnosticTestResult result; + private SwerveSubsystem swerve; + private int moduleNumber; + private double testAngle; + private int cycles; + private int currentCycle = 0; + private boolean testComplete = false; + + public SwerveAngleDriftTestCommand(SwerveSubsystem swerve) { + this.swerve = swerve; + addRequirements(swerve); + } + + @Override + public void initializeParameters() { + // Set up default parameters in SmartDashboard + TestParameterHelper.putInt(PARAM_PREFIX + "ModuleNumber", 0); + TestParameterHelper.putDouble(PARAM_PREFIX + "TestAngle", 90.0); + TestParameterHelper.putInt(PARAM_PREFIX + "NumberOfCycles", 10); + } + + @Override + public void initialize() { + // Read parameters from SmartDashboard using TestParameterHelper + // Initialize test state variables + // Start the first test cycle or begin test execution + } + + @Override + public void execute() { + // ACTUAL TEST LOGIC GOES HERE + // This runs every 20ms while the test is active + // - Check conditions (e.g., has module reached target angle?) + // - Record measurements + // - Manage test state machine (moving to target, holding, moving to zero, etc.) + // - Transition between states + // - Update cycle counter or test progress + } + + @Override + public boolean isFinished() { + // Return true when test is complete (all cycles finished, duration elapsed, etc.) + return testComplete; + } + + @Override + public void end(boolean interrupted) { + // Stop any robot motion + // Create DiagnosticTestResult with metrics and pass/fail status + // Store result for getResult() to return + } + + @Override + public String getTestName() { + return "Swerve Angle Drift Test"; + } + + @Override + public DiagnosticTestResult getResult() { + return result; + } +} +``` + +### Example: Creating a New Test (Minimal Effort) +```java +public class SimpleMotorTest extends Command implements DiagnosticTest { + private static final String PARAM_PREFIX = "DiagnosticTests/Parameters/Simple Motor Test/"; + private final SwerveSubsystem swerve; + private DiagnosticTestResult result; + + // Test state + private double targetSpeed; + private double duration; + private double startTime; + private double maxSpeed = 0.0; + private double totalSpeed = 0.0; + private int sampleCount = 0; + + public SimpleMotorTest(SwerveSubsystem swerve) { + this.swerve = swerve; + addRequirements(swerve); + } + + @Override + public void initializeParameters() { + TestParameterHelper.putDouble(PARAM_PREFIX + "TargetSpeed", 0.5); + TestParameterHelper.putDouble(PARAM_PREFIX + "Duration", 2.0); + } + + @Override + public void initialize() { + // Read parameters from SmartDashboard using TestParameterHelper + // Initialize test state variables (startTime, counters, etc.) + // Start motor or begin test execution + } + + @Override + public void execute() { + // ACTUAL TEST LOGIC GOES HERE + // This runs every 20ms while the test is active + // - Measure current motor speed or other sensor values + // - Track metrics (max speed, average speed, etc.) + // - Update test state + } + + @Override + public boolean isFinished() { + // Return true when test duration has elapsed or test conditions are met + return Timer.getFPGATimestamp() - startTime >= duration; + } + + @Override + public void end(boolean interrupted) { + // Stop any robot motion + // Create DiagnosticTestResult with collected metrics + // Store result for getResult() to return + } + + @Override + public String getTestName() { return "Simple Motor Test"; } + + @Override + public DiagnosticTestResult getResult() { return result; } +} +``` + +### Robot Class Integration +```java +private DiagnosticTestManager testManager; + +@Override +public void testInit() { + CommandScheduler.getInstance().cancelAll(); + testManager = new DiagnosticTestManager(m_robotContainer); +} + +@Override +public void testPeriodic() { + if (testManager != null) { + testManager.periodic(); + } +} + +@Override +public void testEnd() { + if (testManager != null) { + testManager.cleanup(); + testManager = null; + } +} +``` + +### Optional Finite State Machine (FSM) Support for Complex Tests + +Many diagnostic tests follow a state machine pattern (e.g., move to position → hold → measure → repeat). While simple tests don't need state machines, complex multi-phase tests benefit from structured state management. + +#### When to Use FSM Support + +**Use FSM support when your test has:** +- Multiple distinct phases (e.g., moving, holding, measuring) +- Timeouts or waiting conditions +- Repeated cycles or iterations +- Complex state transitions + +**Skip FSM support for:** +- Simple duration-based tests (run motor for X seconds) +- Single-phase tests (set position → measure → done) +- Continuous measurement tests + +#### StateMachineTestCommand Base Class + +The framework provides an optional base class that handles common FSM boilerplate: + +**Design Decision: Minimal Base Class** +- Provides only essential timing helpers to reduce boilerplate +- Keeps framework lightweight - simple tests don't pay unnecessary cost +- Complex tests can add their own logging/validation as needed +- Examples demonstrate best practices for common patterns + +```java +/** + * Optional base class for tests that use finite state machines. + * Provides state tracking, timing, and transition helpers. + * + * Simple tests can extend Command directly - this is only for complex multi-phase tests. + * + * Tests define their own state enums and use the provided helpers for timing and transitions. + * + * Design Philosophy: Minimal but useful. Provides timing helpers to reduce boilerplate, + * but doesn't enforce a specific FSM pattern. Test authors have full control over state + * management while benefiting from common timing utilities. + */ +public abstract class StateMachineTestCommand extends Command implements DiagnosticTest { + + /** + * Timestamp when current state was entered. Protected so subclasses can access it. + */ + protected double stateStartTime = 0.0; + + /** + * Optional flag to enable state transition logging for debugging. + * Set to true in initialize() if you want automatic logging of state transitions. + */ + protected boolean enableStateLogging = false; + + /** + * Gets the elapsed time since entering the current state. + * Subclasses should call this after updating their stateStartTime. + */ + protected double getStateElapsedTime() { + return Timer.getFPGATimestamp() - stateStartTime; + } + + /** + * Helper method to record a state transition timestamp. + * Subclasses should call this when transitioning states. + * + * Optionally logs the transition if enableStateLogging is true. + */ + protected void recordStateTransition() { + stateStartTime = Timer.getFPGATimestamp(); + } + + /** + * Optional helper method for state transitions that includes logging. + * Subclasses can use this instead of manually calling recordStateTransition() + * if they want automatic logging. + * + * @param oldState The state being exited (for logging) + * @param newState The state being entered (for logging) + */ + protected void transitionTo(Object oldState, Object newState) { + if (enableStateLogging) { + System.out.println(String.format("State transition: %s -> %s", oldState, newState)); + } + recordStateTransition(); + } + + @Override + public void initialize() { + recordStateTransition(); // Record initialization time + // Subclasses should call super.initialize() then do their setup + } +} +``` + +**Usage Notes**: +- Subclasses define their own state enums (no base enum required) +- Subclasses manage their own state transitions (base class doesn't enforce a pattern) +- Timing helpers reduce boilerplate for common patterns +- Optional logging can be enabled per test if needed +- Examples show common patterns (state entry actions, transition validation, etc.) + +#### Example: FSM-Based Test + +```java +public class SwerveAngleDriftTestCommand extends StateMachineTestCommand { + // Define test-specific states (can use BaseTestState or define custom states) + private enum TestState { + INITIALIZING, + MOVING_TO_TARGET, + HOLDING_AT_TARGET, + MOVING_TO_ZERO, + HOLDING_AT_ZERO, + COMPLETE + } + + private TestState currentState = TestState.INITIALIZING; + private SwerveModule module; + private double testAngle; + private int cycles; + private int currentCycle = 0; + + // Override base class state tracking to use our custom enum + // Base class provides stateStartTime and getStateElapsedTime() helpers + + @Override + public void initialize() { + super.initialize(); // Initialize FSM base class (sets stateStartTime) + // Read parameters from SmartDashboard + // Initialize test state + transitionTo(TestState.MOVING_TO_TARGET); + } + + @Override + public void execute() { + double elapsed = getStateElapsedTime(); // Use base class helper + + switch (currentState) { + case MOVING_TO_TARGET: + if (isAtAngle(testAngle)) { + transitionTo(TestState.HOLDING_AT_TARGET); + } else if (elapsed > timeout) { + transitionTo(TestState.HOLDING_AT_TARGET); + } + break; + + case HOLDING_AT_TARGET: + if (elapsed >= holdTime) { + recordMeasurement(); + transitionTo(TestState.MOVING_TO_ZERO); + } + break; + + case MOVING_TO_ZERO: + if (isAtAngle(0.0)) { + transitionTo(TestState.HOLDING_AT_ZERO); + } else if (elapsed > timeout) { + transitionTo(TestState.HOLDING_AT_ZERO); + } + break; + + case HOLDING_AT_ZERO: + if (elapsed >= holdTime) { + completeCycle(); + if (currentCycle >= cycles) { + transitionTo(TestState.COMPLETE); + } else { + transitionTo(TestState.MOVING_TO_TARGET); + } + } + break; + + case COMPLETE: + break; + } + } + + /** + * Transitions to a new state and records the transition time. + * Uses base class helper to track state timing. + */ + private void transitionTo(TestState newState) { + currentState = newState; + recordStateTransition(); // Use base class helper + onStateEntered(newState); + } + + /** + * Called when entering a new state. Perform state-specific actions here. + */ + + private void onStateEntered(TestState state) { + switch (state) { + case MOVING_TO_TARGET: + module.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(testAngle)), false); + break; + case MOVING_TO_ZERO: + module.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0)), false); + break; + // ... other state entry actions + } + } + + @Override + public boolean isFinished() { + return currentState == TestState.COMPLETE; + } + + // ... rest of test implementation (getResult(), getTestName(), etc.) +} +``` + +#### Benefits of FSM Support + +- **Reduces Boilerplate**: No need to manually track state, timestamps, or transitions +- **Standardized Patterns**: Consistent state management across complex tests +- **Easier Debugging**: State transitions are explicit and traceable +- **Optional**: Simple tests don't pay the cost - only use when needed + +#### When NOT to Use FSM Support + +For simple tests, extending `Command` directly is perfectly fine: + +```java +public class SimpleMotorTest extends Command implements DiagnosticTest { + private double startTime; + private double duration; + + @Override + public void initialize() { + startTime = Timer.getFPGATimestamp(); + motor.set(0.5); + } + + @Override + public void execute() { + // Just measure - no state machine needed + recordMeasurement(); + } + + @Override + public boolean isFinished() { + return Timer.getFPGATimestamp() - startTime >= duration; + } +} +``` + +## Relationship to Existing Code + +### Current Test Infrastructure +- ✅ `SwerveAngleDriftTestCommand` - Well-designed test command +- ✅ `SwerveDriftTestManager` - Provides dashboard integration +- ✅ `SwerveModuleTestUtils` - Utility helpers for tests +- ✅ Test code in `frc.robot.test` package + +### How Framework Integrates + +**Decision: Framework Replaces SwerveDriftTestManager** ✅ +- Framework provides unified test management +- `SwerveDriftTestManager` will be removed after migration +- All tests use the same framework pattern +- Better consistency and maintainability + +### Impact on Existing Test +- `SwerveAngleDriftTestCommand` needs to: + 1. Extend `StateMachineTestCommand` instead of `Command` directly + 2. Implement `DiagnosticTest` interface (add methods) + 3. Add `initializeParameters()` to set up SmartDashboard parameters + 4. Read parameters from SmartDashboard in `initialize()` using `TestParameterHelper` + 5. Add `getResult()` to return `DiagnosticTestResult` with `TestStatus` and `MetricValue` metrics + 6. Use base class timing helpers (`getStateElapsedTime()`, `recordStateTransition()`) + 7. Remove direct SmartDashboard parameter setup (moved to `initializeParameters()`) + 8. Convert result metrics to use `MetricValue` wrapper +- Test logic remains mostly unchanged +- State machine pattern simplified with base class helpers +- Parameter management moves to framework pattern +- Result reporting becomes standardized with status enum and typed metrics +- `SwerveDriftTestManager` can be removed after migration + +## Implementation Plan + +### Phase 1: Core Framework (MVP) +1. Create `DiagnosticTestRegistry` (Enum-based) with factory methods +2. Create `DiagnosticTestManager` class: + - SmartDashboard UI initialization (dropdown, start button, status display) + - Test selection and parameter initialization (calls `initializeParameters()`) + - Test execution (creates test via registry, schedules via CommandScheduler) + - Test completion monitoring (tracks active test, updates status) + - Cleanup handling +3. Integrate `DiagnosticTestManager` into `Robot.testInit/testPeriodic/testEnd` +4. Create simple example test (e.g., motor speed test) to demonstrate framework usage +5. Test end-to-end flow with example test + +**Note**: The `DiagnosticTest` interface already exists. For MVP, tests are responsible for managing their own parameters and results (no `TestParameterHelper` or `DiagnosticTestResult` yet). + +### Phase 2: Migration +1. Refactor `SwerveAngleDriftTestCommand`: + - Implement `DiagnosticTest` interface + - Add `initializeParameters()` method (moves parameter setup from `SwerveDriftTestManager.initializeDashboard()`) + - Modify constructor to take only `SwerveSubsystem` (remove parameter arguments) + - Modify `initialize()` to read parameters from SmartDashboard (moves logic from `SwerveDriftTestManager.startTestFromDashboard()`) + - Keep existing test logic, state machine, and result reporting unchanged +2. Register `SwerveAngleDriftTestCommand` in `DiagnosticTestRegistry` +3. Test end-to-end flow (parameter configuration, execution, status display) +4. Remove `SwerveDriftTestManager` (replaced by framework) + +### Phase 3: Documentation & Examples +1. Document test creation workflow +2. Add inline code comments and JavaDoc +3. Create README for test authors +4. Document MVP limitations and future enhancements + +### Future Phases (Deferred) +The following components are deferred to later phases to keep the MVP simple: +- `DiagnosticTestResult` class - Tests currently report results directly to SmartDashboard +- `TestParameterHelper` utility class - Tests currently manage their own SmartDashboard parameters +- `StateMachineTestCommand` base class - Not needed for MVP; tests can extend `Command` directly +- Standardized `Results/` section in SmartDashboard - Tests use their own namespaces for now + +## Design Decisions & Alternatives + +### Test Result Status: Enum vs Boolean + +**Decision: TestStatus Enum** ✅ + +**Alternatives Considered**: +1. `boolean passed` - Simple but doesn't distinguish failure modes (rejected) +2. `TestStatus` enum - Provides clear distinction between SUCCESS, FAILED, CANCELLED, ERROR (chosen) + +**Rationale**: A simple `boolean passed` field doesn't distinguish between different failure modes: +- Test completed but failed (e.g., drift exceeded threshold) → `FAILED` +- Test was interrupted/cancelled → `CANCELLED` +- Test encountered an exception → `ERROR` +- Test completed successfully → `SUCCESS` + +**Benefits**: +- Clear distinction between failure modes +- Better error tracking and debugging +- Supports cancelled test handling +- Backward compatible via `passed()` convenience method + +**Tradeoffs**: +- More complex than boolean, but provides essential information +- Requires enum definition, but improves code clarity +- Slightly more verbose, but self-documenting + +**Implementation**: +- `TestStatus` enum with values: SUCCESS, FAILED, CANCELLED, ERROR +- `DiagnosticTestResult` uses `TestStatus status` instead of `boolean passed` +- Factory methods: `pass()`, `fail()`, `cancelled()`, `error()` +- Convenience method: `boolean passed()` returns `status == TestStatus.SUCCESS` + +### Metrics Storage: Type Safety vs Flexibility + +**Decision: MetricValue Wrapper Class** ✅ + +**Alternatives Considered**: +1. `Map` - Flexible but no type safety +2. Separate maps per type - Type-safe but complex API +3. `MetricValue` wrapper - Balanced approach (chosen) +4. Builder pattern - Type-safe but more complex implementation + +**Rationale**: `MetricValue` wrapper provides the best balance: +- **Type Safety**: Preserves type information, validates at runtime +- **Simplicity**: Single map, easy to iterate +- **Flexibility**: Supports Double, Integer, Boolean, String +- **Extensibility**: Can add formatting/display helpers later +- **Self-Documenting**: Type info preserved in the value + +**Tradeoffs**: +- Slightly more complex than `Map` (requires wrapper class) +- More type-safe than `Object` but requires runtime type checking +- Simpler API than separate maps per type +- Less compile-time safety than builder pattern, but more convenient + +**Implementation**: +- `MetricValue` class with factory methods: `of(double)`, `of(int)`, `of(boolean)`, `of(String)` +- Type-safe getters: `getDouble()`, `getInt()`, `getBoolean()`, `getString()` +- `Map metrics` in `DiagnosticTestResult` + +### StateMachineTestCommand: Minimal vs Full FSM Framework + +**Decision: Minimal Base Class with Optional Helpers** ✅ + +**Alternatives Considered**: +1. Minimal (timing only) - Chosen +2. Add state transition logging - Optional helper provided +3. Add state entry/exit callbacks - Too complex, not needed +4. Add transition validation - Too complex, not needed +5. Full FSM framework - Overkill for FRC tests + +**Rationale**: Keep it simple and lightweight: +- **Minimal Overhead**: Simple tests don't pay unnecessary cost +- **Essential Helpers**: Timing helpers reduce common boilerplate +- **Full Control**: Test authors manage their own state transitions +- **Optional Features**: Logging helper available but not required +- **Examples Show Patterns**: Best practices demonstrated in examples + +**Tradeoffs**: +- Less structure than full FSM framework, but more flexible +- Test authors write more code than with callbacks, but have full control +- No built-in validation, but tests can add their own if needed +- Examples demonstrate common patterns instead of enforcing them + +**Implementation**: +- Base class provides: `stateStartTime`, `getStateElapsedTime()`, `recordStateTransition()` +- Optional `enableStateLogging` flag for debugging +- Optional `transitionTo(oldState, newState)` helper with logging +- Subclasses define their own state enums and manage transitions +- Examples show common patterns (state entry actions, validation, etc.) + +### Cancelled Test Handling + +**Decision: Use TestStatus.CANCELLED** ✅ + +**Considerations**: +- Tests can be cancelled/interrupted at any time +- May have partial results that are still useful +- Should distinguish cancellation from failure +- Framework should handle cancellation gracefully + +**Implementation**: +- `TestStatus.CANCELLED` status for interrupted tests +- Tests can return partial results if available +- Framework checks `getResult()` after test ends (whether completed or cancelled) +- Results display clearly indicates cancellation status +- Warnings can include cancellation reason if available + +**Best Practices for Test Authors**: +- In `end(boolean interrupted)`, check `interrupted` parameter +- If interrupted, create result with `TestStatus.CANCELLED` +- Include partial metrics if available (e.g., "Completed 3 of 10 cycles") +- Add warning explaining why test was cancelled if known + +### Package Organization: Framework vs Tests + +**Decision: Framework in `frc.lib.test`, Tests in `frc.robot.test`** ✅ + +**Alternatives Considered**: +1. Everything in `frc.robot.test` - Simple but mixes framework with tests (rejected) +2. Framework in `frc.robot.test.framework` - Clear separation but deeper nesting (rejected) +3. Framework in `frc.lib.test` - Framework as reusable library code (chosen) +4. Framework in `frc.lib.test.framework` - Very clear but deeper nesting (rejected) + +**Rationale**: Separating framework from tests provides: +- **Clear Separation**: Framework is reusable infrastructure, tests are test-specific implementations +- **Follows Existing Pattern**: `frc.lib` already contains library/utility code (e.g., `LimelightHelpers`, `CANSparkUtil`) +- **Dependency Clarity**: Tests depend on framework (one-way: `frc.robot.test` → `frc.lib.test`) +- **Reusability**: Framework could be reused across projects or extracted as a library +- **Organization**: Makes it clear what's framework vs what's a specific test + +**Package Structure**: +``` +frc.lib.test/ + ├── DiagnosticTest (interface) + ├── DiagnosticTestResult + ├── TestStatus (enum) + ├── MetricValue + ├── MetricType (enum) + ├── TestParameterHelper + ├── DiagnosticTestRegistry (enum) + ├── DiagnosticTestManager + └── StateMachineTestCommand (base class) + +frc.robot.test/ + ├── SwerveAngleDriftTestCommand + ├── SwerveModuleTestUtils + └── [other test implementations] +``` + +**Dependencies**: +- `frc.robot.test` imports from `frc.lib.test` (tests use framework) +- `frc.lib.test` does NOT import from `frc.robot.test` (framework doesn't know about tests) +- Production code (`frc.robot.*`) does NOT import from either test package + +### Registry Pattern: Enum vs Dynamic + +**Decision: Enum-Based** ✅ +- ✅ Compile-time safety +- ✅ Easy to see all tests in one place +- ✅ IDE autocomplete support +- ✅ Simple factory methods +- ✅ Adding a test is just adding an enum value (minimal effort) +- ❌ Requires code change to add test (acceptable trade-off for safety) + +**Rationale**: Simpler, safer, and adding a test is just adding an enum value - not a significant burden. The type safety and IDE support are worth it. + +### Test Instantiation: Factory vs Direct Construction + +**Decision: Factory Method** ✅ +- Each enum value has a `createTest(RobotContainer)` method +- Allows tests to access subsystems via RobotContainer +- Parameters read from SmartDashboard when test is created +- ✅ Flexible - can create different test configurations +- ✅ Tests don't need to know about RobotContainer +- ✅ Parameters come from SmartDashboard, not hardcoded + +**Rationale**: Factory method pattern provides better flexibility and dependency management. Parameters are read from SmartDashboard, not hardcoded. + +### Dashboard UI: Dropdown vs Buttons + +**Decision: Dropdown + Start Button** ✅ +- ✅ Scales well to many tests +- ✅ Clear selection process +- ✅ Can show test descriptions +- ✅ Single start mechanism +- ✅ Parameters appear dynamically based on selected test + +**Rationale**: Dropdown scales better as you add more tests. Parameters can be shown contextually for the selected test. + +## Migration Strategy + +### Current State +- `SwerveAngleDriftTestCommand` exists and works +- `SwerveDriftTestManager` provides dashboard integration +- Test code is in `frc.robot.test` package ✅ + +### Migration Approach + +**Decision: Full Replacement** ✅ +1. Create framework components +2. Make `SwerveAngleDriftTestCommand` implement `DiagnosticTest` interface +3. Migrate parameter handling to use `TestParameterHelper` +4. Add result reporting using `DiagnosticTestResult` +5. Register in `DiagnosticTestRegistry` +6. Remove `SwerveDriftTestManager` (no longer needed) +7. Framework becomes the only way to run tests + +**Rationale**: Clean break, no legacy code to maintain, consistent approach from the start. + +## Design Decisions (Finalized) + +### 1. Test Parameters: Configurable via SmartDashboard ✅ +- Parameters are read from SmartDashboard when test is created +- Framework provides helper methods to read parameters with defaults +- Tests define their parameter keys in a standardized location +- Parameters appear in SmartDashboard under `DiagnosticTests/Parameters/[TestName]/` + +### 2. Test Results: Standardized Reporting ✅ +- Framework provides `DiagnosticTestResult` class for structured results +- Tests can report pass/fail, messages, and data +- Results automatically displayed in SmartDashboard +- Results summary view shows key metrics + +### 3. Multiple Test Execution: One at a Time ✅ +- Only one test runs at a time (simpler, safer) +- Framework cancels previous test if new one is started +- Can be extended later if needed + +### 4. Migration: Replace SwerveDriftTestManager ✅ +- Framework replaces `SwerveDriftTestManager` +- Unified approach for all tests +- `SwerveDriftTestManager` will be removed after migration + +## Benefits + +1. **Scalability**: Easy to add new diagnostic tests (just add enum value) +2. **Consistency**: All tests follow same pattern and interface +3. **Discoverability**: All tests visible in one place (dropdown) +4. **Maintainability**: Clear separation, easy to understand +5. **User-Friendly**: Simple dashboard interface for pit crew +6. **Type Safety**: Enum-based registry catches errors at compile time +7. **Separation**: Test code completely isolated from production code +8. **Easy Test Creation**: Minimal boilerplate - just implement interface, use helper utilities +9. **Standardized Results**: Consistent result format makes it easy to compare tests +10. **Parameter Management**: Framework handles parameter initialization and reading +11. **Low Learning Curve**: Simple patterns, clear examples, helper utilities reduce complexity + +## Making Test Creation Easy + +### Key Simplifications + +1. **TestParameterHelper**: One-line parameter setup and reading + ```java + TestParameterHelper.putDouble("key", 90.0); // Initialize + double value = TestParameterHelper.getDouble("key", 90.0); // Read + ``` + +2. **DiagnosticTestResult**: Simple factory methods for results + ```java + result = DiagnosticTestResult.pass("Test passed!"); + result = DiagnosticTestResult.fail("Test failed: reason"); + result = DiagnosticTestResult.cancelled("Test was cancelled"); + result = DiagnosticTestResult.error("Test encountered error: ..."); + + // With metrics (supports multiple types) + Map metrics = new HashMap<>(); + metrics.put("speed", MetricValue.of(5.2)); // Double + metrics.put("cycles", MetricValue.of(10)); // Integer + metrics.put("passed", MetricValue.of(true)); // Boolean + result = DiagnosticTestResult.passWithMetrics("Passed", metrics); + ``` + +3. **Minimal Interface**: Only 2-4 methods to implement + - `getTestName()` - Required + - `getTestDescription()` - Optional (has default) + - `initializeParameters()` - Optional (has default) + - `getResult()` - Optional (has default) + +4. **Clear Examples**: Template code for common patterns + - Simple pass/fail test + - Test with parameters + - Test with metrics + - Test with warnings + +5. **Automatic Dashboard Integration**: Framework handles: + - Parameter display + - Result display + - Status updates + - Test selection UI + +### Test Creation Workflow + +1. **Create test class**: Extend `Command`, implement `DiagnosticTest` +2. **Add metadata**: Implement `getTestName()` and optionally `getTestDescription()` +3. **Define parameters**: Override `initializeParameters()` to set up SmartDashboard +4. **Read parameters**: Use `TestParameterHelper` in `initialize()` or constructor +5. **Report results**: Set result in `end()` method using `DiagnosticTestResult` +6. **Register test**: Add enum value in `DiagnosticTestRegistry` + +That's it! Framework handles the rest. + +## Summary: Why This Design is Easy to Use + +### For Test Authors (Writing Tests) +- **Minimal Interface**: Only 2-4 methods to implement (most have defaults) +- **Helper Utilities**: `TestParameterHelper` eliminates boilerplate for parameters +- **Simple Results**: `DiagnosticTestResult` factory methods make reporting trivial +- **Clear Examples**: Template code shows exactly what to do +- **No Framework Knowledge Needed**: Just implement interface, use helpers, done + +### For Test Operators (Running Tests) +- **One Place for Everything**: All tests in dropdown, all parameters visible +- **No Configuration Files**: Everything in SmartDashboard, visual and immediate +- **Clear Status**: Always know what's running, what passed/failed +- **Standardized Results**: Same format for all tests, easy to understand +- **No Learning Curve**: Select test, set parameters, click start + +### For Framework Maintainers +- **Enum-Based Registry**: All tests visible in one place, type-safe +- **Clear Separation**: Test code isolated, no production dependencies +- **Extensible**: Easy to add features (test suites, result storage, etc.) +- **Well-Documented**: Clear patterns, examples, and JavaDoc + +### Key Simplifications +1. **Parameter Management**: Framework handles SmartDashboard setup/reading +2. **Result Display**: Framework automatically shows results in dashboard +3. **Test Discovery**: Enum registry makes all tests visible automatically +4. **Lifecycle Management**: CommandScheduler handles test execution +5. **Error Handling**: Framework provides consistent error reporting + +This design prioritizes **ease of use** and **low learning curve** while maintaining **type safety** and **code quality**. diff --git a/docs/diagnostic-tests/test-dashboard-design.md b/docs/diagnostic-tests/test-dashboard-design.md new file mode 100644 index 0000000..16b100a --- /dev/null +++ b/docs/diagnostic-tests/test-dashboard-design.md @@ -0,0 +1,1090 @@ +# TestDashboard: Assessment and Design + +## Executive Summary + +Based on analysis of the two existing test implementations (`SwerveAngleDriftTestCommand` and `LedStateTestCommand`), introducing `TestDashboard` would provide **significant benefits** in code quality, maintainability, and developer experience. The current implementation shows clear patterns of repetitive boilerplate that could be eliminated. + +The proposed design **completely eliminates the need for both `PARAM_PREFIX` and `RESULT_PREFIX` constants** by automatically constructing prefixes from the test instance using `getTestName()`. This provides a type-safe, consistent API with symmetric naming that reduces boilerplate and prevents common errors. + +The class is named `TestDashboard` - a simple, concise name that clearly indicates it's for SmartDashboard interactions in diagnostic tests. + +## Current Implementation Analysis + +### SwerveAngleDriftTestCommand + +**Parameter Initialization (lines 136-143):** +```java +private static final String PARAM_PREFIX = "DiagnosticTests/Swerve Angle Drift Test/Parameters/"; + +SmartDashboard.putNumber(PARAM_PREFIX + "ModuleNumber", 0); +SmartDashboard.putNumber(PARAM_PREFIX + "Angle", 90.0); +SmartDashboard.putNumber(PARAM_PREFIX + "NumberOfCycles", 10); +SmartDashboard.putNumber(PARAM_PREFIX + "AngleTolerance", 2.0); +SmartDashboard.putNumber(PARAM_PREFIX + "MaxWaitTime", 1.0); +SmartDashboard.putNumber(PARAM_PREFIX + "MinHoldTime", 0.5); +``` + +**Parameter Reading (lines 160-165):** +```java +moduleNumber = (int) SmartDashboard.getNumber(PARAM_PREFIX + "ModuleNumber", 0); +testAngleDegrees = SmartDashboard.getNumber(PARAM_PREFIX + "Angle", 90.0); +numberOfCycles = (int) SmartDashboard.getNumber(PARAM_PREFIX + "NumberOfCycles", 10); +angleToleranceDegrees = SmartDashboard.getNumber(PARAM_PREFIX + "AngleTolerance", 2.0); +maxWaitTimeSeconds = SmartDashboard.getNumber(PARAM_PREFIX + "MaxWaitTime", 1.0); +minHoldTimeSeconds = SmartDashboard.getNumber(PARAM_PREFIX + "MinHoldTime", 0.5); +``` + +**Issues Identified:** +1. **Prefix repetition**: `PARAM_PREFIX + "ParameterName"` appears 12 times (6 puts + 6 gets) +2. **Default value duplication**: Default values (0, 90.0, 10, 2.0, 1.0, 0.5) appear twice - once in `initializeParameters()` and once in `initialize()` +3. **Manual type casting**: `(int)` casts required for integer parameters, error-prone +4. **String concatenation**: 12 string concatenations that could be typos +5. **No compile-time safety**: Parameter name typos only caught at runtime +6. **PARAM_PREFIX management**: Every test must define and maintain a `PARAM_PREFIX` constant +7. **Error-prone**: Test name must match exactly in the prefix string +8. **Maintenance burden**: If test name changes, prefix must be updated manually + +### LedStateTestCommand + +**Parameter Initialization (lines 71-84):** +```java +private static final String PARAM_PREFIX = "DiagnosticTests/LED State Test/Parameters/"; + +SmartDashboard.putData(PARAM_PREFIX + "LedState", ledStateChooser); +SmartDashboard.putNumber(PARAM_PREFIX + "Duration", 3.0); +``` + +**Parameter Reading (lines 93-110):** +```java +SendableChooser chooser = (SendableChooser) SmartDashboard.getData(PARAM_PREFIX + "LedState"); +String selectedStateName = null; +if (chooser != null) { + selectedStateName = chooser.getSelected(); +} +if (selectedStateName == null) { + selectedStateName = CandleSubsystem.LedStates.None.name(); +} +// ... error handling ... +duration = SmartDashboard.getNumber(PARAM_PREFIX + "Duration", 3.0); +``` + +**Issues Identified:** +1. **Complex chooser retrieval**: Requires casting, null checking, and error handling (17 lines of code) +2. **Default value duplication**: Duration default (3.0) appears in both methods +3. **Inconsistent patterns**: Different approach for SendableChooser vs. simple parameters +4. **Error-prone**: Type casting and null checks can be forgotten +5. **PARAM_PREFIX management**: Must define and maintain prefix constant + +## Benefits of TestDashboard + +### 1. **Eliminates PARAM_PREFIX Constant** + +**Current Problem:** +- Every test must define: `private static final String PARAM_PREFIX = "DiagnosticTests/[TestName]/Parameters/";` +- Test name must match exactly in the prefix string +- If test name changes, prefix must be updated manually +- Risk of inconsistency across tests + +**With TestDashboard (Option 1 - Separate Methods):** +```java +// NO PARAM_PREFIX constant needed! + +@Override +public void initializeParameters() { + // Prefix automatically constructed from getTestName() + TestDashboard.putParamInt(this, "ModuleNumber", 0); + TestDashboard.putParamDouble(this, "Angle", 90.0); +} + +@Override +public void initialize() { + // Same automatic prefix construction + moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); + testAngleDegrees = TestDashboard.getParamDouble(this, "Angle", 90.0); +} +``` + +**With TestDashboard (Option 2 - Unified Methods):** +```java +// NO PARAM_PREFIX constant needed! + +@Override +public void initializeParameters() { + // Prefix automatically constructed from getTestName() + TestDashboard.putInt(this, DataType.PARAMETER, "ModuleNumber", 0); + TestDashboard.putDouble(this, DataType.PARAMETER, "Angle", 90.0); +} + +@Override +public void initialize() { + // Same automatic prefix construction + moduleNumber = TestDashboard.getInt(this, DataType.PARAMETER, "ModuleNumber", 0); + testAngleDegrees = TestDashboard.getDouble(this, DataType.PARAMETER, "Angle", 90.0); +} +``` + +**Benefits:** +- ✅ **No PARAM_PREFIX constant needed** - eliminated entirely +- ✅ **Automatic prefix construction** - uses `getTestName()` from test instance +- ✅ **Consistent format** - all tests use same prefix pattern automatically +- ✅ **Maintainable** - if test name changes, prefix updates automatically +- ✅ **Error prevention** - can't have mismatched test names in prefix + +### 2. **Eliminates Boilerplate Code** + +**Current (SwerveAngleDriftTestCommand):** +- 12 lines for 6 parameters (6 puts + 6 gets) +- 12 string concatenations +- 2 manual type casts +- 1 PARAM_PREFIX constant definition + +**With TestDashboard:** +```java +// initializeParameters() +TestDashboard.putParamInt(this, "ModuleNumber", 0); +TestDashboard.putParamDouble(this, "Angle", 90.0); +TestDashboard.putParamInt(this, "NumberOfCycles", 10); +TestDashboard.putParamDouble(this, "AngleTolerance", 2.0); +TestDashboard.putParamDouble(this, "MaxWaitTime", 1.0); +TestDashboard.putParamDouble(this, "MinHoldTime", 0.5); + +// initialize() +moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); +testAngleDegrees = TestDashboard.getParamDouble(this, "Angle", 90.0); +numberOfCycles = TestDashboard.getParamInt(this, "NumberOfCycles", 10); +angleToleranceDegrees = TestDashboard.getParamDouble(this, "AngleTolerance", 2.0); +maxWaitTimeSeconds = TestDashboard.getParamDouble(this, "MaxWaitTime", 1.0); +minHoldTimeSeconds = TestDashboard.getParamDouble(this, "MinHoldTime", 0.5); +``` + +**Benefits:** +- ✅ No manual type casting needed +- ✅ Type-safe getters (`getInt()` vs `getDouble()`) +- ✅ Consistent API pattern +- ✅ **Code reduction**: Eliminates 2 type casts, removes PARAM_PREFIX constant +- ✅ Makes intent clearer + +### 3. **Prevents Default Value Mismatches** + +**Current Problem:** +- Default values must be manually kept in sync between `initializeParameters()` and `initialize()` +- Easy to introduce bugs if defaults differ: + ```java + // initializeParameters() + SmartDashboard.putNumber(PARAM_PREFIX + "Angle", 90.0); // Default: 90.0 + + // initialize() - BUG: Different default! + testAngleDegrees = SmartDashboard.getNumber(PARAM_PREFIX + "Angle", 45.0); // Default: 45.0 + ``` + +**With TestDashboard:** +- Can use constants for defaults to ensure consistency: + ```java + // Single source of truth for defaults + private static final double DEFAULT_ANGLE = 90.0; + + TestDashboard.putParamDouble(this, "Angle", DEFAULT_ANGLE); + testAngleDegrees = TestDashboard.getParamDouble(this, "Angle", DEFAULT_ANGLE); + ``` + +**Benefits:** +- ✅ Prevents default value mismatches +- ✅ Single source of truth for parameter definitions +- ✅ Compile-time safety + +### 4. **Simplifies Complex Parameter Types** + +**Current (LedStateTestCommand - SendableChooser):** +```java +// initializeParameters() +SendableChooser ledStateChooser = new SendableChooser<>(); +// ... populate chooser ... +SmartDashboard.putData(PARAM_PREFIX + "LedState", ledStateChooser); + +// initialize() - Complex retrieval +SendableChooser chooser = (SendableChooser) SmartDashboard.getData(PARAM_PREFIX + "LedState"); +String selectedStateName = null; +if (chooser != null) { + selectedStateName = chooser.getSelected(); +} +if (selectedStateName == null) { + selectedStateName = CandleSubsystem.LedStates.None.name(); +} +// ... error handling ... +``` + +**With TestDashboard:** +```java +// initializeParameters() +TestDashboard.putParamChooser(this, "LedState", ledStateChooser); + +// initialize() - Simple retrieval with built-in null handling +String selectedStateName = TestDashboard.getParamChooserSelected( + this, + "LedState", + CandleSubsystem.LedStates.None.name() // default +); +``` + +**Benefits:** +- ✅ Reduces 17 lines to 2 lines (~88% code reduction) +- ✅ Handles null checking and defaults automatically +- ✅ Consistent error handling +- ✅ Less error-prone + +### 5. **Improves Code Readability** + +**Current:** +```java +moduleNumber = (int) SmartDashboard.getNumber(PARAM_PREFIX + "ModuleNumber", 0); +``` + +**With TestDashboard:** +```java +moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); +``` + +**Benefits:** +- ✅ Clearer intent: "get an integer" vs "get a number and cast to int" +- ✅ No type casting clutter +- ✅ Self-documenting method names +- ✅ No string concatenation visible + +### 6. **Reduces Error-Prone Patterns** + +**Current Issues:** +1. **String typos**: `PARAM_PREFIX + "ModuleNumber"` vs `PARAM_PREFIX + "ModuleNumbr"` (typo) +2. **Type mismatches**: Forgetting `(int)` cast, getting wrong type +3. **Default mismatches**: Different defaults in put vs get +4. **Null handling**: Forgetting null checks for SendableChooser +5. **Prefix mismatches**: Test name in prefix doesn't match `getTestName()` + +**With TestDashboard:** +- Type-safe methods prevent type errors +- Consistent null handling built-in +- Automatic prefix construction prevents mismatches +- Can use constants for parameter names to reduce typos: + ```java + private static final String KEY_MODULE_NUMBER = "ModuleNumber"; + private static final String KEY_ANGLE = "Angle"; + + TestDashboard.putParamInt(this, KEY_MODULE_NUMBER, 0); + moduleNumber = TestDashboard.getParamInt(this, KEY_MODULE_NUMBER, 0); + ``` + +**Benefits:** +- ✅ Fewer runtime errors +- ✅ Better IDE autocomplete support +- ✅ Easier refactoring (rename parameter key in one place) +- ✅ Automatic prefix consistency + +### 7. **Enables Future Enhancements** + +With a centralized helper, we could add: +- **Parameter validation**: Validate ranges, types, etc. +- **Parameter documentation**: Associate descriptions with parameters +- **Parameter groups**: Organize related parameters +- **Parameter persistence**: Save/load parameter sets +- **Parameter templates**: Predefined parameter sets for common scenarios + +**Example:** +```java +// Future enhancement: Parameter validation +TestDashboard.putParamDouble( + this, + "Angle", + 90.0, + Validator.range(0.0, 360.0) // Optional validation +); +``` + +## Design Solution: Eliminating PARAM_PREFIX + +### Problem + +Currently, each test must manually manage a `PARAM_PREFIX` constant: + +```java +private static final String PARAM_PREFIX = "DiagnosticTests/Swerve Angle Drift Test/Parameters/"; + +@Override +public void initializeParameters() { + SmartDashboard.putNumber(PARAM_PREFIX + "ModuleNumber", 0); + // ... +} + +@Override +public void initialize() { + moduleNumber = (int) SmartDashboard.getNumber(PARAM_PREFIX + "ModuleNumber", 0); + // ... +} +``` + +**Issues:** +1. **Repetitive**: Every test must define `PARAM_PREFIX` +2. **Error-prone**: Test name must match exactly in the prefix string +3. **Maintenance burden**: If test name changes, prefix must be updated +4. **Inconsistency risk**: Different tests might use slightly different prefix formats + +### Solution: Automatic Prefix Construction + +`TestDashboard` automatically constructs the parameter prefix from the test instance using `getTestName()`. This eliminates the need for tests to manage `PARAM_PREFIX` at all. + +### Design Options Evaluated + +#### Option 1: Pass Test Instance to Helper Methods (Recommended) ✅ + +**API Design:** +```java +public class TestDashboard { + private static final String BASE_PREFIX = "DiagnosticTests/"; + private static final String PARAMETERS_SUFFIX = "/Parameters/"; + + /** + * Constructs the parameter prefix for a test. + * Format: "DiagnosticTests/[TestName]/Parameters/" + */ + private static String getParameterPrefix(DiagnosticTest test) { + if (test == null) { + throw new IllegalArgumentException("Test instance cannot be null"); + } + String testName = test.getTestName(); + if (testName == null || testName.isEmpty()) { + throw new IllegalStateException("Test name cannot be null or empty. Implement getTestName() properly."); + } + return BASE_PREFIX + testName + PARAMETERS_SUFFIX; + } + + // Put methods + public static void putInt(DiagnosticTest test, String parameterName, int defaultValue) { + SmartDashboard.putNumber(getParameterPrefix(test) + parameterName, defaultValue); + } + + public static void putDouble(DiagnosticTest test, String parameterName, double defaultValue) { + SmartDashboard.putNumber(getParameterPrefix(test) + parameterName, defaultValue); + } + + public static void putBoolean(DiagnosticTest test, String parameterName, boolean defaultValue) { + SmartDashboard.putBoolean(getParameterPrefix(test) + parameterName, defaultValue); + } + + public static void putString(DiagnosticTest test, String parameterName, String defaultValue) { + SmartDashboard.putString(getParameterPrefix(test) + parameterName, defaultValue); + } + + public static void putChooser(DiagnosticTest test, String parameterName, SendableChooser chooser) { + SmartDashboard.putData(getParameterPrefix(test) + parameterName, chooser); + } + + // Get methods + public static int getInt(DiagnosticTest test, String parameterName, int defaultValue) { + return (int) SmartDashboard.getNumber(getParameterPrefix(test) + parameterName, defaultValue); + } + + public static double getDouble(DiagnosticTest test, String parameterName, double defaultValue) { + return SmartDashboard.getNumber(getParameterPrefix(test) + parameterName, defaultValue); + } + + public static boolean getBoolean(DiagnosticTest test, String parameterName, boolean defaultValue) { + return SmartDashboard.getBoolean(getParameterPrefix(test) + parameterName, defaultValue); + } + + public static String getString(DiagnosticTest test, String parameterName, String defaultValue) { + return SmartDashboard.getString(getParameterPrefix(test) + parameterName, defaultValue); + } + + @SuppressWarnings("unchecked") + public static SendableChooser getChooser(DiagnosticTest test, String parameterName) { + return (SendableChooser) SmartDashboard.getData(getParameterPrefix(test) + parameterName); + } + + public static T getChooserSelected(DiagnosticTest test, String parameterName, T defaultValue) { + SendableChooser chooser = getChooser(test, parameterName); + if (chooser != null && chooser.getSelected() != null) { + return chooser.getSelected(); + } + return defaultValue; + } + + // Optional: Also support full key path for flexibility (backward compatibility) + public static void putInt(String fullKey, int defaultValue) { + SmartDashboard.putNumber(fullKey, defaultValue); + } + + public static int getInt(String fullKey, int defaultValue) { + return (int) SmartDashboard.getNumber(fullKey, defaultValue); + } + // ... similar overloads for other types +} +``` + +**Usage in Tests:** +```java +public class SwerveAngleDriftTestCommand extends Command implements DiagnosticTest { + // NO PARAM_PREFIX needed! + + @Override + public String getTestName() { + return "Swerve Angle Drift Test"; + } + + @Override + public void initializeParameters() { + // Pass 'this' to helper methods - prefix is automatically constructed + TestDashboard.putParamInt(this, "ModuleNumber", 0); + TestDashboard.putParamDouble(this, "Angle", 90.0); + TestDashboard.putParamInt(this, "NumberOfCycles", 10); + TestDashboard.putParamDouble(this, "AngleTolerance", 2.0); + TestDashboard.putParamDouble(this, "MaxWaitTime", 1.0); + TestDashboard.putParamDouble(this, "MinHoldTime", 0.5); + } + + @Override + public void initialize() { + // Read parameters - prefix automatically constructed from test name + moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); + testAngleDegrees = TestDashboard.getParamDouble(this, "Angle", 90.0); + numberOfCycles = TestDashboard.getParamInt(this, "NumberOfCycles", 10); + angleToleranceDegrees = TestDashboard.getParamDouble(this, "AngleTolerance", 2.0); + maxWaitTimeSeconds = TestDashboard.getParamDouble(this, "MaxWaitTime", 1.0); + minHoldTimeSeconds = TestDashboard.getParamDouble(this, "MinHoldTime", 0.5); + // ... + } +} +``` + +**Benefits:** +- ✅ **No PARAM_PREFIX constant needed** - eliminated entirely +- ✅ **Automatic prefix construction** - uses `getTestName()` from test instance +- ✅ **Type-safe** - no manual casting needed +- ✅ **Consistent** - all tests use same prefix format automatically +- ✅ **Maintainable** - if test name changes, prefix updates automatically +- ✅ **Flexible** - still supports full key path for edge cases + +**Considerations:** +- Tests must pass `this` to helper methods (minor verbosity) +- Helper methods need to call `getTestName()` on each call (negligible performance impact) + +#### Option 2: Context Pattern with Initialization ❌ + +**Drawbacks:** +- ❌ More complex - requires context management +- ❌ Error-prone - easy to forget `setContext()` or `clearContext()` +- ❌ Thread-local overhead +- ❌ Not thread-safe if tests run concurrently + +**Verdict:** Not recommended - too complex for the benefit. + +#### Option 3: Builder Pattern ❌ + +**Drawbacks:** +- ❌ More verbose for reading parameters (need to create helper instance) +- ❌ Less intuitive for getters (can't chain as naturally) + +**Verdict:** Nice API, but Option 1 is simpler and more straightforward. + +### Recommended Solution: Option 1 + +**Final API:** +```java +// In initializeParameters() +TestDashboard.putParamInt(this, "ModuleNumber", 0); +TestDashboard.putParamDouble(this, "Angle", 90.0); + +// In initialize() +moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); +testAngleDegrees = TestDashboard.getParamDouble(this, "Angle", 90.0); +``` + +**Key Features:** +1. **No PARAM_PREFIX constant** - completely eliminated +2. **Automatic prefix construction** - uses `getTestName()` from test instance +3. **Type-safe methods** - `getInt()`, `getDouble()`, etc. +4. **Consistent format** - all tests use same prefix pattern +5. **Simple and intuitive** - pass `this`, pass parameter name, done + +## Implementation Notes + +1. **Prefix Format**: `"DiagnosticTests/" + test.getTestName() + "/Parameters/"` +2. **Backward Compatibility**: Can provide overloaded methods that accept full key path for edge cases +3. **Error Handling**: If `test.getTestName()` returns null or empty, throw descriptive exception +4. **Performance**: Calling `getTestName()` on each helper call is negligible (simple string return) + +## Migration Examples + +### SwerveAngleDriftTestCommand + +**Before:** +```java +private static final String PARAM_PREFIX = "DiagnosticTests/Swerve Angle Drift Test/Parameters/"; + +@Override +public void initializeParameters() { + SmartDashboard.putNumber(PARAM_PREFIX + "ModuleNumber", 0); + SmartDashboard.putNumber(PARAM_PREFIX + "Angle", 90.0); + SmartDashboard.putNumber(PARAM_PREFIX + "NumberOfCycles", 10); + SmartDashboard.putNumber(PARAM_PREFIX + "AngleTolerance", 2.0); + SmartDashboard.putNumber(PARAM_PREFIX + "MaxWaitTime", 1.0); + SmartDashboard.putNumber(PARAM_PREFIX + "MinHoldTime", 0.5); +} + +@Override +public void initialize() { + moduleNumber = (int) SmartDashboard.getNumber(PARAM_PREFIX + "ModuleNumber", 0); + testAngleDegrees = SmartDashboard.getNumber(PARAM_PREFIX + "Angle", 90.0); + numberOfCycles = (int) SmartDashboard.getNumber(PARAM_PREFIX + "NumberOfCycles", 10); + angleToleranceDegrees = SmartDashboard.getNumber(PARAM_PREFIX + "AngleTolerance", 2.0); + maxWaitTimeSeconds = SmartDashboard.getNumber(PARAM_PREFIX + "MaxWaitTime", 1.0); + minHoldTimeSeconds = SmartDashboard.getNumber(PARAM_PREFIX + "MinHoldTime", 0.5); +} +``` + +**After:** +```java +// PARAM_PREFIX constant removed! + +@Override +public void initializeParameters() { + TestDashboard.putParamInt(this, "ModuleNumber", 0); + TestDashboard.putParamDouble(this, "Angle", 90.0); + TestDashboard.putParamInt(this, "NumberOfCycles", 10); + TestDashboard.putParamDouble(this, "AngleTolerance", 2.0); + TestDashboard.putParamDouble(this, "MaxWaitTime", 1.0); + TestDashboard.putParamDouble(this, "MinHoldTime", 0.5); +} + +@Override +public void initialize() { + moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); + testAngleDegrees = TestDashboard.getParamDouble(this, "Angle", 90.0); + numberOfCycles = TestDashboard.getParamInt(this, "NumberOfCycles", 10); + angleToleranceDegrees = TestDashboard.getParamDouble(this, "AngleTolerance", 2.0); + maxWaitTimeSeconds = TestDashboard.getParamDouble(this, "MaxWaitTime", 1.0); + minHoldTimeSeconds = TestDashboard.getParamDouble(this, "MinHoldTime", 0.5); +} +``` + +**Changes:** +- ✅ Removed `PARAM_PREFIX` constant (1 line removed) +- ✅ Eliminated 2 type casts +- ✅ Cleaner, more readable code +- ✅ Automatic prefix construction + +### LedStateTestCommand + +**Before:** +```java +private static final String PARAM_PREFIX = "DiagnosticTests/LED State Test/Parameters/"; + +@Override +public void initializeParameters() { + SmartDashboard.putData(PARAM_PREFIX + "LedState", ledStateChooser); + SmartDashboard.putNumber(PARAM_PREFIX + "Duration", 3.0); +} + +@Override +public void initialize() { + SendableChooser chooser = (SendableChooser) SmartDashboard.getData(PARAM_PREFIX + "LedState"); + String selectedStateName = null; + if (chooser != null) { + selectedStateName = chooser.getSelected(); + } + if (selectedStateName == null) { + selectedStateName = CandleSubsystem.LedStates.None.name(); + } + // ... error handling ... + duration = SmartDashboard.getNumber(PARAM_PREFIX + "Duration", 3.0); +} +``` + +**After:** +```java +// PARAM_PREFIX constant removed! + +@Override +public void initializeParameters() { + TestDashboard.putParamChooser(this, "LedState", ledStateChooser); + TestDashboard.putParamDouble(this, "Duration", 3.0); +} + +@Override +public void initialize() { + String selectedStateName = TestDashboard.getParamChooserSelected( + this, + "LedState", + CandleSubsystem.LedStates.None.name() + ); + // ... error handling ... + duration = TestDashboard.getParamDouble(this, "Duration", 3.0); +} +``` + +**Changes:** +- ✅ Removed `PARAM_PREFIX` constant (1 line removed) +- ✅ Simplified chooser retrieval (17 lines → 3 lines, ~82% reduction) +- ✅ Built-in null handling +- ✅ Automatic prefix construction + +## Quantitative Impact + +### Code Reduction + +**SwerveAngleDriftTestCommand:** +- Current: 13 lines for parameter management (1 constant + 12 parameter lines) +- With TestDashboard: 12 lines (no constant needed) +- **Benefit**: Eliminates 1 constant definition, 2 type casts, improves readability + +**LedStateTestCommand:** +- Current: ~21 lines for parameter management (1 constant + ~20 lines including chooser handling) +- With TestDashboard: ~5 lines +- **Benefit**: ~76% code reduction for parameter handling + +### Error Prevention + +**Current Risk Areas:** +1. Type casting errors: 2 per test (int parameters) +2. Default value mismatches: 6 opportunities in SwerveAngleDriftTestCommand +3. String typos: 12 opportunities per test +4. Null handling: 1 complex case in LedStateTestCommand +5. Prefix mismatches: Test name in prefix doesn't match `getTestName()` + +**With TestDashboard:** +- Type casting errors: 0 (type-safe methods) +- Default value mismatches: Can be prevented with constants +- String typos: Can be reduced with constants +- Null handling: Built-in +- Prefix mismatches: 0 (automatic construction) + +## Recommendations + +### High Priority Benefits + +1. **Eliminates PARAM_PREFIX**: No need to define and maintain prefix constants +2. **Type Safety**: Eliminate manual type casting for integer parameters +3. **Code Clarity**: Self-documenting method names (`getInt()` vs `getNumber()`) +4. **Complex Parameter Handling**: Simplify SendableChooser retrieval (~82% code reduction) + +### Medium Priority Benefits + +5. **Default Value Consistency**: Provide patterns to prevent mismatches +6. **Error Reduction**: Built-in null handling and validation + +### Low Priority (Future Enhancements) + +7. **Parameter Validation**: Range checking, type validation +8. **Parameter Documentation**: Associate descriptions with parameters +9. **Parameter Templates**: Predefined parameter sets + +## Results Support + +### Current Results Pattern + +Both tests also use a `RESULT_PREFIX` constant to publish results to SmartDashboard: + +**SwerveAngleDriftTestCommand:** +```java +private static final String RESULT_PREFIX = "DiagnosticTests/Swerve Angle Drift Test/Results/"; + +// Results organized into groups +SmartDashboard.putNumber(RESULT_PREFIX + "Config/Module", moduleNumber); +SmartDashboard.putNumber(RESULT_PREFIX + "Progress/CurrentCycle", currentCycle + 1); +SmartDashboard.putString(RESULT_PREFIX + "Progress/State", currentState.toString()); +SmartDashboard.putNumber(RESULT_PREFIX + "RealTime/CurrentAngleEncoderDrift", currentDrift); +SmartDashboard.putNumber(RESULT_PREFIX + "Summary/TotalDrift", totalDrift); +SmartDashboard.putString(RESULT_PREFIX + "Status/Message", "Complete"); +``` + +**LedStateTestCommand:** +```java +private static final String RESULT_PREFIX = "DiagnosticTests/LED State Test/Results/"; + +SmartDashboard.putString(RESULT_PREFIX + "Status", "Running"); +SmartDashboard.putNumber(RESULT_PREFIX + "ElapsedTime", actualDuration); +SmartDashboard.putNumber(RESULT_PREFIX + "ActualDuration", actualDuration); +``` + +**Issues:** +- Same problems as `PARAM_PREFIX`: repetitive, error-prone, maintenance burden +- Results are only written (never read), so only need `put` methods +- Results often organized into logical groups (Config, Progress, RealTime, Summary, Status) + +### Class Name Alternatives + +Since the helper supports both parameters (inputs) and results (outputs), alternative class names to consider: + +1. **`TestDashboard`** ⭐ (Recommended) + - Simple and concise + - Clear and descriptive + - Covers all SmartDashboard interactions + - Accurately describes the class's purpose + +2. **`TestDashboardHelper`** + - Clear but slightly verbose + - Explicitly indicates it's a helper class + - (Not chosen - using `TestDashboard` instead) + +3. **`DiagnosticTestDashboard`** + - More specific to diagnostic tests + - Slightly longer + - Clear about scope + +4. **`TestDataHelper`** + - Generic but clear + - Covers both inputs and outputs + - Less specific about SmartDashboard + +**Recommendation**: Use `TestDashboard` - it's simple, concise, and accurately describes the class's purpose of managing SmartDashboard interactions for tests. + +### Expanded Helper: TestDashboard with Results Support + +The helper can be expanded to support both parameters and results with symmetric method naming: + +```java +public class TestDashboard { + private static final String BASE_PREFIX = "DiagnosticTests/"; + private static final String PARAMETERS_SUFFIX = "/Parameters/"; + private static final String RESULTS_SUFFIX = "/Results/"; + + /** + * Enum to distinguish between parameters (inputs) and results (outputs). + */ + public enum DataType { + PARAMETER, // Input parameters (read/write) + RESULT // Output results (write-only) + } + + // Prefix construction + private static String getPrefix(DiagnosticTest test, DataType type) { + if (test == null) { + throw new IllegalArgumentException("Test instance cannot be null"); + } + String testName = test.getTestName(); + if (testName == null || testName.isEmpty()) { + throw new IllegalStateException("Test name cannot be null or empty. Implement getTestName() properly."); + } + String suffix = (type == DataType.PARAMETER) ? PARAMETERS_SUFFIX : RESULTS_SUFFIX; + return BASE_PREFIX + testName + suffix; + } + + // ============================================================================ + // Option 1: Separate Methods (Recommended for Common Use) + // Clear, explicit, and symmetric naming + // ============================================================================ + + // Parameter Methods (read/write) + public static void putParamInt(DiagnosticTest test, String parameterName, int defaultValue) { + SmartDashboard.putNumber(getPrefix(test, DataType.PARAMETER) + parameterName, defaultValue); + } + + public static int getParamInt(DiagnosticTest test, String parameterName, int defaultValue) { + return (int) SmartDashboard.getNumber(getPrefix(test, DataType.PARAMETER) + parameterName, defaultValue); + } + + public static void putParamDouble(DiagnosticTest test, String parameterName, double defaultValue) { + SmartDashboard.putNumber(getPrefix(test, DataType.PARAMETER) + parameterName, defaultValue); + } + + public static double getParamDouble(DiagnosticTest test, String parameterName, double defaultValue) { + return SmartDashboard.getNumber(getPrefix(test, DataType.PARAMETER) + parameterName, defaultValue); + } + + public static void putParamBoolean(DiagnosticTest test, String parameterName, boolean defaultValue) { + SmartDashboard.putBoolean(getPrefix(test, DataType.PARAMETER) + parameterName, defaultValue); + } + + public static boolean getParamBoolean(DiagnosticTest test, String parameterName, boolean defaultValue) { + return SmartDashboard.getBoolean(getPrefix(test, DataType.PARAMETER) + parameterName, defaultValue); + } + + public static void putParamString(DiagnosticTest test, String parameterName, String defaultValue) { + SmartDashboard.putString(getPrefix(test, DataType.PARAMETER) + parameterName, defaultValue); + } + + public static String getParamString(DiagnosticTest test, String parameterName, String defaultValue) { + return SmartDashboard.getString(getPrefix(test, DataType.PARAMETER) + parameterName, defaultValue); + } + + public static void putParamChooser(DiagnosticTest test, String parameterName, SendableChooser chooser) { + SmartDashboard.putData(getPrefix(test, DataType.PARAMETER) + parameterName, chooser); + } + + @SuppressWarnings("unchecked") + public static SendableChooser getParamChooser(DiagnosticTest test, String parameterName) { + return (SendableChooser) SmartDashboard.getData(getPrefix(test, DataType.PARAMETER) + parameterName); + } + + public static T getParamChooserSelected(DiagnosticTest test, String parameterName, T defaultValue) { + SendableChooser chooser = getParamChooser(test, parameterName); + if (chooser != null && chooser.getSelected() != null) { + return chooser.getSelected(); + } + return defaultValue; + } + + // Result Methods (write-only, symmetric naming) + public static void putResultInt(DiagnosticTest test, String resultName, int value) { + SmartDashboard.putNumber(getPrefix(test, DataType.RESULT) + resultName, value); + } + + public static void putResultDouble(DiagnosticTest test, String resultName, double value) { + SmartDashboard.putNumber(getPrefix(test, DataType.RESULT) + resultName, value); + } + + public static void putResultBoolean(DiagnosticTest test, String resultName, boolean value) { + SmartDashboard.putBoolean(getPrefix(test, DataType.RESULT) + resultName, value); + } + + public static void putResultString(DiagnosticTest test, String resultName, String value) { + SmartDashboard.putString(getPrefix(test, DataType.RESULT) + resultName, value); + } + + // ============================================================================ + // Option 2: Unified Methods (Optional, for Flexibility) + // Single method that accepts DataType parameter + // ============================================================================ + + /** + * Unified method to put an integer value (parameter or result). + * + * @param test The test instance + * @param type Whether this is a PARAMETER (input) or RESULT (output) + * @param name The name of the parameter/result + * @param value The value to set + */ + public static void putInt(DiagnosticTest test, DataType type, String name, int value) { + SmartDashboard.putNumber(getPrefix(test, type) + name, value); + } + + /** + * Unified method to get an integer parameter value. + * Note: Results are write-only, so this only works for PARAMETER type. + * + * @param test The test instance + * @param type Must be PARAMETER (results are write-only) + * @param name The name of the parameter + * @param defaultValue The default value if not found + * @return The parameter value + */ + public static int getInt(DiagnosticTest test, DataType type, String name, int defaultValue) { + if (type != DataType.PARAMETER) { + throw new IllegalArgumentException("getInt() only supports PARAMETER type. Results are write-only."); + } + return (int) SmartDashboard.getNumber(getPrefix(test, type) + name, defaultValue); + } + + // Similar unified methods for double, boolean, String... + public static void putDouble(DiagnosticTest test, DataType type, String name, double value) { + SmartDashboard.putNumber(getPrefix(test, type) + name, value); + } + + public static double getDouble(DiagnosticTest test, DataType type, String name, double defaultValue) { + if (type != DataType.PARAMETER) { + throw new IllegalArgumentException("getDouble() only supports PARAMETER type. Results are write-only."); + } + return SmartDashboard.getNumber(getPrefix(test, type) + name, defaultValue); + } + + // ... similar for boolean, String, etc. +} +``` + +**Design Decision: Provide Both Options** + +1. **Separate Methods (Recommended for Common Use)** + - `putParamInt()`, `getParamInt()` for parameters + - `putResultInt()` for results + - **Benefits**: Clear, explicit, symmetric naming, better IDE autocomplete + - **Usage**: Most common case - clear intent at call site + +2. **Unified Methods (Optional, for Flexibility)** + - `putInt(test, DataType.PARAMETER, name, value)` + - `putInt(test, DataType.RESULT, name, value)` + - **Benefits**: Single method, flexible, useful for dynamic scenarios + - **Usage**: Less common - when you need to switch between types dynamically + +### Usage Examples + +**SwerveAngleDriftTestCommand - Parameters (using separate methods):** +```java +// Before: +private static final String PARAM_PREFIX = "DiagnosticTests/Swerve Angle Drift Test/Parameters/"; +SmartDashboard.putNumber(PARAM_PREFIX + "ModuleNumber", 0); +moduleNumber = (int) SmartDashboard.getNumber(PARAM_PREFIX + "ModuleNumber", 0); + +// After (Option 1 - Separate Methods): +// PARAM_PREFIX constant removed! +TestDashboard.putParamInt(this, "ModuleNumber", 0); +moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); + +// After (Option 2 - Unified Methods): +TestDashboard.putInt(this, DataType.PARAMETER, "ModuleNumber", 0); +moduleNumber = TestDashboard.getInt(this, DataType.PARAMETER, "ModuleNumber", 0); +``` + +**SwerveAngleDriftTestCommand - Results:** +```java +// Before: +private static final String RESULT_PREFIX = "DiagnosticTests/Swerve Angle Drift Test/Results/"; +SmartDashboard.putNumber(RESULT_PREFIX + "Config/Module", moduleNumber); +SmartDashboard.putNumber(RESULT_PREFIX + "Progress/CurrentCycle", currentCycle + 1); +SmartDashboard.putString(RESULT_PREFIX + "Progress/State", currentState.toString()); +SmartDashboard.putNumber(RESULT_PREFIX + "RealTime/CurrentAngleEncoderDrift", currentDrift); +SmartDashboard.putNumber(RESULT_PREFIX + "Summary/TotalDrift", totalDrift); +SmartDashboard.putString(RESULT_PREFIX + "Status/Message", "Complete"); + +// After (Option 1 - Separate Methods): +// RESULT_PREFIX constant removed! +TestDashboard.putResultInt(this, "Config/Module", moduleNumber); +TestDashboard.putResultInt(this, "Progress/CurrentCycle", currentCycle + 1); +TestDashboard.putResultString(this, "Progress/State", currentState.toString()); +TestDashboard.putResultDouble(this, "RealTime/CurrentAngleEncoderDrift", currentDrift); +TestDashboard.putResultDouble(this, "Summary/TotalDrift", totalDrift); +TestDashboard.putResultString(this, "Status/Message", "Complete"); + +// After (Option 2 - Unified Methods): +TestDashboard.putInt(this, DataType.RESULT, "Config/Module", moduleNumber); +TestDashboard.putInt(this, DataType.RESULT, "Progress/CurrentCycle", currentCycle + 1); +TestDashboard.putString(this, DataType.RESULT, "Progress/State", currentState.toString()); +TestDashboard.putDouble(this, DataType.RESULT, "RealTime/CurrentAngleEncoderDrift", currentDrift); +TestDashboard.putDouble(this, DataType.RESULT, "Summary/TotalDrift", totalDrift); +TestDashboard.putString(this, DataType.RESULT, "Status/Message", "Complete"); +``` + +**LedStateTestCommand - Results:** +```java +// Before: +private static final String RESULT_PREFIX = "DiagnosticTests/LED State Test/Results/"; +SmartDashboard.putString(RESULT_PREFIX + "Status", "Running"); +SmartDashboard.putNumber(RESULT_PREFIX + "ElapsedTime", actualDuration); +SmartDashboard.putNumber(RESULT_PREFIX + "ActualDuration", actualDuration); + +// After (Option 1 - Separate Methods): +// RESULT_PREFIX constant removed! +TestDashboard.putResultString(this, "Status", "Running"); +TestDashboard.putResultDouble(this, "ElapsedTime", actualDuration); +TestDashboard.putResultDouble(this, "ActualDuration", actualDuration); +``` + +### Method Naming Symmetry + +The design provides **symmetric naming** for clarity: + +**Parameters (Inputs - Read/Write):** +- `putParamInt()`, `getParamInt()` +- `putParamDouble()`, `getParamDouble()` +- `putParamBoolean()`, `getParamBoolean()` +- `putParamString()`, `getParamString()` +- `putParamChooser()`, `getParamChooser()`, `getParamChooserSelected()` + +**Results (Outputs - Write-Only):** +- `putResultInt()` +- `putResultDouble()` +- `putResultBoolean()` +- `putResultString()` + +**Benefits of Symmetric Naming:** +- ✅ Clear distinction between parameters and results +- ✅ Consistent naming pattern (`putParam*` vs `putResult*`) +- ✅ Self-documenting code - intent is clear at call site +- ✅ Better IDE autocomplete - separate namespaces for parameters vs results + +### Benefits of Results Support + +1. ✅ **Eliminates RESULT_PREFIX constant** - same benefit as parameters +2. ✅ **Consistent API** - same pattern for parameters and results +3. ✅ **Automatic prefix construction** - uses test name automatically +4. ✅ **Supports result groups** - can use paths like "Config/Module", "Summary/TotalDrift" +5. ✅ **Type-safe** - `putResultInt()` vs `putResultDouble()` makes intent clear +6. ✅ **Reduced boilerplate** - eliminates another constant definition + +### Complete Migration Example + +**SwerveAngleDriftTestCommand - Full Migration:** + +**Before:** +```java +private static final String PARAM_PREFIX = "DiagnosticTests/Swerve Angle Drift Test/Parameters/"; +private static final String RESULT_PREFIX = "DiagnosticTests/Swerve Angle Drift Test/Results/"; + +@Override +public void initializeParameters() { + SmartDashboard.putNumber(PARAM_PREFIX + "ModuleNumber", 0); + // ... +} + +@Override +public void initialize() { + moduleNumber = (int) SmartDashboard.getNumber(PARAM_PREFIX + "ModuleNumber", 0); + // ... + SmartDashboard.putNumber(RESULT_PREFIX + "Config/Module", moduleNumber); + // ... +} +``` + +**After:** +```java +// NO PREFIX CONSTANTS NEEDED! + +@Override +public void initializeParameters() { + TestDashboard.putParamInt(this, "ModuleNumber", 0); + // ... +} + +@Override +public void initialize() { + moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); + // ... + TestDashboard.putResultInt(this, "Config/Module", moduleNumber); + // ... +} +``` + +**Total Elimination:** +- ✅ Removed `PARAM_PREFIX` constant +- ✅ Removed `RESULT_PREFIX` constant +- ✅ Eliminated all type casts +- ✅ Cleaner, more maintainable code + +## Conclusion + +**TestDashboard would provide significant value**, especially for: + +1. **Tests with many parameters** (like SwerveAngleDriftTestCommand with 6 parameters) + - Eliminates PARAM_PREFIX constant + - Eliminates type casting + - Improves readability + - Reduces boilerplate + +2. **Tests with complex parameter types** (like LedStateTestCommand with SendableChooser) + - Dramatic code reduction (~76%) + - Simplifies error handling + - Consistent patterns + +3. **Tests with extensive results** (like SwerveAngleDriftTestCommand with organized result groups) + - Eliminates RESULT_PREFIX constant + - Consistent result publishing API + - Supports organized result groups + +4. **Long-term maintainability** + - Easier to add new parameters and results + - Consistent patterns across all tests + - Foundation for future enhancements + - Automatic prefix consistency for both parameters and results + +**Recommendation**: Implement `TestDashboard` with **both parameter and result support** as a **high-priority enhancement**. The benefits are clear, the implementation is straightforward, and it will improve developer experience for all future tests. + +**Recommended Design:** +1. **Class Name**: `TestDashboard` (simple, concise, clearly describes purpose) +2. **Primary API**: Separate methods with symmetric naming (`putParamInt()`, `putResultInt()`, etc.) +3. **Optional API**: Unified methods with `DataType` enum for flexibility +4. **Prefix Construction**: Automatic from test instance using `getTestName()` + +The recommended design completely eliminates the need for both `PARAM_PREFIX` and `RESULT_PREFIX` constants while providing: +- ✅ Type-safe, clean API +- ✅ Symmetric naming for parameters and results +- ✅ Automatic prefix construction +- ✅ Both explicit (separate methods) and flexible (unified methods) options +- ✅ Simple and intuitive to use + +The minor verbosity of passing `this` is far outweighed by the benefits of eliminating prefix management entirely. diff --git a/docs/led-status-architecture.md b/docs/led-status-architecture.md new file mode 100644 index 0000000..982b6c4 --- /dev/null +++ b/docs/led-status-architecture.md @@ -0,0 +1,350 @@ +# LED Status Architecture with Driver Station Display + +## Overview + +This document describes the architecture for displaying robot status indicators on both: +1. **Physical LEDs** on the robot (visible when driver can see robot) +2. **Driver Station** display (visible when driver can't see robot, looking at camera feed) + +## Architecture: NetworkTables-Based + +We use **NetworkTables** as the single source of truth for robot status, which allows both the LED subsystem and Driver Station to display the same information. + +### Why NetworkTables? + +1. **Single Source of Truth**: Publish status once, consume in multiple places +2. **Automatic Driver Station Access**: NetworkTables data is automatically available to Driver Station/Shuffleboard +3. **No Duplicate Logic**: Status determination happens once, displayed in two places +4. **Already in Codebase**: Consistent with existing NetworkTables usage for swerve states +5. **Decoupled**: Subsystems don't need direct references to each other + +## Architecture Diagram + +``` +┌─────────────────────────────────────────────────────────────┐ +│ Subsystems │ +│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │ +│ │ Shooter │ │ Intake │ │ Swerve │ │ +│ │ Subsystem │ │ Subsystem │ │ Subsystem │ │ +│ └──────┬───────┘ └──────┬───────┘ └──────┬───────┘ │ +│ │ │ │ │ +│ └──────────────────┼──────────────────┘ │ +│ │ │ +│ Publish State to │ +│ NetworkTables │ +│ │ │ +└────────────────────────────┼─────────────────────────────────┘ + │ + ▼ + ┌─────────────────┐ + │ NetworkTables │ + │ (Single Source │ + │ of Truth) │ + └────────┬────────┘ + │ + ┌────────────┼────────────┐ + │ │ │ + ▼ ▼ ▼ + ┌───────────┐ ┌──────────┐ ┌──────────────┐ + │ LED │ │Shuffle- │ │ Driver │ + │Subsystem │ │ board │ │ Station │ + │ (Physical │ │ (Display)│ │ (Display) │ + │ LEDs) │ │ │ │ │ + └───────────┘ └──────────┘ └──────────────┘ +``` + +## Implementation + +### Step 1: Subsystems Publish State to NetworkTables + +Each subsystem publishes its relevant state to NetworkTables in its `periodic()` method. + +**Example: ShooterSubsystem** + +```java +public class ShooterSubsystem extends SubsystemBase { + // NetworkTables publishers + private final BooleanPublisher shooterActivePublisher = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Shooter/Active") + .publish(); + + private final BooleanPublisher shooterAtSpeedPublisher = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Shooter/AtSpeed") + .publish(); + + @Override + public void periodic() { + // ... existing code ... + + // Publish state to NetworkTables + shooterActivePublisher.set(isShooterActive); + shooterAtSpeedPublisher.set(isAtTargetSpeed()); + } +} +``` + +**Example: IntakeSubsystem** + +```java +public class IntakeSubsystem extends SubsystemBase { + private final BooleanPublisher hasGamePiecePublisher = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Intake/HasGamePiece") + .publish(); + + @Override + public void periodic() { + // ... existing code ... + + // Publish state to NetworkTables + hasGamePiecePublisher.set(hasGamePiece()); + } +} +``` + +### Step 2: CandleSubsystem Reads from NetworkTables and Controls LEDs + +`CandleSubsystem` subscribes to NetworkTables and determines LED status based on robot state. + +```java +public class CandleSubsystem extends SubsystemBase { + private final CANdle candle = new CANdle(CANdleConstants.CANDLE_ID); + + // NetworkTables subscribers + private final BooleanSubscriber shooterActiveSubscriber = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Shooter/Active") + .subscribe(false); + + private final BooleanSubscriber shooterAtSpeedSubscriber = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Shooter/AtSpeed") + .subscribe(false); + + private final BooleanSubscriber hasGamePieceSubscriber = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Intake/HasGamePiece") + .subscribe(false); + + private final BooleanSubscriber alignedWithTargetSubscriber = + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Swerve/AlignedWithTarget") + .subscribe(false); + + // Status mode enum + public enum LEDStatusMode { + OFF, + READY_TO_SHOOT, // Green: Shooter ready + aligned + has game piece + SHOOTER_ACTIVE, // Yellow: Shooter spinning but not ready + HAS_GAME_PIECE, // Blue: Has game piece but not ready to shoot + ALIGNED, // Cyan: Aligned with target but not ready + DISABLED_RED, // Red: Disabled, red alliance + DISABLED_BLUE, // Blue: Disabled, blue alliance + ERROR // Red flashing: Error state + } + + @Override + public void periodic() { + LEDStatusMode status = determineStatus(); + setLEDStatus(status); + } + + private LEDStatusMode determineStatus() { + // Priority-based status determination + if (DriverStation.isDisabled()) { + // Show alliance color when disabled + return DriverStation.getAlliance() + .orElse(Alliance.Blue) == Alliance.Red + ? LEDStatusMode.DISABLED_RED + : LEDStatusMode.DISABLED_BLUE; + } + + // Ready to shoot: shooter active + at speed + aligned + has game piece + if (shooterActiveSubscriber.get() + && shooterAtSpeedSubscriber.get() + && alignedWithTargetSubscriber.get() + && hasGamePieceSubscriber.get()) { + return LEDStatusMode.READY_TO_SHOOT; + } + + // Shooter active but not ready + if (shooterActiveSubscriber.get()) { + return LEDStatusMode.SHOOTER_ACTIVE; + } + + // Has game piece but not ready to shoot + if (hasGamePieceSubscriber.get()) { + return LEDStatusMode.HAS_GAME_PIECE; + } + + // Aligned but not ready + if (alignedWithTargetSubscriber.get()) { + return LEDStatusMode.ALIGNED; + } + + return LEDStatusMode.OFF; + } + + private void setLEDStatus(LEDStatusMode status) { + RGBWColor color; + switch (status) { + case READY_TO_SHOOT: + color = new RGBWColor(0, 255, 0, 0); // Green + break; + case SHOOTER_ACTIVE: + color = new RGBWColor(255, 255, 0, 0); // Yellow + break; + case HAS_GAME_PIECE: + color = new RGBWColor(0, 0, 255, 0); // Blue + break; + case ALIGNED: + color = new RGBWColor(0, 255, 255, 0); // Cyan + break; + case DISABLED_RED: + color = new RGBWColor(255, 0, 0, 0); // Red + break; + case DISABLED_BLUE: + color = new RGBWColor(0, 0, 255, 0); // Blue + break; + case ERROR: + color = new RGBWColor(255, 0, 0, 0); // Red (flashing handled separately) + break; + default: + color = new RGBWColor(0, 0, 0, 0); // Off + } + + candle.setControl(new SolidColor(LED_START_INDEX, LED_END_INDEX) + .withColor(color)); + } +} +``` + +### Step 3: Publish LED Status to NetworkTables for Driver Station Display + +`CandleSubsystem` also publishes its determined status to NetworkTables so Driver Station can display it. + +```java +public class CandleSubsystem extends SubsystemBase { + // ... existing code ... + + // Publisher for Driver Station display + private final StringPublisher ledStatusPublisher = + NetworkTableInstance.getDefault() + .getStringTopic("RobotState/LED/Status") + .publish(); + + private final IntegerPublisher ledStatusCodePublisher = + NetworkTableInstance.getDefault() + .getIntegerTopic("RobotState/LED/StatusCode") + .publish(); + + @Override + public void periodic() { + LEDStatusMode status = determineStatus(); + setLEDStatus(status); + + // Publish status for Driver Station display + ledStatusPublisher.set(status.name()); + ledStatusCodePublisher.set(status.ordinal()); + } +} +``` + +### Step 4: Display Status on Driver Station/Shuffleboard + +Create a Shuffleboard tab or use SmartDashboard to display the LED status indicators. + +**Option A: Using SmartDashboard (Simple)** + +```java +// In CandleSubsystem.periodic() +SmartDashboard.putString("Robot Status", status.name()); +SmartDashboard.putBoolean("Ready to Shoot", + status == LEDStatusMode.READY_TO_SHOOT); +SmartDashboard.putBoolean("Has Game Piece", + hasGamePieceSubscriber.get()); +SmartDashboard.putBoolean("Shooter Ready", + shooterActiveSubscriber.get() && shooterAtSpeedSubscriber.get()); +``` + +**Option B: Using Shuffleboard (More Visual)** + +Create a Shuffleboard tab with color-coded indicators that match the LED colors. + +```java +// In RobotContainer or a dedicated StatusDisplay class +public void configureShuffleboard() { + ShuffleboardTab statusTab = Shuffleboard.getTab("Robot Status"); + + // Status indicator (text) + statusTab.add("Status", + NetworkTableInstance.getDefault() + .getStringTopic("RobotState/LED/Status") + .subscribe("UNKNOWN")); + + // Color-coded boolean indicators + statusTab.add("Ready to Shoot", + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Shooter/Ready") + .subscribe(false)) + .withWidget(BuiltInWidgets.kBooleanBox) + .withProperties(Map.of("colorWhenTrue", "#00FF00")); // Green + + statusTab.add("Has Game Piece", + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Intake/HasGamePiece") + .subscribe(false)) + .withWidget(BuiltInWidgets.kBooleanBox) + .withProperties(Map.of("colorWhenTrue", "#0000FF")); // Blue + + statusTab.add("Aligned", + NetworkTableInstance.getDefault() + .getBooleanTopic("RobotState/Swerve/AlignedWithTarget") + .subscribe(false)) + .withWidget(BuiltInWidgets.kBooleanBox) + .withProperties(Map.of("colorWhenTrue", "#00FFFF")); // Cyan +} +``` + +## NetworkTables Key Naming Convention + +Use a consistent naming convention for NetworkTables keys: + +``` +RobotState/ + ├── Shooter/ + │ ├── Active (boolean) + │ ├── AtSpeed (boolean) + │ └── Ready (boolean) // Computed: Active && AtSpeed + ├── Intake/ + │ └── HasGamePiece (boolean) + ├── Swerve/ + │ └── AlignedWithTarget (boolean) + └── LED/ + ├── Status (string) // Current LED status mode name + └── StatusCode (int) // Current LED status mode ordinal +``` + +## Benefits of This Architecture + +1. **Single Source of Truth**: Status is determined once, displayed in multiple places +2. **No Duplicate Logic**: LED subsystem and Driver Station use the same data +3. **Automatic Synchronization**: LEDs and Driver Station always show the same status +4. **Easy to Extend**: Add new status indicators by publishing to NetworkTables +5. **Debugging**: Can see status in Shuffleboard even when robot LEDs aren't visible +6. **Consistent**: Uses same pattern as existing swerve state publishing + +## Testing + +1. **Unit Testing**: Can create mock NetworkTables publishers/subscribers +2. **Integration Testing**: Verify LEDs and Driver Station show same status +3. **Driver Testing**: Verify Driver Station display is visible when robot is behind hub + +## Future Enhancements + +1. **Status History**: Log status changes to NetworkTables for analysis +2. **Custom Dashboard**: Create a custom Driver Station dashboard with visual indicators +3. **Audio Alerts**: Add audio alerts on Driver Station for critical status changes +4. **Status Patterns**: Add flashing/pulsing patterns for different statuses diff --git a/docs/robotstate-networktables-hybrid.md b/docs/robotstate-networktables-hybrid.md new file mode 100644 index 0000000..84106ea --- /dev/null +++ b/docs/robotstate-networktables-hybrid.md @@ -0,0 +1,650 @@ +# RobotState + NetworkTables Hybrid Architecture + +## Overview + +This document describes a hybrid approach that combines: +- **RobotState class** (like KnightKrawlers) - centralized state management with clean API +- **NetworkTables integration** - automatic external visibility and decoupling +- **Singleton pattern** - single instance, accessible from anywhere +- **Type-safe getters/setters** - compile-time safety, no string key typos + +## Why This Approach is Excellent + +### Benefits + +1. **Type Safety** ✅ + - Getter/setter methods provide compile-time type checking + - No string key typos (unlike raw NetworkTables) + - IDE autocomplete works perfectly + +2. **Centralized Key Management** ✅ + - All NetworkTables keys defined in one place + - Easy to refactor (change key once, affects all code) + - Self-documenting (keys are constants) + +3. **Clean API** ✅ + - Simple getter/setter interface + - Hides NetworkTables complexity + - Subsystems don't need to know about NetworkTables + +4. **Automatic External Visibility** ✅ + - Data automatically available to Shuffleboard/AdvantageScope + - No additional code needed for external access + - Perfect for Driver Station display + +5. **Error Handling** ✅ + - Centralized exception handling + - Default values for missing data + - Type conversion handled automatically + +6. **Best of Both Worlds** ✅ + - RobotState benefits: centralized, type-safe, clean API + - NetworkTables benefits: external visibility, decoupled, standard + +7. **Easier Testing** ✅ + - Can mock RobotState more easily than NetworkTables + - Can provide test implementations + - Centralized state makes testing simpler + +### Comparison to Alternatives + +| Feature | Raw NetworkTables | RobotState + NetworkTables | +|---------|------------------|---------------------------| +| **Type Safety** | ❌ Runtime (string keys) | ✅ Compile-time (methods) | +| **Key Management** | ❌ Scattered in code | ✅ Centralized constants | +| **Error Handling** | ⚠️ Manual | ✅ Centralized | +| **API Clarity** | ⚠️ Verbose | ✅ Clean getters/setters | +| **External Visibility** | ✅ Yes | ✅ Yes (automatic) | +| **Refactoring** | ❌ Find/replace strings | ✅ Change constant once | + +## Architecture + +``` +┌─────────────────────────────────────────────────────────────┐ +│ Subsystems │ +│ ┌──────────────┐ ┌──────────────┐ ┌──────────────┐ │ +│ │ Shooter │ │ Intake │ │ Swerve │ │ +│ │ Subsystem │ │ Subsystem │ │ Subsystem │ │ +│ └──────┬───────┘ └──────┬───────┘ └──────┬───────┘ │ +│ │ │ │ │ +│ └──────────────────┼──────────────────┘ │ +│ │ │ +│ Call Setters on │ +│ RobotState │ +│ │ │ +└────────────────────────────┼─────────────────────────────────┘ + │ + ▼ + ┌─────────────────┐ + │ RobotState │ + │ (Singleton) │ + │ │ + │ - Getters │ + │ - Setters │ + │ - Key Constants│ + │ - Error Handle │ + └────────┬────────┘ + │ + Publishes to / Reads from + │ + ▼ + ┌─────────────────┐ + │ NetworkTables │ + │ (WPILib) │ + └────────┬────────┘ + │ + ┌────────────┼────────────┐ + │ │ │ + ▼ ▼ ▼ + ┌───────────┐ ┌──────────┐ ┌──────────────┐ + │ LED │ │Shuffle- │ │ Driver │ + │Subsystem │ │ board │ │ Station │ + │ (Reads │ │ (Reads │ │ (Reads │ + │ from │ │ from │ │ from │ + │RobotState)│ │Network- │ │ Network- │ + │ │ │ Tables) │ │ Tables) │ + └───────────┘ └──────────┘ └──────────────┘ +``` + +## Implementation + +### Step 1: Create RobotState Class + +```java +package frc.robot; + +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.IntegerSubscriber; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringSubscriber; + +// Note: We use both Publishers (for writing) and Subscribers (for reading) +// This ensures we always get the latest value from NetworkTables, even if +// external code writes directly to NetworkTables, bypassing RobotState setters. + +/** + * Centralized robot state management with NetworkTables integration. + * + * Provides type-safe getters and setters that automatically publish/read + * from NetworkTables. This gives us: + * - Type safety (compile-time checking) + * - Centralized key management + * - Automatic external visibility (Shuffleboard, Driver Station) + * - Clean API for subsystems + */ +public class RobotState { + private static RobotState INSTANCE; + + // NetworkTables instance + private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); + + // ============================================ + // NetworkTables Key Constants + // ============================================ + // Centralized key management - change keys here, affects all code + private static final class Keys { + // Shooter state + private static final String SHOOTER_ACTIVE = "RobotState/Shooter/Active"; + private static final String SHOOTER_AT_SPEED = "RobotState/Shooter/AtSpeed"; + private static final String SHOOTER_READY = "RobotState/Shooter/Ready"; // Computed + + // Intake state + private static final String INTAKE_HAS_GAME_PIECE = "RobotState/Intake/HasGamePiece"; + private static final String INTAKE_ACTIVE = "RobotState/Intake/Active"; + + // Swerve state + private static final String SWERVE_ALIGNED_WITH_TARGET = "RobotState/Swerve/AlignedWithTarget"; + private static final String SWERVE_AT_TARGET_DISTANCE = "RobotState/Swerve/AtTargetDistance"; + + // LED status + private static final String LED_STATUS = "RobotState/LED/Status"; + private static final String LED_STATUS_CODE = "RobotState/LED/StatusCode"; + + // Match state + private static final String ALLIANCE_COLOR = "RobotState/Match/AllianceColor"; + } + + // ============================================ + // Publishers (for writing to NetworkTables) + // ============================================ + private final BooleanPublisher shooterActivePublisher = + ntInstance.getBooleanTopic(Keys.SHOOTER_ACTIVE).publish(); + private final BooleanPublisher shooterAtSpeedPublisher = + ntInstance.getBooleanTopic(Keys.SHOOTER_AT_SPEED).publish(); + private final BooleanPublisher shooterReadyPublisher = + ntInstance.getBooleanTopic(Keys.SHOOTER_READY).publish(); + + private final BooleanPublisher intakeHasGamePiecePublisher = + ntInstance.getBooleanTopic(Keys.INTAKE_HAS_GAME_PIECE).publish(); + private final BooleanPublisher intakeActivePublisher = + ntInstance.getBooleanTopic(Keys.INTAKE_ACTIVE).publish(); + + private final BooleanPublisher swerveAlignedPublisher = + ntInstance.getBooleanTopic(Keys.SWERVE_ALIGNED_WITH_TARGET).publish(); + private final BooleanPublisher swerveAtDistancePublisher = + ntInstance.getBooleanTopic(Keys.SWERVE_AT_TARGET_DISTANCE).publish(); + + private final StringPublisher ledStatusPublisher = + ntInstance.getStringTopic(Keys.LED_STATUS).publish(); + private final IntegerPublisher ledStatusCodePublisher = + ntInstance.getIntegerTopic(Keys.LED_STATUS_CODE).publish(); + + private final StringPublisher allianceColorPublisher = + ntInstance.getStringTopic(Keys.ALLIANCE_COLOR).publish(); + + // ============================================ + // Local State (for fast reads) + // ============================================ + // Maintain local state for fast access. Subscribers will update this + // when NetworkTables values change (including external writes). + + private boolean shooterActive = false; + private boolean shooterAtSpeed = false; + private boolean shooterReady = false; + + private boolean intakeHasGamePiece = false; + private boolean intakeActive = false; + + private boolean swerveAlignedWithTarget = false; + private boolean swerveAtTargetDistance = false; + + private String ledStatus = "UNKNOWN"; + private int ledStatusCode = 0; + + private String allianceColor = "UNKNOWN"; + + // ============================================ + // Subscribers (for detecting NetworkTables changes) + // ============================================ + // Subscribers detect when NetworkTables values change (including external writes) + // and update local state. This gives us fast reads while staying in sync. + + private final BooleanSubscriber shooterActiveSubscriber = + ntInstance.getBooleanTopic(Keys.SHOOTER_ACTIVE).subscribe(false); + private final BooleanSubscriber shooterAtSpeedSubscriber = + ntInstance.getBooleanTopic(Keys.SHOOTER_AT_SPEED).subscribe(false); + private final BooleanSubscriber shooterReadySubscriber = + ntInstance.getBooleanTopic(Keys.SHOOTER_READY).subscribe(false); + + private final BooleanSubscriber intakeHasGamePieceSubscriber = + ntInstance.getBooleanTopic(Keys.INTAKE_HAS_GAME_PIECE).subscribe(false); + private final BooleanSubscriber intakeActiveSubscriber = + ntInstance.getBooleanTopic(Keys.INTAKE_ACTIVE).subscribe(false); + + private final BooleanSubscriber swerveAlignedSubscriber = + ntInstance.getBooleanTopic(Keys.SWERVE_ALIGNED_WITH_TARGET).subscribe(false); + private final BooleanSubscriber swerveAtDistanceSubscriber = + ntInstance.getBooleanTopic(Keys.SWERVE_AT_TARGET_DISTANCE).subscribe(false); + + private final StringSubscriber ledStatusSubscriber = + ntInstance.getStringTopic(Keys.LED_STATUS).subscribe("UNKNOWN"); + private final IntegerSubscriber ledStatusCodeSubscriber = + ntInstance.getIntegerTopic(Keys.LED_STATUS_CODE).subscribe(0); + + private final StringSubscriber allianceColorSubscriber = + ntInstance.getStringTopic(Keys.ALLIANCE_COLOR).subscribe("UNKNOWN"); + + private RobotState() { + // Private constructor - singleton pattern + // Set default values (publishes to NetworkTables, subscribers will pick them up) + setShooterActive(false); + setShooterAtSpeed(false); + setIntakeHasGamePiece(false); + setIntakeActive(false); + setSwerveAlignedWithTarget(false); + setSwerveAtTargetDistance(false); + } + + public static RobotState getInstance() { + if (INSTANCE == null) { + INSTANCE = new RobotState(); + } + return INSTANCE; + } + + // ============================================ + // Shooter State Getters/Setters + // ============================================ + + public void setShooterActive(boolean active) { + // Update local state immediately (fast) + this.shooterActive = active; + // Publish to NetworkTables + shooterActivePublisher.set(active); + // Update computed state + updateShooterReady(); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public boolean isShooterActive() { + return shooterActive; + } + + public void setShooterAtSpeed(boolean atSpeed) { + // Update local state immediately (fast) + this.shooterAtSpeed = atSpeed; + // Publish to NetworkTables + shooterAtSpeedPublisher.set(atSpeed); + // Update computed state + updateShooterReady(); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public boolean isShooterAtSpeed() { + return shooterAtSpeed; + } + + /** + * Computed state: Shooter is ready to shoot (active + at speed) + * Updates both local state and NetworkTables. + */ + private void updateShooterReady() { + // Compute from current local state + boolean ready = shooterActive && shooterAtSpeed; + // Update local state + this.shooterReady = ready; + // Publish to NetworkTables + shooterReadyPublisher.set(ready); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public boolean isShooterReady() { + return shooterReady; + } + + // ============================================ + // Intake State Getters/Setters + // ============================================ + + public void setIntakeHasGamePiece(boolean hasGamePiece) { + // Update local state immediately (fast) + this.intakeHasGamePiece = hasGamePiece; + // Publish to NetworkTables + intakeHasGamePiecePublisher.set(hasGamePiece); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public boolean hasGamePiece() { + return intakeHasGamePiece; + } + + public void setIntakeActive(boolean active) { + // Update local state immediately (fast) + this.intakeActive = active; + // Publish to NetworkTables + intakeActivePublisher.set(active); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public boolean isIntakeActive() { + return intakeActive; + } + + // ============================================ + // Swerve State Getters/Setters + // ============================================ + + public void setSwerveAlignedWithTarget(boolean aligned) { + // Update local state immediately (fast) + this.swerveAlignedWithTarget = aligned; + // Publish to NetworkTables + swerveAlignedPublisher.set(aligned); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public boolean isSwerveAlignedWithTarget() { + return swerveAlignedWithTarget; + } + + public void setSwerveAtTargetDistance(boolean atDistance) { + // Update local state immediately (fast) + this.swerveAtTargetDistance = atDistance; + // Publish to NetworkTables + swerveAtDistancePublisher.set(atDistance); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public boolean isSwerveAtTargetDistance() { + return swerveAtTargetDistance; + } + + // ============================================ + // LED Status Getters/Setters + // ============================================ + + public void setLEDStatus(String status) { + // Update local state immediately (fast) + this.ledStatus = status; + // Publish to NetworkTables + ledStatusPublisher.set(status); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public String getLEDStatus() { + return ledStatus; + } + + public void setLEDStatusCode(int code) { + // Update local state immediately (fast) + this.ledStatusCode = code; + // Publish to NetworkTables + ledStatusCodePublisher.set(code); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public int getLEDStatusCode() { + return ledStatusCode; + } + + // ============================================ + // Match State Getters/Setters + // ============================================ + + public void setAllianceColor(String color) { + // Update local state immediately (fast) + this.allianceColor = color; + // Publish to NetworkTables + allianceColorPublisher.set(color); + } + + /** + * Fast read from local state. Local state is kept in sync with NetworkTables + * via subscribers that detect changes (including external writes). + */ + public String getAllianceColor() { + return allianceColor; + } + + // ============================================ + // Periodic Update (Required) + // ============================================ + // This method MUST be called periodically (e.g., from Robot.robotPeriodic()) + // to sync local state with NetworkTables changes (including external writes). + + public void periodic() { + // Sync local state with NetworkTables subscribers + // This detects changes from external writes (Driver Station, other code) + // and updates local state accordingly + + // Check if NetworkTables values have changed + if (shooterActiveSubscriber.get() != shooterActive) { + shooterActive = shooterActiveSubscriber.get(); + // Note: Don't republish - this was an external change + } + + if (shooterAtSpeedSubscriber.get() != shooterAtSpeed) { + shooterAtSpeed = shooterAtSpeedSubscriber.get(); + updateShooterReady(); // Recompute derived state + } + + if (intakeHasGamePieceSubscriber.get() != intakeHasGamePiece) { + intakeHasGamePiece = intakeHasGamePieceSubscriber.get(); + } + + if (intakeActiveSubscriber.get() != intakeActive) { + intakeActive = intakeActiveSubscriber.get(); + } + + if (swerveAlignedSubscriber.get() != swerveAlignedWithTarget) { + swerveAlignedWithTarget = swerveAlignedSubscriber.get(); + } + + if (swerveAtDistanceSubscriber.get() != swerveAtTargetDistance) { + swerveAtTargetDistance = swerveAtDistanceSubscriber.get(); + } + + String newLedStatus = ledStatusSubscriber.get(); + if (!newLedStatus.equals(ledStatus)) { + ledStatus = newLedStatus; + } + + int newLedStatusCode = (int) ledStatusCodeSubscriber.get(); + if (newLedStatusCode != ledStatusCode) { + ledStatusCode = newLedStatusCode; + } + + String newAllianceColor = allianceColorSubscriber.get(); + if (!newAllianceColor.equals(allianceColor)) { + allianceColor = newAllianceColor; + } + + // Update computed states + updateShooterReady(); + + // Update match state if needed + // (DriverStation state can be read here if needed) + } +} +``` + +### Step 2: Subsystems Use RobotState + +**ShooterSubsystem Example:** + +```java +public class ShooterSubsystem extends SubsystemBase { + private final RobotState robotState = RobotState.getInstance(); + + @Override + public void periodic() { + // ... existing shooter control code ... + + // Update RobotState (automatically publishes to NetworkTables) + robotState.setShooterActive(isShooterActive); + robotState.setShooterAtSpeed(isAtTargetSpeed()); + } +} +``` + +**CandleSubsystem Example:** + +```java +public class CandleSubsystem extends SubsystemBase { + private final RobotState robotState = RobotState.getInstance(); + private final CANdle candle = new CANdle(CANdleConstants.CANDLE_ID); + + @Override + public void periodic() { + // Read from RobotState (type-safe, clean API, fast local state access) + boolean shooterReady = robotState.isShooterReady(); + boolean hasGamePiece = robotState.hasGamePiece(); + boolean aligned = robotState.isSwerveAlignedWithTarget(); + + // Determine LED status + LEDStatusMode status = determineStatus(shooterReady, hasGamePiece, aligned); + setLEDStatus(status); + + // Publish LED status back to RobotState + robotState.setLEDStatus(status.name()); + robotState.setLEDStatusCode(status.ordinal()); + } + + private LEDStatusMode determineStatus(boolean shooterReady, + boolean hasGamePiece, + boolean aligned) { + // Priority-based status determination + if (shooterReady && hasGamePiece && aligned) { + return LEDStatusMode.READY_TO_SHOOT; + } + // ... rest of logic ... + } +} +``` + +**Important: Call RobotState.periodic() in Robot.robotPeriodic()** + +```java +// Robot.java +@Override +public void robotPeriodic() { + CommandScheduler.getInstance().run(); + RobotState.getInstance().periodic(); // Sync local state with NetworkTables +} +``` + +## Important Design Decision: Hybrid Approach (Local State + Subscribers) + +### The Best of Both Worlds + +**Problem with Local State Only:** +- If external code writes directly to NetworkTables (bypassing RobotState setters) +- Or if Driver Station writes to NetworkTables +- Local state in RobotState would become **stale/incorrect** + +**Problem with Subscribers Only:** +- NetworkTables reads have slight overhead (~1ms) +- Not as fast as local variable access + +**Solution: Hybrid Approach (Local State + Subscribers)** +- **Maintain local state** for fast reads (getters return local variables) +- **Use subscribers** to detect NetworkTables changes +- **Sync local state** in `periodic()` when subscribers detect changes +- **Setters update both** local state and NetworkTables immediately + +**How It Works:** +1. **Setters**: Update local state immediately (fast) + publish to NetworkTables +2. **Getters**: Return local state (fast, no NetworkTables read) +3. **Periodic**: Check subscribers for changes, sync local state if changed + +**Benefits:** +- ✅ **Fast reads** (local variable access, no NetworkTables overhead) +- ✅ **Always up-to-date** (subscribers detect external writes, sync in periodic) +- ✅ **NetworkTables is source of truth** (subscribers detect all changes) +- ✅ **Best performance** (fast reads, minimal overhead) + +**Trade-offs:** +- ⚠️ Must call `periodic()` regularly (e.g., from `Robot.robotPeriodic()`) +- ⚠️ Small delay for external writes (synced in next periodic call, usually <20ms) + +**This is the recommended approach** - gives you fast reads while maintaining correctness! + +## Benefits Summary + +1. **Type Safety**: `robotState.isShooterReady()` vs `nt.getBooleanTopic("RobotState/Shooter/Ready").subscribe(false).get()` +2. **Centralized Keys**: Change key once in `Keys` class, affects all code +3. **Clean API**: Simple getters/setters, no NetworkTables complexity exposed +4. **Automatic External Visibility**: Data automatically in Shuffleboard +5. **Error Handling**: Centralized default values and error handling +6. **Computed State**: Can compute derived state (e.g., `isShooterReady()`) +7. **Easy Testing**: Can mock RobotState or provide test implementation +8. **Fast Reads + Always Up-to-Date**: Local state for fast reads, subscribers sync on changes (including external writes) +9. **Best Performance**: Fast local variable access with automatic sync to NetworkTables + +## Usage Pattern + +**Writing State (Subsystems):** +```java +RobotState.getInstance().setShooterActive(true); +``` + +**Reading State (Other Subsystems):** +```java +boolean ready = RobotState.getInstance().isShooterReady(); +``` + +**External Access (Shuffleboard/Driver Station):** +- Automatically available via NetworkTables +- Can read directly from NetworkTables using the same keys +- Or use RobotState getters if accessing from robot code + +## Migration Path + +1. Create `RobotState` class with key constants +2. Add getters/setters for each state variable +3. Update subsystems to use `RobotState` instead of direct NetworkTables +4. Update `CandleSubsystem` to read from `RobotState` +5. Verify external visibility in Shuffleboard + +This approach gives you the best of both worlds: RobotState's clean API with NetworkTables' external visibility! diff --git a/docs/sensors-and-pose-estimation.md b/docs/sensors-and-pose-estimation.md new file mode 100644 index 0000000..8b3efe0 --- /dev/null +++ b/docs/sensors-and-pose-estimation.md @@ -0,0 +1,126 @@ +# Sensors, Robot Pose, and Pose Estimation + +## Overview + +The robot needs to know **where it is on the field** at all times. This field position +is called the robot's **pose** — an (x, y, θ) value representing its position and the +direction it is facing. + +No single sensor can reliably provide the pose. Instead, the robot **estimates** its pose +by combining readings from multiple sensors, each of which measures a different aspect +of the robot's state. + +## Sensors + +| Sensor | What it measures | Output | +|--------|-----------------|--------| +| **Pigeon2 gyro** | Which direction the robot is facing (yaw) | Heading in degrees | +| **Wheel encoders** | How far each swerve module wheel has traveled | Distance in meters per wheel | +| **Limelight cameras** | Position of AprilTags in the camera's view | Full pose estimate (x, y, θ) | + +## The Pose Estimator + +WPILib's `SwerveDrivePoseEstimator` fuses sensor readings into a single pose estimate. +Each robot loop (every 20 ms), it: + +1. Reads the pigeon yaw +2. Reads the four wheel encoder positions +3. Computes how the pose changed since the last cycle +4. Optionally incorporates vision measurements from Limelight cameras + +``` +Pigeon Yaw ──────┐ + ├──► SwerveDrivePoseEstimator ──► Pose2d (x, y, θ) +Wheel Encoders ──┘ + ▲ +Limelight Cameras ──────┘ (optional soft corrections) +``` + +## How Sensor Readings Are Used: Absolute vs. Delta + +The estimator treats the pigeon and encoders **differently**. Understanding this +distinction is important for getting hard resets and simulation right. + +### Pigeon Yaw — Absolute Value + +The estimator reads the pigeon's yaw as an **absolute heading**. It records the yaw +at reset time as a baseline, and on each subsequent cycle computes: + +``` +rotation change = current pigeon yaw − baseline pigeon yaw +``` + +The pigeon holds a persistent value that the estimator reads directly. If anything +externally changes the pigeon's value (e.g., simulation overwriting it), the estimator +interprets the change as real robot rotation. + +**Key implication:** If the pigeon's value is overwritten to something unexpected, the +estimator sees a large sudden rotation and the pose gets corrupted. + +### Wheel Encoders — Delta (Relative Change) + +The estimator reads each wheel encoder's **position** (total distance traveled), records +it at reset time as a baseline, and on each subsequent cycle computes: + +``` +distance traveled = current encoder position − baseline encoder position +``` + +The absolute encoder value doesn't matter — only how much it has **changed** since the +baseline was recorded. If the encoders read [100, 200, 150, 175] at reset time, those +become the baseline. A reading of [100.05, 200.03, 150.04, 175.02] on the next cycle +means each wheel moved a small amount — regardless of the absolute numbers. + +**Key implication:** As long as encoder values change incrementally (by small deltas each +cycle), the estimator tracks correctly. The starting absolute value is irrelevant. + +### Vision — Soft Correction + +Limelight cameras provide an independent full-pose estimate by detecting AprilTags. +These are added as **soft corrections** via `addVisionMeasurement()` — the estimator +blends them in using a Kalman filter rather than overwriting the pose. This means vision +data nudges the estimate gently rather than causing sudden jumps. + +## Hard Resets + +A **hard reset** occurs when we have an authoritative "oracle" pose (e.g., a known +starting position for autonomous) and want to force the estimator to adopt it. + +We call `resetPosition(gyroAngle, modulePositions, newPose)`, which: + +1. Records the current pigeon yaw as the new baseline for rotation deltas +2. Records the current encoder positions as the new baseline for distance deltas +3. Sets the internal pose estimate to the new pose + +After a reset, the estimator computes all future changes relative to these new baselines. + +### Why the Pigeon Must Be Updated During a Hard Reset + +Because the estimator uses the pigeon's value as an absolute reference, the pigeon must +be set to match the new pose's rotation **before or at the same time** as the reset. +Otherwise: + +- Reset records baseline gyro = (whatever the pigeon currently reads) +- If the pigeon reads something different on the next cycle (e.g., simulation overwrites + it), the estimator computes a large false rotation delta +- The pose estimate gets corrupted immediately + +### Why Encoders Don't Need Updating During a Hard Reset + +Because the estimator uses encoder **deltas**, and `resetPosition()` records the current +encoder values as the new baseline, the absolute encoder values are irrelevant. Whatever +the encoders happen to read at reset time becomes the zero point for future distance +calculations. + +## Simulation Implications + +In simulation, the `SimulationManager` updates simulated sensors each cycle: + +| Sensor | How simulation updates it | Safe across resets? | +|--------|--------------------------|-------------------| +| **Pigeon** | Overwrites with absolute value from internal sim pose | ❌ No — can corrupt baseline | +| **Encoders** | Adds incremental delta to current value | ✅ Yes — preserves baseline | + +This is why the simulation must be notified of hard resets: the `SimulationManager`'s +internal pose must be synced to the new pose so that the absolute pigeon value it writes +each cycle is consistent with what the estimator expects. diff --git a/docs/sim-alternatives.md b/docs/sim-alternatives.md new file mode 100644 index 0000000..548f81c --- /dev/null +++ b/docs/sim-alternatives.md @@ -0,0 +1,766 @@ +# Simulation Approach Comparison + +This document compares the two simulation implementations present in our codebase and places them in context alongside approaches used by other FRC teams. + +--- + +## Table of Contents + +1. [Overview of Our Two Approaches](#overview-of-our-two-approaches) +2. [Detailed Comparison](#detailed-comparison) +3. [External FRC Team Approaches](#external-frc-team-approaches) +4. [Summary Comparison Matrix](#summary-comparison-matrix) +5. [Analysis](#analysis) +6. [Appendix A — Feature Portability and Calculation Review](#appendix-a--feature-portability-and-calculation-review) +7. [Appendix B — SimulationManager Data Flow and Code Path Analysis](#appendix-b--simulationmanager-data-flow-and-code-path-analysis) + +--- + +## Overview of Our Two Approaches + +### `SimulationManager` — `jmm-sim-test-support` branch + +**File**: `src/main/java/frc/robot/sim/SimulationManager.java` (154 lines) + +`SimulationManager` is a standalone class in its own `sim` package. It is instantiated in `Robot.simulationInit()` and called from `Robot.simulationPeriodic()`. It operates externally to the subsystem — it reads desired module states from `SwerveSubsystem`, computes simulated robot motion via kinematic integration, then writes simulated values back into vendor hardware simulation APIs (Pigeon2 `SimState`, CANcoder `SimState`, REV `RelativeEncoder.setPosition()`). + +**How it works:** + +1. Reads desired swerve module states from `SwerveSubsystem.getDesiredStates()`, which in turn reads per-module `desiredState` fields stored by `SwerveModule.setDesiredState()`. +2. Converts module states to `ChassisSpeeds` via the swerve kinematics. +3. Integrates chassis speeds over `dt` using `Pose2d.exp(Twist2d)` to update an internal `simPose`. +4. Writes `simPose` heading into `Pigeon2SimState.setRawYaw()`. +5. Writes per-module drive distance and angle into each module's `RelativeEncoder.setPosition()` and `CANcoderSimState.setRawPosition()`. +6. Relies on the normal `SwerveSubsystem.periodic()` (which calls `odometry.update(getYaw(), getPositions())`) to read back these simulated sensor values and update odometry and Field2d as usual. + +**Key design decisions:** + +- Simulation logic is fully separated from production subsystem code. `SwerveSubsystem` has no `if (isSimulation())` conditionals in its `periodic()` method — it always calls `odometry.update()` using whatever sensor values are present. +- The `SimulationManager` writes to the same sensor simulation APIs that vendors provide, so the subsystem's production code path runs identically in sim and on hardware. +- `SwerveModule` stores a `desiredState` field (written by `setDesiredState()`, read by `getDesiredState()`) specifically to give the simulation access to individual module targets. This is the only production-code change required for simulation support. +- Additional accessor methods were added to `SwerveSubsystem` (`getDesiredStates()`, `getPigeon()`, `getModules()`, `getKinematics()`) for simulation and diagnostic test access. These are grouped in a clearly marked "Simulation and Test Support Methods" section. + +### `RobotSimulation` — `NonoAuto` branch + +**File**: `src/main/java/frc/robot/RobotSimulation.java` (68 lines) + +`RobotSimulation` is a top-level class that lives alongside `Robot.java` and `RobotContainer.java`. It is instantiated in the `Robot` constructor (not lazily in `simulationInit()`) and delegates to `SwerveSubsystem` for the actual physics work. + +**How it works:** + +1. `simulationInit()` configures `DriverStationSim` state (attached, disabled, mode flags) and calls `drive.simulationReset()`. +2. `simulationPeriodic()` computes `dt`, then calls `drive.simulationUpdate(dtSeconds)`. +3. `SwerveSubsystem.simulationUpdate()` does the physics: + - Reads `lastCommandedSpeeds` (stored by `driveFromChassisSpeeds()`) — or zero if disabled. + - Integrates yaw via `simYawDegrees += Math.toDegrees(speeds.omegaRadiansPerSecond * dt)`. + - Converts chassis speeds to module states, accumulates per-wheel position in `simWheelPositionsMeters[]` arrays. + - Calls `pigeon.setYaw()` and `odometry.update()` directly with the computed positions. +4. Also simulates battery voltage via `BatterySim` based on estimated current draw from drive and rotation fractions. + +**Key design decisions:** + +- Simulation physics lives inside `SwerveSubsystem` itself. The subsystem has dedicated simulation state fields (`simYawDegrees`, `simWheelPositionsMeters[]`, `simWheelAngles[]`, `lastCommandedSpeeds`). +- `SwerveSubsystem.periodic()` wraps its real odometry and vision update in `if (!RobotBase.isSimulation())` — in sim mode, `periodic()` only updates `Field2d`, while `simulationUpdate()` separately calls `odometry.update()` with simulated positions. +- The `RobotSimulation` class handles DriverStation sim configuration and battery voltage simulation, adding a layer of environmental realism. +- No vendor-specific SimState APIs are used. Instead, the subsystem maintains its own parallel arrays of simulated positions and passes them directly to the odometry estimator. + +--- + +## Detailed Comparison + +### Architecture and Separation of Concerns + +| Aspect | `SimulationManager` | `RobotSimulation` | +|--------|---------------------|-------------------| +| Where physics lives | Dedicated `sim/` package class | Inside `SwerveSubsystem` | +| Subsystem awareness of sim | None — no `isSimulation()` checks in `periodic()` | Yes — `periodic()` has `if (!RobotBase.isSimulation())` guard | +| Sensor simulation method | Writes to vendor SimState APIs (Pigeon2, CANcoder, REV encoders) | Maintains parallel state arrays, calls `odometry.update()` directly | +| Production code modifications | Adds `desiredState` field to `SwerveModule`, adds accessor methods to `SwerveSubsystem` | Adds `lastCommandedSpeeds`, `simYawDegrees`, `simWheelPositionsMeters[]`, `simWheelAngles[]` to `SwerveSubsystem`; adds `simulationReset()`, `simulationUpdate()`, `getLastCommandedSpeeds()` methods | +| Robot.java integration | Created in `simulationInit()`, called in `simulationPeriodic()` | Created in `Robot` constructor, init/periodic delegated | + +**`SimulationManager` pros:** +- Production code path (`SwerveSubsystem.periodic()`) is identical in sim and on hardware. There is no branching in the subsystem based on runtime environment. +- Writing to vendor SimState APIs means the full sensor-to-odometry pipeline is exercised in simulation, including any conversion factors, offsets, or configuration applied by the subsystem when reading sensors. +- The simulation can be removed entirely (delete the `sim/` package, remove two lines from `Robot.java`) without touching any subsystem code. + +**`SimulationManager` cons:** +- Depends on vendor SimState APIs working correctly in desktop simulation. REV `RelativeEncoder.setPosition()` behavior in sim is not officially documented and may not faithfully model real encoder behavior. +- Adds accessor methods (`getDesiredStates()`, `getPigeon()`, `getModules()`) to `SwerveSubsystem` that exist only for simulation and test support. These widen the subsystem's public API surface. +- The `desiredState` field in `SwerveModule` is production code that exists solely for simulation access. + +**`RobotSimulation` pros:** +- Does not depend on any vendor SimState APIs. The simulation directly computes positions and passes them to the odometry estimator, avoiding any uncertainty about vendor sim fidelity. +- Battery voltage simulation (via `BatterySim`) adds environmental realism. +- DriverStation sim configuration (`setDsAttached`, `setEnabled`, etc.) provides a more complete sim environment setup. +- Fewer public accessor methods added to the subsystem. + +**`RobotSimulation` cons:** +- `SwerveSubsystem.periodic()` has different code paths for sim vs. real. In simulation, the real odometry update and vision fusion are skipped. This means the sim does not exercise the same `periodic()` logic that runs on hardware. +- Simulation state fields (`simYawDegrees`, `simWheelPositionsMeters[]`, etc.) live inside `SwerveSubsystem`, adding member variables that are unused on hardware. +- The `lastCommandedSpeeds` field that simulation depends on is only set by `driveFromChassisSpeeds()`. If any code path sets module states without going through `driveFromChassisSpeeds()` (e.g., individual module control during testing), those commands will not appear in simulation. + +### Physics Model + +Both approaches use the same fundamental simulation model: **kinematic integration of chassis speeds** (no force-based or motor-model physics). Neither models motor dynamics, wheel slip, friction, inertia, or current draw affecting motor behavior. + +| Aspect | `SimulationManager` | `RobotSimulation` | +|--------|---------------------|-------------------| +| Pose integration | `Pose2d.exp(Twist2d)` | Linear `yaw +=` and per-wheel position accumulation | +| Per-module simulation | Yes — individual module states drive encoders | Yes — module states derived from chassis speeds | +| Source of commanded motion | `SwerveModule.getDesiredState()` per module | `lastCommandedSpeeds` (chassis-level) | +| Disabled behavior | Modules report zero desired state (no special handling needed) | Explicitly checks `DriverStation.isDisabled()` and substitutes zero speeds | +| dt clamping | `if (dt <= 0 \|\| dt > 1.0) dt = 0.02` | `MathUtil.clamp(dt, 0.0, 0.05)` | + +**Notable difference:** `SimulationManager` reads desired states at the per-module level, while `RobotSimulation` reads at the chassis-speeds level. This matters for scenarios like diagnostic test commands that control individual modules — `SimulationManager` would simulate those correctly, while `RobotSimulation` would not (since `lastCommandedSpeeds` is only set by the normal drive path). + +### Code Footprint + +| Metric | `SimulationManager` | `RobotSimulation` | +|--------|---------------------|-------------------| +| Simulation class lines | 154 | 68 | +| SwerveSubsystem sim-related additions | ~65 lines (accessor methods section) | ~45 lines (fields + `simulationReset()` + `simulationUpdate()`) | +| SwerveModule changes | +1 field, +1 method (`getDesiredState()`) | None | +| Total sim-related code | ~220 lines | ~113 lines | +| `periodic()` changes | None | Added `if (!RobotBase.isSimulation())` guard | + +--- + +## External FRC Team Approaches + +### FRC 6328 — Mechanical Advantage: AdvantageKit IO Layer Pattern + +**Repository**: [Mechanical-Advantage/RobotCode2025Public](https://github.com/Mechanical-Advantage/RobotCode2025Public) + +**Key files:** +- `ModuleIO.java` — interface defining all module operations and a structured `ModuleIOInputs` class +- `ModuleIOComp.java` — real hardware implementation using CTRE TalonFX motors +- `ModuleIOSim.java` (126 lines) — simulation implementation using WPILib `DCMotorSim` +- `Drive.java` — subsystem that accepts `ModuleIO` implementations via constructor injection + +**Architecture:** + +Team 6328 uses the **IO layer pattern** from their AdvantageKit framework. Every hardware interaction is defined through a Java interface (e.g., `ModuleIO`). The subsystem (`Drive`) only interacts with these interfaces, never with concrete hardware classes. At robot startup, the appropriate implementation is injected based on the robot type: + +``` +case COMPBOT -> new Drive(new GyroIOPigeon2(), new ModuleIOComp(...), ...); +case SIMBOT -> new Drive(new GyroIO() {}, new ModuleIOSim(), ...); +``` + +The `ModuleIOSim` class uses WPILib's `DCMotorSim` to model individual motor physics (voltage in → angular velocity/position out), including motor models, gear ratios, and moments of inertia. The simulated motors respond to the same PID and feedforward commands that the real hardware would receive. + +The subsystem's `periodic()` method and all command logic are identical regardless of whether real or simulated IO is plugged in. There are zero `isSimulation()` checks in subsystem or command code. + +**Pros:** +- Complete decoupling between robot logic and hardware. The subsystem and all commands are tested against the exact same code path in sim and on hardware. +- Motor-level physics (via `DCMotorSim`) provide more realistic behavior than kinematic-only integration — PID tuning in sim has some correlation to real-world tuning. +- Log replay: AdvantageKit can replay logged inputs through the same code to reproduce and debug issues offline. +- Each subsystem (drive, elevator, climber, etc.) has its own `IOSim` class, enabling simulation of the full robot. + +**Cons:** +- Requires significant upfront architectural investment. Every hardware interaction must be abstracted through an interface, which approximately doubles the number of files per subsystem (interface + real impl + sim impl + inputs class). +- The `DCMotorSim` physics model is still an approximation — it does not model wheel-floor friction, tire slip, or robot-environment collisions. +- Adopting AdvantageKit is a team-wide decision that affects the entire codebase structure. It is not something that can be added incrementally to a single subsystem. +- Steeper learning curve for team members unfamiliar with interface-based design and dependency injection. + +**Reference**: [AdvantageKit Documentation](https://docs.advantagekit.org/) + +### FRC 254 — The Cheesy Poofs: IO Layer + MapleSim Physics Engine + +**Repository**: [Team254/FRC-2025-Public](https://github.com/Team254/FRC-2025-Public) + +**Key files:** +- `DriveIO.java` — interface for drive subsystem operations +- `DriveIOHardware.java` — real hardware implementation extending CTRE `SwerveDrivetrain` +- `DriveIOSim.java` (117 lines) — simulation implementation that extends `DriveIOHardware` and integrates MapleSim +- `MapleSimSwerveDrivetrain.java` (275 lines) — adapter connecting MapleSim physics to CTRE device SimState APIs +- `SimulatedRobotState.java` (907 lines) — comprehensive simulation state manager handling game piece tracking, intake simulation, scoring simulation, and mechanism state + +**Architecture:** + +Team 254 uses an IO interface pattern similar to 6328, but with a different simulation backend. Their `DriveIOSim` class extends `DriveIOHardware` (which wraps CTRE's `SwerveDrivetrain`) and replaces the CTRE built-in sim with a MapleSim-powered physics engine. + +MapleSim (from the [maple-sim library](https://github.com/Shenzhen-Robotics-Alliance/maple-sim)) uses the [dyn4j](https://github.com/dyn4j/dyn4j) 2D rigid-body dynamics engine to simulate forces, collisions, and friction. The simulated robot has mass, bumper dimensions, wheel coefficients of friction, and interacts with field elements and game pieces as rigid bodies in a physics world. + +Their `MapleSimSwerveDrivetrain` class bridges MapleSim's physics output back into CTRE's vendor SimState APIs (`TalonFXSimState`, `CANcoderSimState`, `Pigeon2SimState`), so the real `SwerveDrivetrain` code processes simulated sensor data through the same pipeline it uses on hardware. + +The `SimulatedRobotState` class goes further by simulating game-piece intake, indexing, scoring at reef branches, and climber mechanics — creating a nearly complete game simulation. + +Team 254 supports both MapleSim and CTRE's built-in sim, toggled by a `Constants.useMapleSim` flag. + +**Pros:** +- Force-based physics with collision detection enables testing autonomous paths that interact with field elements, game pieces, and field boundaries. +- Writing simulated values back through vendor SimState APIs means the full CTRE `SwerveDrivetrain` code path (including odometry, status signals, etc.) is exercised in sim. +- Game-piece simulation enables end-to-end autonomous testing including intake and scoring. +- MapleSim is available as a vendor dependency (no need to build from source). + +**Cons:** +- Substantial complexity. The simulation layer for Team 254 totals over 1,300 lines across `DriveIOSim`, `MapleSimSwerveDrivetrain`, and `SimulatedRobotState` — and that does not include the MapleSim library itself. +- Requires the IO interface pattern as a prerequisite (same architectural investment as 6328's approach). +- MapleSim is a third-party dependency maintained by a single FRC team (Shenzhen Robotics Alliance / FRC 5516). Its long-term maintenance and WPILib compatibility are not guaranteed. +- The 2D rigid-body physics, while more realistic than kinematic integration, still cannot model 3D effects (tipping, weight transfer during acceleration, etc.). +- Game-piece simulation requires season-specific implementation work (though MapleSim provides some season-specific modules). + +**Reference**: [MapleSim Documentation](https://shenzhen-robotics-alliance.github.io/maple-sim/) + +### MapleSim — Shenzhen Robotics Alliance (FRC 5516) + +**Repository**: [Shenzhen-Robotics-Alliance/maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim) + +MapleSim is an open-source FRC simulation library that integrates the dyn4j 2D rigid-body dynamics engine into the WPILib simulation framework. It is used by Team 254 (above) and provides template projects for use with AdvantageKit. + +**Key capabilities:** +- `SwerveDriveSimulation` — simulates swerve drivetrain physics including motor propulsion forces, wheel friction, and centripetal forces during turning +- `SwerveModuleSimulation` — per-module simulation with motor models, gear ratios, and wheel friction coefficients +- `GyroSimulation` — simulates IMU readings with configurable drift +- `SimulatedArena` — manages the physics world, including field walls, game pieces, and robot-to-robot collisions +- Season-specific modules for game-piece simulation (Reefscape 2025 coral/algae, Crescendo 2024 notes, Rebuilt 2026 fuel) +- `IntakeSimulation` — simulates game-piece acquisition with proximity detection + +**Architecture notes:** +- MapleSim is designed to be used alongside an IO-layer pattern. It provides the physics engine; the user's code provides the IO implementations that bridge simulated motor/sensor outputs to their subsystem code. +- It runs on a configurable tick rate (default 5ms) independent of the robot loop, providing higher-fidelity physics integration. +- It outputs motor positions and velocities that can be fed into vendor SimState APIs or used directly. + +**Pros:** +- Most realistic physics model available for FRC simulation — models force, friction, collisions, and game-piece interactions. +- Actively maintained with season-specific support. +- Available as a vendor dependency with published JavaDocs and documentation. +- Template projects lower the barrier to adoption. + +**Cons:** +- Third-party dependency with uncertain long-term support. +- Designed to work with IO-layer architectures — adoption into a codebase without hardware abstraction layers requires additional adapter code. +- Adds significant build complexity and dependency overhead. +- The 2D physics engine, while more realistic, is still an approximation and may not match real-world behavior closely enough to trust for PID tuning or precise trajectory validation. + +### WPILib Built-In Simulation + +**Reference**: [WPILib Simulation Documentation](https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/physics-sim.html) + +WPILib provides a set of built-in simulation classes as part of the standard library: + +- `DCMotorSim` — simulates a DC motor with gearing and moment of inertia +- `FlywheelSim`, `ElevatorSim`, `SingleJointedArmSim` — mechanism-specific simulations +- `DifferentialDrivetrainSim` — simulates a differential drivetrain (no official swerve equivalent) +- Vendor SimState APIs (`Pigeon2SimState`, `CANcoderSimState`, `TalonFXSimState`) — allow setting simulated sensor values that are read by vendor library code + +WPILib does not provide a built-in swerve drive simulation class. Teams simulating swerve drives must implement their own kinematic or dynamic model. Both of our approaches and Team 6328's approach do this; Team 254 uses MapleSim for the physics layer. + +The `simulationInit()` and `simulationPeriodic()` hooks in `Robot.java` provide the standard entry points for simulation code. The `DriverStationSim` class allows configuring the simulated driver station state. + +**Pros:** +- Part of the standard library — no additional dependencies. +- Well-documented and widely used by the FRC community. +- `DCMotorSim` provides motor-level physics for individual mechanisms. + +**Cons:** +- No built-in swerve drive simulation — teams must build their own. +- Mechanism sims are independent — there is no built-in way to simulate interactions between mechanisms or with the field environment. +- Vendor SimState API support varies by vendor and is not always well-documented for desktop simulation. + +--- + +## Summary Comparison Matrix + +| Criterion | `SimulationManager` (ours) | `RobotSimulation` (ours) | 6328 IO Layer | 254 IO + MapleSim | +|-----------|---------------------------|-------------------------|---------------|-------------------| +| **Separation from production code** | High — separate package, no `isSimulation()` in subsystem | Low — sim fields and methods inside subsystem, `isSimulation()` in `periodic()` | Complete — interface boundary | Complete — interface boundary | +| **Sensor pipeline exercised** | Yes — writes to vendor SimState, subsystem reads back normally | No — bypasses sensor reading, calls `odometry.update()` directly | Yes — sim IO produces same input structure as real IO | Yes — writes to vendor SimState APIs | +| **Physics model** | Kinematic (velocity × time) | Kinematic (velocity × time) | Motor-level (`DCMotorSim`) | Force-based (dyn4j rigid body) | +| **Per-module control in sim** | Yes | No (chassis-speeds only) | Yes | Yes | +| **Battery simulation** | No | Yes | No (handled separately) | Yes (via MapleSim) | +| **Game-piece simulation** | No | No | No | Yes | +| **Collision detection** | No | No | No | Yes | +| **Dependency on vendor sim APIs** | Yes (Pigeon2, CANcoder, REV encoder) | No | No (uses WPILib `DCMotorSim`) | Yes (Pigeon2, TalonFX, CANcoder) | +| **External dependencies** | None | None | AdvantageKit | AdvantageKit + MapleSim | +| **Code complexity (total sim lines)** | ~220 | ~113 | ~126 (drive module sim only) | ~1,300+ (drive + game sim) | +| **Architectural prerequisite** | None — works with existing codebase | None — works with existing codebase | IO interface pattern across all subsystems | IO interface pattern across all subsystems | +| **Removability** | Delete package + 2 lines in Robot.java | Delete class + remove fields/methods from SwerveSubsystem | Replace IO implementations | Replace IO implementations | + +--- + +## Analysis + +### What both of our approaches get right + +Both `SimulationManager` and `RobotSimulation` achieve the core goal of making the robot move on a Field2d in desktop simulation. Both use kinematic integration (the simplest possible physics model), which is appropriate for the team's current needs: driver practice and basic autonomous path validation. + +### Where they differ meaningfully + +The most significant architectural difference is how each approach interacts with production code: + +**`SimulationManager`** treats the subsystem as a black box. It writes to simulated sensors and lets the subsystem's normal `periodic()` code read those sensors and update odometry. This means: +- The sim exercises the same `periodic()` code path as hardware. +- Bugs in the odometry update logic (e.g., wrong conversion factor, incorrect gyro offset handling) would manifest in sim, potentially catching them earlier. +- However, it depends on vendor SimState APIs working correctly in desktop simulation, which is a partially documented area. + +**`RobotSimulation`** treats the subsystem as a collaborator. It calls `simulationUpdate()`, which directly computes simulated positions and feeds them to the odometry estimator, bypassing the normal sensor reading path. This means: +- The sim is simpler and has fewer external dependencies. +- It does not exercise the sensor-reading code path, so issues in that path would not be caught in sim. +- It requires the subsystem to have a runtime check (`if (!RobotBase.isSimulation())`) in `periodic()`, creating two distinct code paths. + +### How our approaches compare to external teams + +Both of our approaches are **simpler and less architecturally invasive** than what Teams 6328 and 254 use. This is a tradeoff, not a deficiency — the IO-layer pattern requires restructuring the entire codebase around hardware abstraction interfaces. That investment pays off when a team has the experience and bandwidth to maintain it, but it imposes significant upfront and ongoing complexity costs. + +Teams 6328 and 254 have dedicated software mentors with professional software engineering backgrounds, multi-year codebases built around these patterns, and enough team members to maintain the abstraction layers. These are relevant context factors when evaluating whether their approaches would be appropriate for our team. + +Both of our approaches use **kinematic-only physics**, which is the same fundamental model as the simplest implementations from external teams. The force-based physics from MapleSim provides more realism but is primarily useful for autonomous path testing with field interactions and game-piece simulation — capabilities we do not currently need. + +### Practical considerations + +**For the team's current situation**, either approach is functional for the stated goals of driver practice and basic autonomous validation. The choice between them involves these tradeoffs: + +- If exercising the real sensor-reading code path in simulation is valued (catching odometry bugs in sim), `SimulationManager` provides that. +- If minimal code complexity and no vendor sim API dependencies are valued, `RobotSimulation` provides that. +- If per-module simulation capability is needed (e.g., for diagnostic tests that control individual modules), `SimulationManager` provides that. +- If battery voltage and DriverStation state simulation are valued, `RobotSimulation` provides those. + +Neither approach prevents a future migration to an IO-layer pattern if the team decides to pursue that direction. + +--- + +## Appendix A — Feature Portability and Calculation Review + +This appendix evaluates features present in `RobotSimulation` that are absent from `SimulationManager`, assesses whether each is worth integrating, and reviews calculation differences between the two approaches. + +### Feature Evaluation + +#### 1. Battery Voltage Simulation (`BatterySim`) + +**What `RobotSimulation` does:** + +`RobotSimulation.simulationPeriodic()` estimates current draw based on the commanded drive and rotation fractions, then uses WPILib's `BatterySim` to compute a loaded battery voltage: + +```java +double driveFraction = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) / SwerveConstants.maxSpeed; +double rotateFraction = Math.abs(speeds.omegaRadiansPerSecond) / SwerveConstants.maxAngularVelocity; +double estimatedCurrentAmps = 8.0 + 80.0 * driveFraction + 40.0 * rotateFraction; +RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(estimatedCurrentAmps)); +``` + +This models a nominal 12V battery with internal resistance. At idle the simulated draw is 8A; at full translational speed it reaches ~88A; at full translation + rotation it reaches ~128A. `BatterySim.calculateDefaultBatteryLoadedVoltage()` applies `V_loaded = V_nominal - I * R_internal` (default R_internal = 0.02Ω), producing a voltage drop of ~1.6–2.6V under full load. + +**Is it worth integrating into `SimulationManager`?** + +Low priority. The current estimate is rough — it uses speed fractions as a proxy for current draw rather than actual motor models or torque calculations. The resulting voltage drop affects the value returned by `RobboRioSim.getVInVoltage()`, which matters only if: + +- Subsystem code reads `RobotController.getBatteryVoltage()` and adjusts behavior (e.g., voltage compensation on motor controllers). +- Dashboard or logging tools display battery voltage during simulation. +- Other subsystems (LEDs, pneumatics compressor) use battery voltage to make decisions. + +Currently, none of our subsystem code reads battery voltage to modify behavior. Motor controllers configured with voltage compensation read voltage from their own internal firmware, not from `RobotController.getBatteryVoltage()`, so the simulated RoboRIO voltage does not affect motor output in sim. + +**If integrated**, it would be straightforward — approximately 5 lines added to `SimulationManager.simulationPeriodic()` after computing `desiredChassisSpeeds`. It does not require any changes to `SwerveSubsystem` or `SwerveModule`. The main benefit would be a more realistic voltage readout in the Sim GUI and AdvantageScope logs. + +**Assessment:** Not harmful to add, but provides no functional benefit with the current codebase. Could be added later if subsystem code begins using battery voltage. + +#### 2. DriverStation State Simulation + +**What `RobotSimulation` does:** + +`RobotSimulation.simulationInit()` programmatically sets DriverStation state: + +```java +DriverStationSim.setDsAttached(true); +DriverStationSim.setEnabled(false); +DriverStationSim.setAutonomous(false); +DriverStationSim.setTest(false); +DriverStationSim.notifyNewData(); +``` + +This establishes a known initial state: driver station connected, robot disabled, not in autonomous or test mode. + +**Is it worth integrating into `SimulationManager`?** + +This programmatic setup is redundant with functionality provided by the existing simulation infrastructure. Two mechanisms already control DriverStation state without any code in the simulation framework: + +1. **Sim GUI (`halsim_gui`)** — enabled by `wpi.sim.addGui()` in `build.gradle`. The Sim GUI displays a "Robot State" widget with clickable buttons for Disabled / Autonomous / Teleoperated / Test, and shows DS connection status. Its default startup state is DS connected + robot disabled — identical to what the `RobotSimulation` code sets programmatically. + +2. **Real FRC Driver Station via `halsim_ds_socket`** — enabled by `wpi.sim.addDriverstation()` in `build.gradle`. When the real Driver Station application connects to the simulation (Windows only), it takes over control of all DriverStation state: enabled/disabled, mode selection, joystick data, match time, and alliance info. Any values set programmatically by `DriverStationSim` are overridden by the real DS on connection. + +In both cases, the DriverStation state is managed externally — by the Sim GUI interactively, or by the real DS application via the socket protocol. The programmatic `DriverStationSim` calls in `RobotSimulation` set the same defaults that these mechanisms already establish. + +The programmatic setup would provide independent value if the simulation were run headless (no Sim GUI, no real DS) — for example, in a CI/CD pipeline. That does not apply to the current workflow. + +Note: `SimulationManager` on the current branch already handles joystick warning suppression in `Robot.simulationInit()` (via the `sim.silenceJoystick` system property), which addresses the most common sim startup annoyance. + +**Assessment:** Not integrating. The functionality is already provided by the Sim GUI and the `halsim_ds_socket` extension, both of which are configured in `build.gradle`. + +#### 3. Simulation Reset + +**What `RobotSimulation` does:** + +`RobotSimulation.simulationInit()` calls `drive.simulationReset()`, which resets all simulation state to a known origin: + +```java +public void simulationReset() { + simYawDegrees = getYaw().getDegrees(); + for (int i = 0; i < 4; i++) { + simWheelPositionsMeters[i] = 0.0; + simWheelAngles[i] = new Rotation2d(); + } + pigeon.setYaw(simYawDegrees); + odometry.resetPosition(Rotation2d.fromDegrees(simYawDegrees), positions, new Pose2d()); +} +``` + +**Is it worth integrating into `SimulationManager`?** + +No. The startup initialization that `simulationReset()` provides is already handled by `SimulationManager`'s construction. When `SimulationManager` is created in `Robot.simulationInit()`, its `simPose` field initializes to `new Pose2d()` (origin), `lastTime` is set to the current FPGA timestamp, and encoder positions start at whatever the REV sim state defaults to (typically zero). This achieves the same clean starting state without an explicit reset method. + +Note that `RobotSimulation`'s reset is also startup-only — `simulationReset()` is called from `simulationInit()`, which WPILib calls once when the simulation process starts. There is no user-facing button, command, or key binding to trigger it mid-run in that framework either. The only way to re-trigger it is to restart the simulation process. Neither approach provides a user-accessible mid-run reset. + +**What about a mid-run reset as a new enhancement?** + +A separate question is whether `SimulationManager` should expose a mid-run reset capability that neither approach currently provides. Evaluating the scenarios where this might be useful: + +- **Testing sequential autonomous paths:** Each auto begins with `startAutoAt()` or PathPlanner's `resetPose()` callback, which resets odometry to the path's starting pose — not necessarily origin. The problem in this scenario is not "dirty state" but rather that `SimulationManager.simPose` is not synced to the new pose. On the next cycle, `SimulationManager` writes the stale heading to the pigeon SimState, corrupting the freshly-reset odometry baseline (see `sensors-and-pose-estimation.md`). The correct fix for this is a **pose sync mechanism** (`SimulationManager.syncPose(Pose2d)`) called from the odometry reset path — not a user-triggered "reset to origin." +- **Driver practice — returning to origin:** Restarting the simulation takes a few seconds and guarantees a fully clean state. A "reset to origin" button offers marginal convenience over a restart. +- **Recovering from simulation artifacts:** If odometry becomes corrupted (e.g., from the simPose desync described above), a "reset to origin" is a workaround for a bug, not a feature. Fixing the underlying desync is the proper solution. + +**Assessment:** Not integrating. The startup initialization is already handled by construction. A mid-run "reset to origin" has limited practical value — the scenarios that motivate it are better addressed by pose synchronization (keeping `SimulationManager.simPose` in sync when odometry is reset by autonomous commands), which is a different problem that would be addressed separately if needed. + +#### 4. Explicit Disabled-State Handling + +**What `RobotSimulation` does:** + +Inside `SwerveSubsystem.simulationUpdate()`: + +```java +ChassisSpeeds speeds = DriverStation.isDisabled() ? new ChassisSpeeds() : lastCommandedSpeeds; +``` + +This explicitly substitutes zero speeds when the robot is disabled, regardless of what `lastCommandedSpeeds` contains. + +**What `SimulationManager` does:** + +No explicit disabled check. It reads `swerveSubsystem.getDesiredStates()`, which reads each module's `desiredState` field. When the robot is disabled, the command scheduler does not run commands, so no new desired states are set. Modules retain their last `desiredState` from before disable. + +**Is it worth integrating into `SimulationManager`?** + +This is a minor robustness concern. When the robot transitions from enabled to disabled, the modules' `desiredState` fields retain the last commanded values. Since no new commands run while disabled, `SimulationManager` would continue integrating those stale velocities, causing the simulated robot to drift. + +In practice, this is partially mitigated by the fact that the swerve default command (which typically commands zero speeds when the joystick is centered) runs while enabled and would have set near-zero desired states before disable. But if the robot is disabled while actively driving, a brief drift would occur until the next `simulationPeriodic()` reads the stale non-zero desired states. + +A simple fix would be to add a `DriverStation.isDisabled()` check in `SimulationManager.simulationPeriodic()`: + +```java +if (DriverStation.isDisabled()) { + desiredChassisSpeeds = new ChassisSpeeds(); + desiredStates = null; // skip module encoder updates +} +``` + +**Assessment:** Worth integrating. It is a one-line guard that prevents a real (if minor) simulation artifact. It does not affect the architecture. + +--- + +### Calculation Differences + +#### 1. Time Delta (`dt`) Clamping + +| | `SimulationManager` | `RobotSimulation` | +|---|---|---| +| **Code** | `if (dt <= 0 \|\| dt > 1.0) dt = 0.02` | `MathUtil.clamp(now - lastTimestampSeconds, 0.0, 0.05)` | +| **Behavior when dt = 0** | Substitutes 0.02s (simulates motion that did not happen) | Uses 0.0 (no motion — correct) | +| **Behavior when dt = 0.2s** (e.g., GC pause) | Uses 0.2s (large pose jump) | Clamps to 0.05s (limits jump, loses 0.15s of motion) | +| **Behavior when dt = -0.001s** (clock jitter) | Substitutes 0.02s | Clamps to 0.0 | +| **Upper bound** | 1.0s | 0.05s | + +**Analysis:** + +The `RobotSimulation` approach is more defensive. The 50ms upper clamp prevents large pose jumps during GC pauses or debugger breakpoints. `SimulationManager`'s 1.0s upper bound is permissive enough that a 500ms pause (not uncommon during debugging) would cause a visible position jump. + +However, the `RobotSimulation` approach of clamping dt to 0 when it is zero or negative is more correct than substituting 0.02s. A dt of zero should produce zero motion, not an artificial 20ms of movement. + +**Recommendation:** Adopt the `MathUtil.clamp()` pattern with a tighter upper bound. A reasonable upper bound is 50ms (2.5× the nominal loop period). This prevents large pose jumps while still producing smooth motion during normal operation: + +```java +double dt = MathUtil.clamp(currentTime - lastTime, 0.0, 0.05); +``` + +#### 2. Pose Integration Method + +| | `SimulationManager` | `RobotSimulation` | +|---|---|---| +| **Code** | `simPose = simPose.exp(new Twist2d(vx*dt, vy*dt, omega*dt))` | `simYawDegrees += Math.toDegrees(omega * dt)` + per-wheel linear accumulation | +| **Mathematical model** | Exponential map (SE(2) Lie group) | Euler integration | + +**Analysis:** + +`Pose2d.exp(Twist2d)` computes the exact integral of constant-velocity motion over the time step, correctly modeling the arc the robot follows when simultaneously translating and rotating. The Euler integration in `RobotSimulation` accumulates yaw separately from position, which is less accurate when the robot is turning and translating simultaneously. + +At 20ms time steps, the numerical difference between exponential and Euler integration is small (the error is proportional to `dt²`). For a robot rotating at 1 rad/s while translating at 3 m/s, the per-step position error is on the order of 0.0006 meters — negligible for driver practice. + +However, the exponential map is the standard approach recommended by WPILib (it is what `SwerveDrivePoseEstimator` uses internally), it has no additional computational cost, and it is already implemented in `SimulationManager`. + +**Recommendation:** No change needed. `SimulationManager` already uses the more correct method. + +#### 3. Wheel Speed Desaturation + +| | `SimulationManager` | `RobotSimulation` | +|---|---|---| +| **Code** | Reads `getDesiredState()` directly (already desaturated by `setDesiredState()` call chain) | `SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.maxSpeed)` after converting back from chassis speeds | + +**Analysis:** + +In `SimulationManager`, the desired states have already passed through the normal swerve command pipeline: `driveFromChassisSpeeds()` → `SwerveDriveKinematics.toSwerveModuleStates()` → `desaturateWheelSpeeds()` → `setDesiredState()`. So the states `SimulationManager` reads are already desaturated. + +`RobotSimulation` applies desaturation again after converting `lastCommandedSpeeds` back to module states. This is necessary in its design because it starts from chassis speeds (which may round-trip through kinematics differently), but it is a redundant operation. + +In `SimulationManager`, direct module commands from diagnostic tests bypass `driveFromChassisSpeeds()` and may not be desaturated. However, diagnostic tests typically command individual modules at controlled speeds, so desaturation is not expected to apply. + +**Recommendation:** No change needed. `SimulationManager` reads already-desaturated states. + +#### 4. Gyro Update Method + +| | `SimulationManager` | `RobotSimulation` | +|---|---|---| +| **Code** | `pigeonSimState.setRawYaw(simPose.getRotation().getDegrees())` | `pigeon.setYaw(simYawDegrees)` | +| **API used** | Vendor `Pigeon2SimState.setRawYaw()` (sim API) | `Pigeon2.setYaw()` (production API) | +| **Source of truth** | Derived from integrated `simPose` heading | Accumulated in `simYawDegrees` via `+= Math.toDegrees(omega * dt)` | + +**Analysis:** + +`SimulationManager` uses the CTRE `Pigeon2SimState` API, which is the designated simulation path — it sets the simulated raw sensor value without affecting configuration state. `RobotSimulation` uses `Pigeon2.setYaw()`, which is the production API for zeroing/setting the gyro heading. Using the production API in sim works, but it conflates "setting the simulated sensor reading" with "commanding the gyro to recalibrate to a heading," which are conceptually different operations. + +`SimulationManager` derives the yaw from `simPose.getRotation()`, which is computed via the exponential map. `RobotSimulation` accumulates yaw separately via Euler integration (`+=`). This means `SimulationManager`'s yaw is consistent with its x/y pose, while `RobotSimulation`'s yaw could accumulate a small drift relative to the pose that its odometry computes (though in practice the difference is negligible). + +**Recommendation:** No change needed. `SimulationManager` already uses the more appropriate API. + +--- + +### Summary of Recommendations + +| Feature / Calculation | Worth Integrating? | Effort | Priority | +|---|---|---|---| +| **Disabled-state guard** | Yes | ~3 lines | Medium — prevents simulation drift when disabled | +| **Tighter dt clamping** | Yes | ~1 line change | Medium — prevents large pose jumps during debugging | +| **Battery voltage simulation** | Not currently | ~5 lines | Low — no subsystem code reads battery voltage | +| **DriverStation state setup** | No | ~5 lines | None — Sim GUI provides the same functionality | +| **Simulation reset method** | No | N/A | None — startup handled by construction; mid-run scenarios need pose sync, not reset | +| **Pose integration method** | Already better in `SimulationManager` | N/A | N/A | +| **Gyro update API** | Already better in `SimulationManager` | N/A | N/A | +| **Wheel desaturation** | Not needed | N/A | N/A | + +The two changes recommended for near-term integration (disabled-state guard and tighter dt clamping) are both small, isolated, and do not affect the `SimulationManager` architecture or its relationship with the subsystem. + +--- + +## Appendix B — `SimulationManager` Data Flow and Code Path Analysis + +This appendix explains how the `SimulationManager` approach achieves its goal of reusing production code paths in simulation. It provides side-by-side comparisons of the data flow and code execution in real robot mode vs. simulation mode, showing that the production code is identical in both modes. + +### The Core Idea + +On the real robot, physical motors turn physical wheels, physical encoders measure how far the wheels have moved, and a physical gyro measures which direction the robot is facing. The subsystem reads these sensors every 20 ms and feeds the readings into the pose estimator to determine where the robot is on the field. + +In simulation, there are no physical motors, wheels, encoders, or gyro. The `SimulationManager` fills the gap by computing what the sensor readings *would be* if the robot were moving as commanded, and writing those computed values into the vendor simulation APIs. When the subsystem's `periodic()` method runs — the exact same code as on the real robot — it reads those simulated sensor values and updates the pose estimator as usual. + +The result: the subsystem does not know or care whether it is running on a real robot or in simulation. Its `periodic()` method, its odometry, and its Field2d visualization all work identically in both modes. + +### Data Flow Comparison + +The following diagrams show the complete data flow for a single 20 ms loop cycle in each mode. **Bold** text marks the steps that differ between real and simulation mode. All other steps are identical code. + +#### Real Robot Mode + +``` +Driver Input (joystick) + │ + ▼ +TeleopSwerve.execute() + │ applies deadband, slew rate limiting, scales by max speed + ▼ +SwerveSubsystem.drive(x, y, rot, fieldOriented) + │ converts to ChassisSpeeds (field-relative or robot-relative) + ▼ +SwerveSubsystem.driveFromChassisSpeeds(speeds, openLoop) + │ kinematics.toSwerveModuleStates() → desaturateWheelSpeeds() + ▼ +SwerveModule.setDesiredState(state, openLoop) ← ×4 modules + │ optimize() → stores desiredState → setAngle() → setSpeed() + ▼ +Motor controllers execute PID commands + │ ▪ angle motor rotates wheel to target angle + │ ▪ drive motor spins wheel at target speed + ▼ +Physical wheels move → physical sensors update + │ ▪ drive encoder position increases as wheel rolls + │ ▪ angle encoder position reflects current wheel angle + │ ▪ Pigeon2 gyro yaw reflects current robot heading + ▼ +SwerveSubsystem.periodic() + │ getYaw() → pigeon.getYaw() → reads physical gyro + │ getPositions() → driveEncoder.getPosition() → reads physical encoder + │ → angleEncoder.getPosition() → reads physical encoder + │ odometry.update(yaw, positions) → fuses into pose estimate + │ field.setRobotPose(getPose()) → updates Field2d + │ robotPose.set(getPose()) → publishes pose for AdvantageScope + ▼ +Dashboard / AdvantageScope shows robot position on field +``` + +#### Simulation Mode + +``` +Driver Input (joystick — real or simulated) + │ + ▼ +TeleopSwerve.execute() ← SAME CODE + │ applies deadband, slew rate limiting, scales by max speed + ▼ +SwerveSubsystem.drive(x, y, rot, fieldOriented) ← SAME CODE + │ converts to ChassisSpeeds (field-relative or robot-relative) + ▼ +SwerveSubsystem.driveFromChassisSpeeds(speeds, openLoop) ← SAME CODE + │ kinematics.toSwerveModuleStates() → desaturateWheelSpeeds() + ▼ +SwerveModule.setDesiredState(state, openLoop) ← ×4, SAME CODE + │ optimize() → stores desiredState → setAngle() → setSpeed() + ▼ +Motor controllers NO-OP (no physical hardware) + │ ▪ PID commands are issued but have no effect + │ ▪ No physical wheels move + │ ▪ desiredState field retains the commanded state + ▼ + ╔══════════════════════════════════════════════════════════╗ + ║ SimulationManager.simulationPeriodic() — SIM ONLY ║ + ║ ║ + ║ 1. Read desired states from modules ║ + ║ desiredStates = swerveSubsystem.getDesiredStates() ║ + ║ ║ + ║ 2. Compute what the robot would do ║ + ║ chassisSpeeds = kinematics.toChassisSpeeds(states) ║ + ║ simPose = simPose.exp(Twist2d(vx*dt, vy*dt, ω*dt)) ║ + ║ ║ + ║ 3. Write simulated sensor values ║ + ║ pigeonSimState.setRawYaw(simPose heading) ║ + ║ driveEncoder.setPosition(position + speed*dt) ║ + ║ angleEncoder.setPosition(desired angle) ║ + ║ cancoderSimState.setRawPosition(desired angle) ║ + ╚══════════════════════════════════════════════════════════╝ + │ + ▼ +SwerveSubsystem.periodic() ← SAME CODE + │ getYaw() → pigeon.getYaw() → reads SIMULATED gyro + │ getPositions() → driveEncoder.getPosition() → reads SIMULATED encoder + │ → angleEncoder.getPosition() → reads SIMULATED encoder + │ odometry.update(yaw, positions) → fuses into pose estimate + │ field.setRobotPose(getPose()) → updates Field2d + │ robotPose.set(getPose()) → publishes pose for AdvantageScope + ▼ +Dashboard / AdvantageScope shows robot position on field ← SAME CODE +``` + +The only difference is the boxed section: `SimulationManager` runs between the motor commands and the sensor reads, filling in the sensor values that physical hardware would have produced. Everything above the box (command processing) and everything below the box (odometry, Field2d) is identical production code. + +### Code Path Comparison + +The following table shows the actual methods called during a single loop cycle. The "Real Robot" and "Simulation" columns indicate what each method call does in each mode. Methods where the code itself is identical are marked with **=**. + +| Step | Method | Real Robot | Simulation | +|------|--------|-----------|------------| +| 1 | `TeleopSwerve.execute()` | Reads joystick, computes speeds | **=** Same code | +| 2 | `SwerveSubsystem.drive()` | Converts to `ChassisSpeeds` | **=** Same code | +| 3 | `driveFromChassisSpeeds()` | Kinematics → module states | **=** Same code | +| 4 | `SwerveModule.setDesiredState()` | Optimizes, stores state, commands motors | **=** Same code (motors no-op) | +| 4a | `setAngle()` | `angleController.setReference()` → motor turns | **=** Same code (no-op in sim) | +| 4b | `setSpeed()` | `driveController.setReference()` → motor spins | **=** Same code (no-op in sim) | +| 5 | **`SimulationManager.simulationPeriodic()`** | *Does not run* | Computes motion, writes to sim sensors | +| 6 | `SwerveSubsystem.periodic()` | Reads physical sensors | **=** Same code (reads simulated sensors) | +| 6a | `pigeon.getYaw()` | Returns physical gyro heading | **=** Same code (vendor lib returns sim value) | +| 6b | `driveEncoder.getPosition()` | Returns physical encoder distance | **=** Same code (vendor lib returns sim value) | +| 6c | `integratedAngleEncoder.getPosition()` | Returns physical encoder angle | **=** Same code (vendor lib returns sim value) | +| 7 | `odometry.update(yaw, positions)` | Fuses physical sensor readings | **=** Same code (fuses simulated readings) | +| 8 | `field.setRobotPose(getPose())` | Displays physical pose on Field2d | **=** Same code (displays simulated pose) | +| 9 | `robotPose.set(getPose())` | Publishes pose to NetworkTables for AdvantageScope | **=** Same code (publishes simulated pose) | + +Steps 1–4 and 6–9 execute the same Java methods with the same code in both modes. Step 5 is the only addition — it runs exclusively in simulation and only writes to sensor simulation APIs. + +### How Vendor Libraries Enable This + +The key to this design is that vendor libraries (CTRE Phoenix 6 for Pigeon2 and CANcoder, REV for SparkMax/SparkFlex encoders) internally handle the real-vs-simulation routing: + +``` +Production code calls: pigeon.getYaw() + │ + ┌────────────┴────────────┐ + ▼ ▼ + Real robot mode Simulation mode + Read hardware via CAN Return value from SimState + (physical sensor) (set by SimulationManager) +``` + +The production code — `pigeon.getYaw()`, `driveEncoder.getPosition()`, `integratedAngleEncoder.getPosition()` — never checks `RobotBase.isSimulation()`. The vendor library does that internally. This means: + +- `SwerveSubsystem.periodic()` contains **zero** simulation-specific conditionals +- `SwerveModule.setDesiredState()` contains **zero** simulation-specific conditionals +- `SwerveModule.getState()` and `getPosition()` contain **zero** simulation-specific conditionals + +The `SimulationManager` writes to the "back door" of these vendor objects (the SimState APIs), and the production code reads from the "front door" (the normal getter methods). The vendor library connects the two internally. + +### What This Means for Bug Detection + +Because the production `periodic()` code path runs identically in simulation, certain categories of bugs would manifest in simulation the same way they do on the real robot: + +| Bug Category | Detected in Sim? | Why | +|-------------|------------------|-----| +| Wrong encoder conversion factor | ✅ Yes | `periodic()` reads the same encoder object with the same conversion factor | +| Gyro sign inversion (e.g., `invertPigeon` configured wrong) | ✅ Yes | `getYaw()` applies the same inversion logic to simulated yaw | +| Odometry reset not updating gyro baseline | ✅ Yes | `resetOdometry()` calls the same `resetPosition()` with the same gyro value | +| Wrong kinematics (module positions) | ✅ Yes | Same `SwerveDriveKinematics` instance used in both modes | +| Module optimization bug (e.g., angle accumulation) | ✅ Yes | Same `optimize()` method runs in both modes | +| Motor PID tuning issues | ❌ No | Motors no-op in sim; desired speed is assumed to be achieved instantly | +| Wheel slip / friction effects | ❌ No | No force-based physics model | +| Mechanical issues (loose belt, broken encoder) | ❌ No | Simulation assumes perfect hardware | + +The first five rows are the primary benefit of the `SimulationManager` approach: the full sensor-to-odometry pipeline is exercised in simulation using the same code path, so bugs in that pipeline are caught. + +### Production Code Modifications Required + +The `SimulationManager` approach requires a small set of additions to production code. These are accessor methods only — they do not change any existing behavior. + +**`SwerveModule` additions:** + +| Addition | Purpose | Lines | +|----------|---------|-------| +| `desiredState` field | Stores the optimized state from `setDesiredState()` for simulation to read | 1 | +| `getDesiredState()` | Returns the stored desired state | 3 | +| `getCanCoderDevice()` | Exposes CANcoder hardware object for SimState access | 3 | +| `getDriveEncoder()` | Exposes drive encoder for `setPosition()` in sim | 3 | +| `getAngleEncoder()` | Exposes angle encoder for `setPosition()` in sim | 3 | + +**`SwerveSubsystem` additions:** + +| Addition | Purpose | Lines | +|----------|---------|-------| +| `getDesiredStates()` | Collects desired states from all four modules | 6 | +| `getPigeon()` | Exposes Pigeon2 for SimState access | 3 | +| `getModules()` | Exposes module array (defensive copy) | 3 | +| `getKinematics()` | Exposes kinematics for chassis speed calculation | 3 | +| `getOdometry()` | Exposes pose estimator for reset support | 3 | + +**Unchanged production methods** (these run identically in both modes): + +- `SwerveSubsystem.periodic()` — no `isSimulation()` check +- `SwerveSubsystem.drive()` +- `SwerveSubsystem.driveFromChassisSpeeds()` +- `SwerveSubsystem.getYaw()` +- `SwerveSubsystem.getPositions()` +- `SwerveSubsystem.resetOdometry()` +- `SwerveModule.setDesiredState()` +- `SwerveModule.getState()` +- `SwerveModule.getPosition()` +- All command classes (`TeleopSwerve`, `AutoAlign`, autonomous commands) + +### Removability + +The simulation support can be completely removed without affecting production code behavior: + +1. Delete `src/main/java/frc/robot/sim/SimulationManager.java` +2. Remove two lines from `Robot.java` (`simManager` field declaration and `simulationInit()`/`simulationPeriodic()` bodies) +3. Optionally remove the accessor methods from `SwerveModule` and `SwerveSubsystem` (they are unused by production code, but leaving them causes no harm) + +No production code behavior changes because the accessor methods are never called by production code — they are only called by `SimulationManager`. diff --git a/docs/simulation-test-architecture.md b/docs/simulation-test-architecture.md new file mode 100644 index 0000000..88feb42 --- /dev/null +++ b/docs/simulation-test-architecture.md @@ -0,0 +1,1818 @@ +# Simulation and Test Architecture - Complete Guide + +**Date:** February 15, 2026 +**Branch:** `jmm-sim-test-support` +**Status:** Design Review and Recommendations + +--- + +## Table of Contents + +1. [Quick Reference](#1-quick-reference) +2. [Executive Summary](#2-executive-summary) +3. [Recommended Code Changes](#3-recommended-code-changes) +4. [Architecture Diagrams](#4-architecture-diagrams) +5. [Control Flow Analysis](#5-control-flow-analysis) +6. [Implementation Assessment](#6-implementation-assessment) +7. [Design Principles](#7-design-principles) +8. [Known Issues and Future Improvements](#8-known-issues-and-future-improvements) +9. [Understanding Hardware Simulation](#9-understanding-hardware-simulation) +10. [Implementation Guidelines](#10-implementation-guidelines) +11. [Appendices](#11-appendices) + +--- + +## 1. Quick Reference + +**TL;DR:** Your implementation is fundamentally correct. Only minor improvements needed. + +### ✅ What's Working Well + +1. **Control flow is correct:** Commands → Subsystem → Modules → `setDesiredState()` +2. **Simulation is externalized:** All logic in `SimulationManager.java` +3. **Tests are externalized:** All logic in `test` package +4. **Capture point is correct:** Store desired states at `setDesiredState()` level +5. **Field2d works:** Same object for real robot and simulation + +### ⚠️ Minor Issues to Fix + +1. **Unused `pointInDirection()` method** - Already fixed to use `setDesiredState()` +2. **Documentation** - Add clarifying comments (see Recommended Code Changes) +3. **Diagnostics** - Add SmartDashboard output (optional but helpful) + +### 🎯 Key Design Principles + +1. **Single Entry Point:** All module commands flow through `setDesiredState()` +2. **Separation of Concerns:** Production, simulation, and test code in separate layers +3. **No Conditionals:** Production code never checks `RobotBase.isSimulation()` + +### 🏗️ Architecture Overview + +**Control Flow:** +``` +User Input → TeleopSwerve → SwerveSubsystem.drive() + → driveFromChassisSpeeds() → module.setDesiredState() + → setAngle() + setSpeed() → Motor Controllers +``` + +**Simulation Flow:** +``` +module.setDesiredState() stores optimizedState + ↓ +SimulationManager reads getDesiredStates() + ↓ +Calculates ChassisSpeeds (kinematics) + ↓ +Integrates: simPose += speeds * deltaTime + ↓ +Updates: Pigeon2SimState, Encoder positions, CANcoder + ↓ +SwerveSubsystem.periodic() reads simulated sensors + ↓ +SwerveSubsystem.periodic() updates odometry and Field2d +``` + +**Test Flow:** +``` +SmartDashboard trigger → SwerveDriftTestManager + → Schedules SwerveAngleDriftTestCommand + → Gets module via getModule() + → Calls module.setDesiredState() + → Reads via getState(), getCanCoder() + → Records and analyzes results +``` + +--- + +## 2. Executive Summary + +This document provides a comprehensive analysis of the current simulation and test architecture for the swerve drive robot code. After careful review of the control flow, **your current implementation is fundamentally sound and well-designed**. The architecture successfully separates concerns between production code, simulation logic, and test code. + +**Key Findings:** +- ✅ Control flow from user input → commands → subsystem → modules is correct +- ✅ Simulation logic is properly externalized to `SimulationManager` +- ✅ Test logic is properly externalized to test packages +- ✅ The design handles both normal operation and individual module testing +- ⚠️ Minor improvements needed for consistency and completeness + +**Assessment Summary:** + +**What's Working Well:** +1. Control flow is correct: User input → Commands → Subsystem → Modules +2. Simulation is properly externalized: All simulation logic in `SimulationManager` +3. Test code is properly separated: Test commands in `test` package +4. Capture point is correct: Desired states stored at `setDesiredState()` level (not lower) +5. Field2d integration works correctly: Same object for real robot and simulation + +**Minor Issues to Address:** +1. Unused `pointInDirection()` method - **Already fixed** to use `setDesiredState()` +2. Documentation could be clearer - Add comments about simulation architecture +3. Diagnostics could be better - Add SmartDashboard output for debugging + +--- + +## 3. Recommended Code Changes + +### 3.1 Fix or Remove `pointInDirection()` Method + +**Status:** ✅ **Already Fixed** - The method now uses `setDesiredState()` internally. + +**Location:** `SwerveModule.java:358-361` + +**Current Implementation (Fixed):** +```java +/** + * Points the wheel in a specific direction without changing drive speed. + *

+ * This method rotates the wheel to the specified angle (in degrees) while keeping + * the drive motor stopped. Useful for testing, calibration, or positioning the wheel + * without moving the robot. + *

+ * This method internally uses {@link #setDesiredState(SwerveModuleState, boolean)} + * to ensure simulation and test code can track the commanded state. This maintains + * consistency with the simulation architecture where all module commands flow through + * {@code setDesiredState()}. + * + * @param degrees The target wheel angle in degrees (0-360) + */ +public void pointInDirection(double degrees){ + // Use setDesiredState to maintain consistency with simulation + // Speed = 0.0 (wheel doesn't drive), angle = desired direction, closed loop control + setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(degrees)), false); +} +``` + +--- + +### 3.2 Improve Documentation in `setDesiredState()` + +**Location:** `SwerveModule.java:140-158` + +**Current Javadoc:** +```java +/** + * Sets the module to the desired state (speed and angle). + *

+ * This is the main method for controlling the swerve module. It optimizes the desired + * state to minimize rotation distance, then sets both the wheel angle and drive speed. + * + * @param desiredState The target module state (speed in m/s and wheel angle) + * @param isOpenLoop If true, uses open loop control for drive motor; if false, uses closed loop velocity control + */ +``` + +**Recommended Addition:** +```java +/** + * Sets the module to the desired state (speed and angle). + *

+ * This is the main method for controlling the swerve module. It optimizes the desired + * state to minimize rotation distance, then sets both the wheel angle and drive speed. + *

+ * IMPORTANT FOR SIMULATION/TESTING: This method stores the optimized state in + * the {@code desiredState} field, which is read by {@link frc.robot.sim.SimulationManager} + * to simulate robot motion. All control commands (driving, testing, autonomous) must + * flow through this method to ensure simulation works correctly. + *

+ * Control Flow: + *

    + *
  • Normal driving: TeleopSwerve → SwerveSubsystem.drive() → this method
    • + *
  • Test commands: TestCommand → this method (directly)
    • + *
  • Autonomous: Auto command → SwerveSubsystem → this method
    • + *
+ * + * @param desiredState The target module state (speed in m/s and wheel angle) + * @param isOpenLoop If true, uses open loop control for drive motor; if false, uses closed loop velocity control + */ +public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { + // Optimize the desired state to minimize rotation (flip wheel 180° if needed) + SwerveModuleState optimizedState = optimize(desiredState, getAngle()); + // Store desired state for simulation access + this.desiredState = optimizedState; + // Set the wheel angle to the optimized direction + setAngle(optimizedState); + // Set the drive motor speed (open loop or closed loop based on parameter) + setSpeed(optimizedState, isOpenLoop); +} +``` + +--- + +### 3.3 Add Simulation Diagnostics to SmartDashboard + +**Location:** `SimulationManager.java:66-111` + +**Recommended Addition:** +```java +public void simulationPeriodic() { + // ... existing code ... + + // === NEW: Add diagnostics to SmartDashboard === + publishSimulationDiagnostics(dt, desiredChassisSpeeds); +} + +/** + * Publishes simulation diagnostics to SmartDashboard for debugging. + * This helps verify that simulation is working correctly. + * + * @param dt The time delta for this simulation update + * @param speeds The current chassis speeds + */ +private void publishSimulationDiagnostics(double dt, ChassisSpeeds speeds) { + SmartDashboard.putString("Sim/Status", "Running"); + SmartDashboard.putNumber("Sim/DeltaTime", dt); + SmartDashboard.putNumber("Sim/Pose/X", simPose.getX()); + SmartDashboard.putNumber("Sim/Pose/Y", simPose.getY()); + SmartDashboard.putNumber("Sim/Pose/Rotation", simPose.getRotation().getDegrees()); + SmartDashboard.putNumber("Sim/Speeds/VX", speeds.vxMetersPerSecond); + SmartDashboard.putNumber("Sim/Speeds/VY", speeds.vyMetersPerSecond); + SmartDashboard.putNumber("Sim/Speeds/Omega", speeds.omegaRadiansPerSecond); +} +``` + +**Don't forget to add the import:** +```java +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +``` + +--- + +### 3.4 Add Simulation Section Comment in SwerveModule + +**Location:** `SwerveModule.java:488-492` + +**Current Code:** +```java +// ============================================================================ +// Simulation Support Methods +// These methods are only used by SimulationManager. +// They expose internal objects needed for simulating robot motion. +// ============================================================================ +``` + +**Recommended Enhancement:** +```java +// ============================================================================ +// Simulation Support Methods +// ============================================================================ +// These methods are only used by SimulationManager to access internal +// hardware objects for simulation. They should NOT be used by production code. +// +// ARCHITECTURE NOTE: +// The simulation architecture works as follows: +// 1. Commands (driving, testing) call setDesiredState() +// 2. setDesiredState() stores the optimized state in desiredState field +// 3. SimulationManager reads desiredState via getDesiredState() +// 4. SimulationManager updates hardware simulation state objects based on the +// desired states: +// - CTRE devices (Pigeon2, CANcoder): Updates separate SimState objects +// (e.g., pigeonSimState.setRawYaw(), cancoderSimState.setRawPosition()) +// - REV devices (RelativeEncoder): Updates encoder directly via setPosition() +// When production code reads the real hardware objects in sim mode, they +// automatically return values from these simulation states. The vendor libraries +// (CTRE Phoenix, REV) internally check RobotBase.isSimulation() to determine +// whether to read from actual hardware (real mode) or simulation state (sim mode). +// 5. Production code (SwerveSubsystem.periodic()) reads real hardware objects +// (which return simulated values in sim mode) and updates odometry +// 6. Production code (SwerveSubsystem.periodic()) updates Field2d based on odometry +// (which is updated using simulated sensor readings in sim mode) +// +// This design keeps simulation logic completely separate from production code. +// ============================================================================ +``` + +--- + +### 3.5 Add Validation Comments in Robot.java + +**Location:** `Robot.java:104-115` + +**Current Code:** +```java +@Override +public void simulationInit() { + // Initialize simulation manager for driver practice simulation + simManager = new SimulationManager(m_robotContainer.getSwerveSubsystem()); +} + +@Override +public void simulationPeriodic() { + // Run simulation manager (handles both normal simulation and API testing) + if (simManager != null) { + simManager.simulationPeriodic(); + } +} +``` + +**Recommended Enhancement:** +```java +@Override +public void simulationInit() { + // Initialize simulation manager for driver practice simulation + // The SimulationManager handles all simulation logic: + // - Reads desired module states from modules + // - Integrates robot motion (position, rotation) + // - Updates hardware simulations (gyro, encoders) + // - Works for both normal driving and individual module testing + simManager = new SimulationManager(m_robotContainer.getSwerveSubsystem()); +} + +@Override +public void simulationPeriodic() { + // Run simulation manager every 20ms to update simulated hardware + // This runs in addition to robotPeriodic() which runs commands and subsystems + if (simManager != null) { + simManager.simulationPeriodic(); + } +} +``` + +--- + +## 4. Architecture Diagrams + +### 4.1 Overall System Architecture + +``` +┌──────────────────────────────────────────────────────────────────────┐ +│ Robot.java │ +│ ┌────────────────────┐ ┌───────────────────────┐ │ +│ │ robotPeriodic() │ │ simulationPeriodic() │ │ +│ │ - Runs scheduler │ │ - Updates simulation │ │ +│ │ - Every 20ms │ │ - Sim mode only │ │ +│ └─────────┬──────────┘ └──────────┬────────────┘ │ +└────────────┼─────────────────────────────────┼────────────────────────┘ + ↓ ↓ +┌────────────┼──────────────────────────────────┼────────────────────────┐ +│ ↓ ↓ │ +│ ┌─────────────────────┐ ┌──────────────────────┐ │ +│ │ Command Scheduler │ │ SimulationManager │ │ +│ │ - Runs commands │ │ - Reads states │ │ +│ │ - Updates subs │ │ - Updates hardware │ │ +│ └─────────┬───────────┘ │ simulations │ │ +│ ↓ └──────────┬───────────┘ │ +│ ┌─────────────────────┐ │ │ +│ │ TeleopSwerve │ │ │ +│ │ AutoAlign │ │ │ +│ │ DriftTestCommand │ │ │ +│ └─────────┬───────────┘ │ │ +└────────────┼────────────────────────────────────────────────────────────┘ + ↓ ↑ +┌────────────┼──────────────────────────────────┼────────────────────────┐ +│ ↓ │ (reads) │ +│ ┌──────────────────────┐ │ │ +│ │ SwerveSubsystem │────────────────────→│ │ +│ │ - drive() │ getDesiredStates() │ │ +│ │ - driveFromChassis..│ │ │ +│ └──────────┬───────────┘ │ │ +│ ↓ │ │ +│ ┌──────────────────────┐ │ │ +│ │ SwerveModule (×4) │────────────────────→│ │ +│ │ - setDesiredState() │ (exposes state) │ │ +│ │ - setAngle() │ │ │ +│ │ - setSpeed() │ │ │ +│ │ - getState() │←────────────────────┘ │ +│ │ - getDesiredState() │ (updates sensors) │ +│ └──────────┬───────────┘ │ +└─────────────┼──────────────────────────────────────────────────────────┘ + ↓ +┌─────────────┼──────────────────────────────────────────────────────────┐ +│ HARDWARE / SIMULATION │ +│ ┌──────────────────┐ ┌──────────────────┐ ┌──────────────────┐ │ +│ │ Motor Controllers│ │ Encoders │ │ Gyro (Pigeon2) │ │ +│ │ - Real: SparkMax │ │ - Real: Physical │ │ - Real: Physical │ │ +│ │ - Sim: No-op │ │ - Sim: Updated │ │ - Sim: Updated │ │ +│ │ │ │ by SimManager │ │ by SimManager │ │ +│ └──────────────────┘ └──────────────────┘ └──────────────────┘ │ +└────────────────────────────────────────────────────────────────────────┘ +``` + +### 4.2 Normal Driving Data Flow + +``` +Step 1: USER INPUT +┌──────────────────┐ +│ Xbox Controller │ +│ - Left Stick Y │ (forward/back) +│ - Left Stick X │ (strafe) +│ - Right Stick X │ (rotation) +└────────┬─────────┘ + ↓ + +Step 2: COMMAND PROCESSING +┌────────────────────────────────────────────┐ +│ TeleopSwerve.execute() │ +│ - Read joystick values │ +│ - Apply deadband (0.1) │ +│ - Apply slew rate limiter (3.0 m/s²) │ +│ - Scale by max speed (3 m/s) │ +│ Output: xSpeed, ySpeed, rotSpeed │ +└────────┬───────────────────────────────────┘ + ↓ + +Step 3: CONVERT TO ROBOT SPEEDS +┌────────────────────────────────────────────┐ +│ SwerveSubsystem.drive() │ +│ - Field-oriented: transform by gyro │ +│ - Robot-oriented: use as-is │ +│ Output: ChassisSpeeds │ +│ (vx, vy, omega) │ +└────────┬───────────────────────────────────┘ + ↓ + +Step 4: CONVERT TO MODULE SPEEDS +┌────────────────────────────────────────────┐ +│ SwerveSubsystem.driveFromChassisSpeeds() │ +│ - Use kinematics to convert: │ +│ ChassisSpeeds → SwerveModuleState[4] │ +│ - Desaturate wheel speeds │ +│ Output: 4 module states │ +│ [speed, angle] for each module │ +└────────┬───────────────────────────────────┘ + ↓ + +Step 5: OPTIMIZE AND COMMAND MODULES +┌────────────────────────────────────────────┐ +│ SwerveModule.setDesiredState() [×4] │ +│ - Optimize (minimize rotation) │ +│ - Store optimized state ← SIMULATION READS│ +│ - setAngle() → angle controller │ +│ - setSpeed() → drive controller │ +└────────┬───────────────────────────────────┘ + ↓ + +Step 6: HARDWARE EXECUTION +┌────────────────────────────────────────────┐ +│ Motor Controllers │ +│ - Real: Execute PID control │ +│ - Sim: No-op (motors don't move) │ +└────────────────────────────────────────────┘ + +PARALLEL: SIMULATION UPDATE (every 20ms) +┌────────────────────────────────────────────┐ +│ SimulationManager.simulationPeriodic() │ +│ 1. Read: getDesiredStates() from modules │ +│ 2. Calculate: ChassisSpeeds │ +│ 3. Integrate: pose += speeds × dt │ +│ 4. Update: Pigeon2SimState (gyro) │ +│ 5. Update: Encoder positions │ +│ 6. Update: CANcoder positions │ +└────────────────────────────────────────────┘ + ↓ + +Step 7: SENSOR READBACK +┌────────────────────────────────────────────┐ +│ SwerveSubsystem.periodic() │ +│ - Read encoders (real or simulated) │ +│ - Update odometry │ +│ - Update Field2d visualization │ +│ - Publish to NetworkTables │ +└────────────────────────────────────────────┘ +``` + +### 4.3 Test Mode Data Flow + +``` +Step 1: TEST INITIATION +┌────────────────────────────────────────────┐ +│ SmartDashboard │ +│ - User sets: Module #, Angle, Cycles │ +│ - User clicks: "Start Test" │ +└────────┬───────────────────────────────────┘ + ↓ +┌────────────────────────────────────────────┐ +│ SwerveSubsystem.periodic() │ +│ - Calls: SwerveDriftTestManager │ +│ .checkAndStartTest() │ +└────────┬───────────────────────────────────┘ + ↓ +┌────────────────────────────────────────────┐ +│ SwerveDriftTestManager.startTest() │ +│ - Validate parameters │ +│ - Create SwerveAngleDriftTestCommand │ +│ - Schedule command │ +└────────┬───────────────────────────────────┘ + ↓ + +Step 2: TEST EXECUTION (State Machine) +┌────────────────────────────────────────────┐ +│ SwerveAngleDriftTestCommand.initialize() │ +│ - Get test module via getModule() │ +│ - Initialize state: MOVING_TO_TARGET │ +│ - Command: module.setDesiredState() │ +│ → [0 m/s, testAngle°] │ +└────────┬───────────────────────────────────┘ + ↓ (every 20ms) +┌────────────────────────────────────────────┐ +│ SwerveAngleDriftTestCommand.execute() │ +│ │ +│ State: MOVING_TO_TARGET │ +│ - Check: isAtAngle(testAngle) ? │ +│ - Yes → Record encoders │ +│ → State: AT_TARGET │ +│ - No → Wait (timeout if too long) │ +│ │ +│ State: AT_TARGET │ +│ - Hold for minHoldTime (0.5s) │ +│ - Then → State: MOVING_TO_ZERO │ +│ → Command: module.setDesiredState()│ +│ → [0 m/s, 0°] │ +│ │ +│ State: MOVING_TO_ZERO │ +│ - Check: isAtAngle(0) ? │ +│ - Yes → Record encoders │ +│ → State: AT_ZERO │ +│ - No → Wait (timeout if too long) │ +│ │ +│ State: AT_ZERO │ +│ - Hold for minHoldTime (0.5s) │ +│ - Complete cycle, check if more cycles │ +│ - More → Next cycle (back to TARGET) │ +│ - Done → State: COMPLETE │ +└────────┬───────────────────────────────────┘ + ↓ + +Step 3: TEST COMPLETION +┌────────────────────────────────────────────┐ +│ SwerveAngleDriftTestCommand.end() │ +│ - Calculate drift statistics │ +│ - Print results to console │ +│ - Update SmartDashboard │ +└────────────────────────────────────────────┘ + +PARALLEL: SIMULATION (if in sim mode) +┌────────────────────────────────────────────┐ +│ SimulationManager sees: │ +│ - Module 0: desired state from test │ +│ - Module 1-3: zero state (not moving) │ +│ │ +│ Updates: │ +│ - Only Module 0's encoders │ +│ - Robot pose stays same (no chassis move) │ +│ │ +│ Test reads: │ +│ - Simulated relative encoder │ +│ - Simulated absolute encoder │ +│ - Compares to detect drift │ +└────────────────────────────────────────────┘ +``` + +### 4.4 Simulation Update Cycle (Detailed) + +``` +EVERY 20ms IN SIMULATION MODE: + +┌─────────────────────────────────────────────────────────────┐ +│ SimulationManager.simulationPeriodic() │ +└─────────────────────────────────────────────────────────────┘ + │ + │ // 1. Calculate time delta + ├─→ currentTime = Timer.getFPGATimestamp() + ├─→ dt = currentTime - lastTime + ├─→ if (dt invalid) → dt = 0.02 // safety + │ + │ // 2. Read desired states from modules + ├─→ desiredStates[4] = swerveSubsystem.getDesiredStates() + │ ↓ + │ ┌─────────────────────────────────────┐ + │ │ For each module: │ + │ │ state = module.getDesiredState() │ + │ │ (reads the field stored by │ + │ │ setDesiredState() method) │ + │ └─────────────────────────────────────┘ + │ + │ // 3. Calculate overall robot motion + ├─→ ChassisSpeeds = kinematics.toChassisSpeeds(desiredStates) + │ → vx (m/s) - forward/back speed + │ → vy (m/s) - left/right speed + │ → omega (rad/s) - rotation speed + │ + │ // 4. Integrate robot pose (physics) + ├─→ deltaX = vx × dt + ├─→ deltaY = vy × dt + ├─→ deltaRotation = omega × dt + ├─→ simPose = simPose.exp(Twist2d(deltaX, deltaY, deltaRotation)) + │ + │ // 5. Update gyro simulation + ├─→ pigeonSimState.setRawYaw(simPose.getRotation().getDegrees()) + │ + │ // 6. Update each module's encoders + ├─→ For each module i: + │ │ + │ ├─→ // Update drive encoder (distance traveled) + │ │ currentPos = driveEncoder.getPosition() + │ │ deltaMeters = desiredStates[i].speed × dt + │ │ newPos = currentPos + deltaMeters + │ │ driveEncoder.setPosition(newPos) + │ │ + │ ├─→ // Update angle encoder (wheel rotation) + │ │ desiredAngle = desiredStates[i].angle.getDegrees() + │ │ angleEncoder.setPosition(desiredAngle) + │ │ + │ └─→ // Update CANcoder (absolute encoder) + │ posRotations = desiredStates[i].angle.getRotations() + │ cancoderSimState.setRawPosition(posRotations) + │ + │ // 7. Production code now reads updated sensors + ├─→ (Automatically happens in SwerveSubsystem.periodic()) + │ │ + │ ├─→ yaw = pigeon.getYaw() // from simulation + │ ├─→ positions = getPositions() // from simulated encoders + │ ├─→ odometry.update(yaw, positions) + │ └─→ field.setRobotPose(odometry.getEstimatedPosition()) + │ + └─→ (Optional) publishSimulationDiagnostics() + → SmartDashboard for debugging +``` + +### 4.5 Component Dependencies + +``` +┌─────────────────────────────────────────────────────────────┐ +│ PRODUCTION CODE │ +│ (No knowledge of simulation or test infrastructure) │ +└─────────────────────────────────────────────────────────────┘ + │ + ├─→ SwerveModule + │ - Public APIs: + │ • setDesiredState(state, isOpenLoop) ← ALL COMMANDS USE THIS + │ • getState() → current module state + │ • getPosition() → current module position + │ • getCanCoder() → absolute encoder + │ - Simulation accessors (marked with comments): + │ • getDesiredState() → last commanded state + │ • getCanCoderDevice() → hardware object + │ • getDriveEncoder() → hardware object + │ • getAngleEncoder() → hardware object + │ + ├─→ SwerveSubsystem + │ - Public APIs: + │ • drive(x, y, rot, fieldOriented) + │ • driveFromChassisSpeeds(speeds, openLoop) + │ • getModule(number) → specific module + │ • getStates() → all module states + │ • getPositions() → all module positions + │ - Simulation accessors (marked with comments): + │ • getDesiredStates() → commanded states + │ • getField() → Field2d object + │ • getPigeon() → gyro object + │ • getModules() → array of modules + │ • getOdometry() → pose estimator + │ • getKinematics() → kinematics object + │ + └─→ Commands (TeleopSwerve, AutoAlign, etc.) + - Use public APIs only + - No knowledge of simulation + +┌─────────────────────────────────────────────────────────────┐ +│ SIMULATION CODE │ +│ (Reads production code, updates hardware simulations) │ +└─────────────────────────────────────────────────────────────┘ + │ + └─→ SimulationManager + - Called from Robot.simulationPeriodic() + - Reads: getDesiredStates() from subsystem + - Updates: Pigeon2SimState, encoder positions + - No modification of production code + +┌─────────────────────────────────────────────────────────────┐ +│ TEST CODE │ +│ (Uses production APIs, schedules test commands) │ +└─────────────────────────────────────────────────────────────┘ + │ + ├─→ SwerveDriftTestManager + │ - Static utility class + │ - Manages test lifecycle + │ - Integrates with SmartDashboard + │ + ├─→ SwerveAngleDriftTestCommand + │ - WPILib Command + │ - Uses: module.setDesiredState() + │ - Reads: module.getState(), module.getCanCoder() + │ + └─→ SwerveModuleTestUtils + - Helper methods for test code + - Wraps public APIs + - No production code pollution +``` + +### 4.6 Field2d Data Flow + +``` +┌─────────────────────────────────────────────────────────────┐ +│ HOW FIELD2D WORKS (SAME FOR REAL ROBOT AND SIMULATION) │ +└─────────────────────────────────────────────────────────────┘ + +Step 1: SENSORS PROVIDE DATA +┌──────────────────┐ ┌──────────────────┐ +│ Real Robot │ │ Simulation │ +├──────────────────┤ ├──────────────────┤ +│ • Physical gyro │ │ • Simulated gyro │ +│ • Physical │ │ • Simulated │ +│ encoders │ │ encoders │ +└────────┬─────────┘ └────────┬─────────┘ + │ │ + └──────────┬─────────────────┘ + ↓ +Step 2: ODOMETRY CALCULATES POSE +┌────────────────────────────────────────────┐ +│ SwerveSubsystem.periodic() │ +│ (runs every 20ms in all modes) │ +│ │ +│ yaw = pigeon.getYaw() │ +│ → Real mode: reads physical sensor │ +│ → Sim mode: reads simulated sensor │ +│ │ +│ positions = getPositions() │ +│ → Real mode: reads physical encoders │ +│ → Sim mode: reads simulated encoders │ +│ │ +│ odometry.update(yaw, positions) │ +│ → Calculates robot pose on field │ +│ → Returns: Pose2d(x, y, rotation) │ +└────────┬───────────────────────────────────┘ + ↓ +Step 3: FIELD2D DISPLAYS POSE +┌────────────────────────────────────────────┐ +│ field.setRobotPose(odometry.getEstimated │ +│ Position()) │ +│ → Updates Field2d widget │ +│ → Displays in: │ +│ - SmartDashboard │ +│ - Shuffleboard │ +│ - Glass │ +│ - AdvantageScope │ +│ - Simulation GUI │ +└────────────────────────────────────────────┘ + +KEY INSIGHT: The SAME code path works for both real and sim! +- Real mode: sensors are physical → odometry is accurate → Field2d shows real pose +- Sim mode: sensors are simulated → odometry uses sim data → Field2d shows sim pose + +NO SEPARATE SIMULATION LOGIC NEEDED FOR FIELD2D! +``` + +### 4.7 Why Current Design is Correct + +``` +┌─────────────────────────────────────────────────────────────┐ +│ QUESTION: Should we capture at setAngle/setSpeed level? │ +└─────────────────────────────────────────────────────────────┘ + +OPTION A: Current Design (Capture at setDesiredState) +┌────────────────────────────────────────────┐ +│ module.setDesiredState(state, openLoop) │ +│ ↓ │ +│ optimizedState = optimize(state) │ +│ this.desiredState = optimizedState ← STORE HERE +│ ↓ │ +│ setAngle(optimizedState) │ +│ setSpeed(optimizedState, openLoop) │ +└────────────────────────────────────────────┘ + ↓ +✅ PROS: + • Single source of truth + • Captures optimized state (correct intent) + • Simple to read and maintain + • Works for all use cases + +❌ CONS: + • None identified + + +OPTION B: Alternative (Capture at setAngle/setSpeed) +┌────────────────────────────────────────────┐ +│ module.setDesiredState(state, openLoop) │ +│ ↓ │ +│ optimizedState = optimize(state) │ +│ ↓ │ +│ setAngle(optimizedState) │ +│ ↓ │ +│ capturedAngle = ... ← STORE HERE │ +│ ↓ │ +│ setSpeed(optimizedState, openLoop) │ +│ ↓ │ +│ capturedSpeed = ... ← STORE HERE │ +└────────────────────────────────────────────┘ + ↓ +❌ CONS: + • Two separate pieces of data to track + • Need to recombine into SwerveModuleState + • More complex synchronization + • Risk of desync if one is set without the other + • More code to maintain + +✅ PROS: + • None that outweigh the cons + + +VERDICT: Current design (Option A) is correct! ✅ +``` + +### 4.8 Test Mode State Machine + +``` +SwerveAngleDriftTestCommand State Machine +(Example: testAngle=90°, cycles=3) + + START + ↓ +┌───────────────────────┐ +│ INITIALIZE │ +│ - Get test module │ +│ - cycle = 0 │ +│ - Command: 90° │ +└───────┬───────────────┘ + ↓ + ╔════════════════╗ + ║ CYCLE 0 ║ + ╚════════════════╝ + ↓ +┌───────────────────────┐ +│ MOVING_TO_TARGET │ +│ - Wait for 90° │ +│ - Check every 20ms │ +│ - Timeout: 1.0s │ +└───────┬───────────────┘ + ↓ (reached 90° ± 2°) +┌───────────────────────┐ +│ AT_TARGET │ +│ - Record encoders │ +│ • Relative: 90.1° │ +│ • Absolute: 90.0° │ +│ • Drift: 0.1° │ +│ - Hold: 0.5s │ +└───────┬───────────────┘ + ↓ (hold complete) +┌───────────────────────┐ +│ MOVING_TO_ZERO │ +│ - Command: 0° │ +│ - Wait for 0° │ +│ - Check every 20ms │ +│ - Timeout: 1.0s │ +└───────┬───────────────┘ + ↓ (reached 0° ± 2°) +┌───────────────────────┐ +│ AT_ZERO │ +│ - Record encoders │ +│ • Relative: 0.2° │ +│ • Absolute: 0.0° │ +│ • Drift: 0.2° │ +│ - Hold: 0.5s │ +│ - cycle++ │ +└───────┬───────────────┘ + ↓ + cycle < 3? + ├─ Yes ─→ Back to MOVING_TO_TARGET (next cycle) + │ + └─ No + ↓ + ╔════════════════╗ + ║ ALL CYCLES ║ + ║ COMPLETE ║ + ╚════════════════╝ + ↓ +┌───────────────────────┐ +│ COMPLETE │ +│ - Calculate stats │ +│ - Print results │ +│ - Update dashboard │ +└───────────────────────┘ + ↓ + END +``` + +### 4.9 Key Architecture Principles + +``` +┌─────────────────────────────────────────────────────────────┐ +│ PRINCIPLE 1: SINGLE ENTRY POINT │ +└─────────────────────────────────────────────────────────────┘ + + ALL control commands flow through ONE method: + + ┌──────────────────────────┐ + │ setDesiredState(state) │ ← SINGLE ENTRY POINT + └────────────┬─────────────┘ + ↓ + ┌────────────────────────────┐ + │ Stores: this.desiredState │ + └────────────┬───────────────┘ + ↓ + ┌────────────────────────────┐ + │ Calls: setAngle(), setSpeed│ + └────────────────────────────┘ + + ✅ Makes simulation work automatically + ✅ Ensures consistent behavior + ✅ Single source of truth + + +┌─────────────────────────────────────────────────────────────┐ +│ PRINCIPLE 2: SEPARATION OF CONCERNS │ +└─────────────────────────────────────────────────────────────┘ + + Production Code Simulation Code Test Code + │ │ │ + │ No simulation │ No production │ No simulation + │ knowledge │ code changes │ knowledge + │ │ │ + ├─ SwerveModule │ │ + ├─ SwerveSubsystem ←───┼─ Reads states │ + ├─ Commands │ via getters │ + │ │ │ + │ ├─ SimulationMgr │ + │ │ Updates sims │ + │ │ │ + └─────────────────────────┴────────────────────┴─ TestManager + ├─ TestCommand + └─ TestUtils + + +┌─────────────────────────────────────────────────────────────┐ +│ PRINCIPLE 3: NO CONDITIONALS IN PRODUCTION CODE │ +└─────────────────────────────────────────────────────────────┘ + + ❌ DON'T DO THIS: + + public void setSpeed(...) { + if (RobotBase.isSimulation()) { + // simulation-specific logic + } + // production logic + } + + ✅ DO THIS INSTEAD: + + // SwerveModule.java (production) + public void setSpeed(...) { + // Only production logic + controller.setReference(...); + } + + // SimulationManager.java (simulation) + public void updateSimulation() { + // Only simulation logic + encoder.setPosition(...); + } +``` + +--- + +## 5. Control Flow Analysis + +### 5.1 Normal Driving Control Flow + +The control flow for normal robot driving is correctly implemented: + +``` +User Controller Input + ↓ +RobotContainer (lines 98-112) + - Reads joystick axes + - Applies speed multipliers + ↓ +TeleopSwerve Command (lines 67-73) + - Applies deadband + - Applies slew rate limiting + - Scales by max speed/angular velocity + ↓ +SwerveSubsystem.drive() (lines 113-123) + - Converts to ChassisSpeeds (field or robot relative) + - Calls driveFromChassisSpeeds() + ↓ +SwerveSubsystem.driveFromChassisSpeeds() (lines 125-134) + - Converts ChassisSpeeds → SwerveModuleStates (via kinematics) + - Desaturates wheel speeds + - Calls setDesiredState() on each module + ↓ +SwerveModule.setDesiredState() (lines 149-158) + - Optimizes state (minimize rotation) + - Stores desired state for simulation + - Calls setAngle() and setSpeed() + ↓ +SwerveModule.setAngle() (lines 323-332) +SwerveModule.setSpeed() (lines 292-309) + - Sets motor controller references + - Hardware executes the commands +``` + +### 5.2 Test Mode Control Flow + +The control flow for individual module testing: + +``` +SmartDashboard/Test Manager + ↓ +SwerveDriftTestManager.startTest() + - Validates parameters + - Schedules SwerveAngleDriftTestCommand + ↓ +SwerveAngleDriftTestCommand + - Gets specific module via SwerveSubsystem.getModule() + - Calls module.setDesiredState() directly + - Monitors progress via SwerveModuleTestUtils + ↓ +SwerveModule.setDesiredState() + - Same path as normal driving + - Optimizes, stores state, calls setAngle/setSpeed +``` + +**Key Insight:** Both normal driving and test mode use the same `setDesiredState()` entry point, ensuring consistent behavior. + +--- + +## 6. Implementation Assessment + +### 6.1 What Works Well ✅ + +#### A. Simulation Manager Design +**File:** `SimulationManager.java` + +**Strengths:** +- ✅ **Externalized:** Simulation logic is completely separate from production code +- ✅ **Self-contained:** Manages all simulation state internally +- ✅ **Correct integration:** Called from `Robot.simulationPeriodic()` +- ✅ **Handles both use cases:** Works for normal driving AND individual module testing + +**Key Design Decision:** The simulation reads `getDesiredStates()` from modules, which captures the optimized states after `setDesiredState()` is called. This is correct because: +1. The optimized state represents what the module is actually trying to achieve +2. It works for both coordinated driving (all modules) and individual module commands +3. Motors don't actually move in simulation, so we simulate based on commands + +#### B. Test Architecture +**Files:** `SwerveAngleDriftTestCommand.java`, `SwerveDriftTestManager.java`, `SwerveModuleTestUtils.java` + +**Strengths:** +- ✅ **Separation of concerns:** Test code is in separate package (`frc.robot.test`) +- ✅ **Minimal intrusion:** Uses existing public APIs (`setDesiredState`, `getState`, `getCanCoder`) +- ✅ **Manager pattern:** `SwerveDriftTestManager` handles test lifecycle +- ✅ **Utility class:** `SwerveModuleTestUtils` provides test helpers without polluting production code +- ✅ **Command framework integration:** Uses WPILib Command pattern correctly + +#### C. Production Code Cleanliness +**Files:** `SwerveModule.java`, `SwerveSubsystem.java` + +**Strengths:** +- ✅ **Clear separation:** Simulation support methods are clearly marked with comments +- ✅ **Minimal intrusion:** Only 4 accessor methods added for simulation +- ✅ **No conditional logic:** No `if (simulation)` checks in production code +- ✅ **Public API is clean:** Test code uses existing public methods where possible + +### 6.2 Minor Issues and Improvements Needed ⚠️ + +#### Issue 1: `pointInDirection()` Not Integrated +**Status:** ✅ **Already Fixed** - Now uses `setDesiredState()` internally. + +#### Issue 2: `resetToAbsolute()` Called Outside Normal Flow +**Location:** `SwerveModule.java:412-416` + +**Analysis:** This is actually **correct** because: +- It's a calibration/initialization operation, not a control command +- Simulation handles encoder updates separately in `updateModuleEncoders()` +- The CANcoder simulation is updated based on desired angle, so this read-modify-write works correctly + +**Recommendation:** No change needed, but document this in simulation comments + +### 6.3 Current Capture Point Analysis + +**Current Design:** The simulation captures desired states at the `SwerveModule.setDesiredState()` level (stored in `desiredState` field). + +**Question:** Should we capture at the lower level (`setAngle` and `setSpeed` methods)? + +**Answer:** **No, the current design is correct.** Here's why: + +| Aspect | Current Design (setDesiredState) | Alternative (setAngle/setSpeed) | +|--------|----------------------------------|----------------------------------| +| **Optimization** | Captures optimized state (after minimize rotation) | Would need to capture separately and recombine | +| **Consistency** | Single source of truth | Two separate pieces of data | +| **Test compatibility** | Works perfectly for individual module tests | Would work but more complex | +| **Implementation** | Simple, clean | More code, more maintenance | +| **Correctness** | Represents actual module intent | Would need to handle desync issues | + +**Recommendation:** Keep capturing at `setDesiredState()` level. This is the right architectural choice. + +--- + +## 7. Design Principles + +The current implementation follows excellent design principles that should be maintained: + +### 7.1 Separation of Concerns + +``` +┌─────────────────────────────────────────────────────────────┐ +│ PRODUCTION CODE │ +│ (SwerveModule, SwerveSubsystem, Commands) │ +│ - No simulation-specific logic │ +│ - No test-specific logic │ +│ - Only accessor methods for external systems │ +└─────────────────────────────────────────────────────────────┘ + ↓ ↑ + ┌──────────────────┴──┴──────────────────┐ + ↓ ↓ +┌───────────────────┐ ┌──────────────────────┐ +│ SIMULATION │ │ TEST CODE │ +│ (SimulationMgr) │ │ (test package) │ +│ - Reads states │ │ - Test commands │ +│ - Updates sims │ │ - Test managers │ +│ - Updates Field2d│ │ - Test utilities │ +└───────────────────┘ └──────────────────────┘ +``` + +### 7.2 Data Flow for Simulation + +``` +[User Input] → [Commands] → [Subsystem] → [Modules] + ↓ + setDesiredState() + ↓ + stores optimized state + ↓ + setAngle/setSpeed + ↓ + Motor controllers + (no-op in sim) + + +[SimulationManager.simulationPeriodic()] + ↓ + Reads: getDesiredStates() from all modules + ↓ + Calculates: ChassisSpeeds from desired states + ↓ + Integrates: simPose += speed * deltaTime + ↓ + Updates: Pigeon2 simulation (yaw) + Updates: Module encoders (drive position, angle position) + Updates: CANcoder simulation + ↓ + SwerveSubsystem.periodic() reads simulated sensors (gyro, encoders) + ↓ + SwerveSubsystem.periodic() updates: Odometry (line 227), Field2d (line 230) +``` + +### 7.3 Single Entry Point Principle + +**All module control commands flow through `setDesiredState()`** +- Normal driving: ✅ Yes (via `driveFromChassisSpeeds`) +- Test commands: ✅ Yes (directly call `setDesiredState`) +- Future features: ⚠️ Must follow this pattern (e.g., `pointInDirection` now fixed) + +--- + +## 8. Known Issues and Future Improvements + +### 8.1 Issue: Angle Accumulation in Custom `optimize()` Method + +**Location:** `SwerveModule.java:252-275` + +**Problem:** The custom `optimize()` method can cause angle accumulation in simulation, where angles grow beyond 360° (e.g., 360°, 450°, 720°, etc.) instead of staying in the 0-360° range. + +**Root Cause:** +```java +// Line 273 in optimize() +double direction = currentAngle.getDegrees() + turnAmount; +return new SwerveModuleState (speed, Rotation2d.fromDegrees(direction)); +``` + +This adds `turnAmount` to `currentAngle` without normalizing, which can produce angles like: +- Cycle 1: 0° → 90° ✓ +- Cycle 2: 90° → 0° (but optimizes to 360°) +- Cycle 3: 360° → 450° ❌ +- Cycle 4: 450° → 720° ❌ + +**Symptoms:** +- In simulation/test mode, `DriftTest/Angle/Current` may show values like 360-450° instead of 0-90° +- Occurs intermittently depending on optimization path taken +- Does not affect real robot (encoders eventually wrap), but makes simulation/testing confusing + +**Recommended Fix:** Replace custom `optimize()` with WPILib's built-in method: + +```java +// Current custom implementation (252-275) +private SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle){ + // 25 lines of custom optimization logic... +} + +// Recommended: Use WPILib's built-in method +private SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle){ + return SwerveModuleState.optimize(desiredState, currentAngle); +} + +// Or better yet, call it directly in setDesiredState() and remove this method entirely: +public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { + SwerveModuleState optimizedState = SwerveModuleState.optimize(desiredState, getAngle()); + this.desiredState = optimizedState; + setAngle(optimizedState); + setSpeed(optimizedState, isOpenLoop); +} +``` + +**Why WPILib's Method Fixes It:** +- `Rotation2d` objects automatically normalize angles when constructed +- `Rotation2d.fromDegrees(450.0).getDegrees()` returns `90.0` (normalized) +- WPILib's implementation is well-tested and used by thousands of FRC teams +- Reduces custom code maintenance burden + +**Benefits:** +1. ✅ Fixes angle accumulation bug in simulation/test +2. ✅ Removes 25 lines of custom code +3. ✅ Uses standard, well-tested FRC approach +4. ✅ Benefits from future WPILib improvements + +**Priority:** Medium - Affects simulation/test clarity but not real robot operation + +**Note:** This fix should be implemented in a separate PR focused on production code changes, not in the simulation/test support branch. + +--- + +## 9. Understanding Hardware Simulation + +**How do hardware objects know they're in simulation mode?** + +This is a common question about WPILib simulation. The answer: **vendor libraries have built-in simulation support**. + +### 9.1 The Magic Behind the Scenes + +When you call methods on hardware objects like `Pigeon2.getYaw()` or `RelativeEncoder.getPosition()`, the vendor library code internally checks `RobotBase.isSimulation()` to determine what to do: + +**Simplified example (conceptual, not actual library code):** + +```java +// Inside the Pigeon2 class (CTRE Phoenix 6 library) +public class Pigeon2 { + private Pigeon2SimState simState = new Pigeon2SimState(); + + // GETTER: Read sensor value + public StatusSignal getYaw() { + if (RobotBase.isSimulation()) { + // Simulation mode: return value from simulation state + return simState.getSimulatedYaw(); + } else { + // Real robot mode: read from actual hardware via CAN bus + return readYawFromHardware(); + } + } + + // SETTER: Set/calibrate sensor + public StatusCode setYaw(double angle) { + if (RobotBase.isSimulation()) { + // Simulation mode: update simulation state + simState.setSimulatedYaw(angle); + return StatusCode.OK; + } else { + // Real robot mode: send command to hardware via CAN bus + return sendYawCommandToHardware(angle); + } + } + + // Simulation code gets access to sim state for physics updates + public Pigeon2SimState getSimState() { + return simState; + } +} +``` + +**Key insight:** Both getters AND setters check simulation mode internally! + +### 9.2 How This Works in Your Code + +**Step 1: SimulationManager gets simulation state objects** +```java +// In SimulationManager constructor +pigeonSimState = swerveSubsystem.getPigeon().getSimState(); +``` + +**Step 2: SimulationManager updates simulation state (physics updates)** +```java +// In simulationPeriodic() +// This simulates the gyro changing due to robot rotation +pigeonSimState.setRawYaw(simPose.getRotation().getDegrees()); +``` + +**Step 3: Production code reads hardware object (same code for real and sim)** +```java +// In SwerveSubsystem.periodic() - works in both modes! +Rotation2d yaw = pigeon.getYaw(); // Internally routes to sim state in sim mode +``` + +**Step 4: Production code can also set/calibrate hardware (same code for real and sim)** +```java +// In SwerveSubsystem.zeroGyro() - works in both modes! +pigeon.setYaw(0); // Real: commands hardware; Sim: updates sim state internally +``` + +### 9.3 Two Types of Simulation Updates + +There are **two ways** simulation state gets updated: + +1. **Physics updates (SimulationManager does this):** + - Encoders advance based on velocity × time + - Gyro rotates based on robot rotation + - Use `.getSimState()` to get the sim state object and update it + +2. **Calibration/reset commands (Hardware object does this internally):** + - `pigeon.setYaw(0)` - Reset gyro to zero + - `encoder.setPosition(0)` - Reset encoder position + - `module.resetToAbsolute()` - Calibrate to absolute encoder + - These methods internally check `RobotBase.isSimulation()` and update sim state + +**Why the difference?** Physics updates are continuous and based on calculations. Calibration commands are discrete user actions that should work the same way in both modes. + +### 9.4 Different Vendor Implementations + +**CTRE devices (Pigeon2, CANcoder):** +- Provide separate `SimState` objects (`Pigeon2SimState`, `CANcoderSimState`) +- You update the sim state object using methods like `setRawYaw()`, `setRawPosition()` +- Real hardware object automatically reads from sim state in simulation mode +- **Design philosophy**: Separate APIs for calibration (`setYaw()`) vs simulation (`setRawYaw()`) + +**REV devices (SparkMax, SparkFlex encoders):** +- No separate sim state object +- You call `encoder.setPosition()` directly for both calibration and simulation +- The encoder object internally maintains simulation state +- When you read in sim mode, it returns the simulated value +- **Design philosophy**: Same API for calibration and simulation + +**Note:** You could technically use `pigeon.setYaw()` instead of `pigeonSimState.setRawYaw()` in simulation code, as `setYaw()` internally checks simulation mode. However, using the `SimState` API is preferred because: +1. It follows CTRE's intended API pattern +2. It makes semantic intent clearer (physics updates vs calibration) +3. It's more explicit about simulation-specific code +4. It's more robust if vendor implementations change + +### 9.5 Why This Design is Elegant + +✅ **Production code is mode-agnostic** - Same code works for real robot and simulation +✅ **No conditionals needed** - Your code doesn't need `if (RobotBase.isSimulation())` checks +✅ **Simulation logic is isolated** - All sim updates happen in `SimulationManager` +✅ **Type-safe** - Hardware objects have the same type in both modes + +### 9.6 Key Takeaway + +**You never need to check `RobotBase.isSimulation()` in production code!** The vendor libraries handle the mode detection internally: + +- **All getter methods** (reading sensors) automatically return simulated values in sim mode +- **All setter methods** (commanding/calibrating hardware) automatically update sim state in sim mode +- **SimulationManager** only needs to update physics-related changes (motion, velocity, etc.) +- **Production code** (like `zeroGyro()`, `resetToAbsolute()`) works identically in both modes + +This is why your architecture works so well - the production code is truly identical for real and simulated robots, and the vendor libraries provide all the mode-switching logic internally. + +--- + +## 10. Implementation Guidelines + +### 10.1 Adding New Control Paths + +**Rule:** All control commands must flow through `SwerveModule.setDesiredState()` + +**Example: Adding a new "lock wheels" command** + +```java +// ✅ CORRECT +public void lockWheels() { + // Create states that form an X pattern + SwerveModuleState[] lockStates = new SwerveModuleState[] { + new SwerveModuleState(0, Rotation2d.fromDegrees(45)), // Front-left + new SwerveModuleState(0, Rotation2d.fromDegrees(-45)), // Front-right + new SwerveModuleState(0, Rotation2d.fromDegrees(-45)), // Back-left + new SwerveModuleState(0, Rotation2d.fromDegrees(45)) // Back-right + }; + + for (SwerveModule mod : mSwerveMods) { + mod.setDesiredState(lockStates[mod.moduleNumber], false); + } +} + +// ❌ WRONG +public void lockWheels() { + // Don't bypass setDesiredState! + for (SwerveModule mod : mSwerveMods) { + mod.pointInDirection(45); // Bypasses desired state storage + } +} +``` + +### 10.2 Adding New Test Commands + +**Pattern to follow:** + +1. Create test command class in `frc.robot.test` package +2. Use `SwerveModuleTestUtils` for helper methods +3. Access modules via `SwerveSubsystem.getModule()` +4. Send commands via `module.setDesiredState()` +5. Read state via public accessor methods +6. Create manager class for test lifecycle + +**Example: Adding a new "drive motor velocity test"** + +```java +// In frc/robot/test/SwerveVelocityTestCommand.java +public class SwerveVelocityTestCommand extends Command { + private final SwerveModule testModule; + private final double targetVelocity; + + @Override + public void execute() { + // Send command via setDesiredState + testModule.setDesiredState( + new SwerveModuleState(targetVelocity, Rotation2d.fromDegrees(0)), + false // closed loop + ); + + // Read actual velocity via public API + double actualVelocity = testModule.getState().speedMetersPerSecond; + + // Record for analysis + recordVelocityMeasurement(actualVelocity); + } +} +``` + +### 10.3 Modifying Simulation Behavior + +**Guideline:** Keep simulation logic in `SimulationManager`, don't modify production code. + +**Example: Adding wheel slip simulation** + +```java +// ✅ CORRECT: Add to SimulationManager +private void updateModuleEncoders(SwerveModuleState[] desiredStates, double dt) { + for (int i = 0; i < modules.length; i++) { + SwerveModuleState desiredState = desiredStates[i]; + + // Add slip simulation (only in SimulationManager) + double slipFactor = calculateSlipFactor(desiredState.speedMetersPerSecond); + double actualSpeed = desiredState.speedMetersPerSecond * slipFactor; + + // Update encoder with slipped speed + double deltaMeters = actualSpeed * dt; + driveEncoder.setPosition(currentPosition + deltaMeters); + + // ... rest of simulation ... + } +} + +// ❌ WRONG: Don't add to SwerveModule +public void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { + if (RobotBase.isSimulation()) { + // Don't add simulation-specific logic here! + } + // ... production code ... +} +``` + +### 10.4 Field2d Usage + +**Current Design:** The `Field2d` object is used for both real robot visualization and simulation visualization. + +**How it works:** + +```java +// In SwerveSubsystem constructor +field = new Field2d(); +SmartDashboard.putData("Field", field); + +// In SwerveSubsystem.periodic() (runs on real robot) +field.setRobotPose(getPose()); + +// In SimulationManager (runs only in simulation) +// The Field2d is updated via the normal odometry flow: +// 1. SimulationManager updates encoder simulations +// 2. SwerveSubsystem.periodic() runs odometry.update() +// 3. SwerveSubsystem.periodic() updates field.setRobotPose() +``` + +**Key insight:** You don't need separate simulation logic for Field2d! The existing `SwerveSubsystem.periodic()` code handles it correctly because it reads from odometry, which in turn reads from encoders (simulated in sim mode, real in real mode). + +**Verification:** +- ✅ Real robot: Encoders are real → Odometry is real → Field2d shows real position +- ✅ Simulation: Encoders are simulated → Odometry uses sim data → Field2d shows sim position + +### 10.5 Common Questions Answered + +**Q1: Should we capture at `setAngle`/`setSpeed` level instead of `setDesiredState`?** +**A:** No, current design is correct. `setDesiredState()` is the right level because: +- It captures the optimized state (after minimize rotation) +- Single source of truth +- Works for all use cases (driving, testing) +- Simpler implementation + +**Q2: Do we need separate logic for Field2d in simulation?** +**A:** No, current design is correct. The same code works for both: +- Real robot: Real encoders → Real odometry → Field2d +- Simulation: Sim encoders → Sim odometry → Field2d +- No separate logic needed! + +**Q3: Does `resetToAbsolute()` need special handling?** +**A:** No, current design is correct. It's a calibration operation, not a control command. Simulation handles it correctly. + +**Q4: Can test commands work with simulation?** +**A:** Yes! Current design works perfectly: +- Test command calls `setDesiredState()` on one module +- Simulation sees that module's desired state +- Other modules have zero state (not moving) +- Simulation updates only the test module +- Test reads simulated encoders +- Everything works! + +--- + +## 11. Appendices + +### 11.1 File Organization + +``` +src/main/java/frc/robot/ +├── Robot.java [Lifecycle, integrates SimulationManager] +├── RobotContainer.java [Subsystem creation, command binding] +├── Constants.java [Configuration constants] +├── SwerveModule.java [✅ Production code with minimal sim accessors] +│ +├── Subsystems/ +│ └── SwerveSubsystem.java [✅ Production code with minimal sim accessors] +│ +├── Command/ +│ └── TeleopSwerve.java [✅ Production command] +│ +├── sim/ +│ └── SimulationManager.java [✅ All simulation logic] +│ +└── test/ + ├── SwerveDriftTestManager.java [✅ Test lifecycle management] + ├── SwerveAngleDriftTestCommand.java [✅ Test command] + └── SwerveModuleTestUtils.java [✅ Test utilities] +``` + +### 11.2 Code Review Checklist + +Use this checklist when adding new features: + +**For new control features:** +- [ ] All module commands flow through `setDesiredState()` +- [ ] No direct calls to `setAngle()` or `setSpeed()` from outside SwerveModule +- [ ] No `if (simulation)` conditional logic in production code + +**For new test commands:** +- [ ] Test code is in `frc.robot.test` package +- [ ] Uses existing public APIs where possible +- [ ] New accessors are clearly marked as "Simulation Support" or "Test Support" +- [ ] Test uses Command framework lifecycle +- [ ] Test integrates with SmartDashboard for control + +**For simulation changes:** +- [ ] Changes are in `SimulationManager`, not production code +- [ ] Simulation reads desired states via `getDesiredStates()` +- [ ] Simulation updates hardware simulation objects (Pigeon2SimState, etc.) +- [ ] Production code remains unchanged + +### 11.3 Testing Checklist + +After implementing changes, verify: + +**Simulation Testing:** +- [ ] Start robot in simulation mode +- [ ] Drive robot with controller - verify Field2d updates +- [ ] Check SmartDashboard for "Sim/" entries (if you added diagnostics) +- [ ] Run drift test from SmartDashboard +- [ ] Verify module angles update in simulation +- [ ] Verify test completes and prints results + +**Real Robot Testing:** +- [ ] Deploy to real robot (or verify compilation) +- [ ] Verify normal driving works +- [ ] Verify test commands work (if deploying tests to robot) +- [ ] Verify no simulation code runs on real robot + +**Code Quality:** +- [ ] No linter errors introduced +- [ ] All comments are clear and accurate +- [ ] No unused imports +- [ ] Javadoc is complete + +### 11.4 Priority Action Items + +**Must Do (Critical):** +1. ✅ **Review this document** - Understand the architecture +2. ✅ **`pointInDirection()` method** - Already fixed to use `setDesiredState()` + +**Should Do (Important):** +3. 📝 **Improve documentation** - Add clarifying comments (see Section 3.2) +4. 📊 **Add diagnostics** - Add SmartDashboard output for debugging (see Section 3.3) + +**Future Improvements (Separate PR):** +5. 🔧 **Replace custom `optimize()`** - Use WPILib's built-in method (fixes angle accumulation in sim) - See Section 8.1 + +**Nice to Have (Optional):** +6. 📋 **Add architecture comments** - Enhance section comments (see Sections 3.4-3.5) +7. 🔧 **Add TestManagerBase** - Create base class for future test managers (see Optional Enhancements) + +### 11.5 Optional Enhancements + +These are nice-to-have improvements but not critical: + +#### Optional 1: Add TestManagerBase Pattern + +**Location:** Create new file `src/main/java/frc/robot/test/TestManagerBase.java` + +```java +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.test; + +/** + * Base class for test managers. + * Provides a consistent pattern for test infrastructure across different test types. + * + *

Test managers follow these design principles: + *

    + *
  • Test managers are static utility classes (no instances)
    • + *
  • Test managers use existing public APIs where possible
    • + *
  • Test managers integrate with SmartDashboard for control
    • + *
  • Test managers use Command framework for test execution
    • + *
  • Test code is kept separate from production code
    • + *
+ * + *

Example usage: + * 

+ * public class MyTestManager extends TestManagerBase {
+ *     public static void initializeDashboard() {
+ *         SmartDashboard.putNumber("MyTest/Parameter", 0.0);
+ *         SmartDashboard.putBoolean("MyTest/Start", false);
+ *     }
+ *     
+ *     public static void checkAndStartTest(MySubsystem subsystem) {
+ *         boolean start = SmartDashboard.getBoolean("MyTest/Start", false);
+ *         if (start) {
+ *             SmartDashboard.putBoolean("MyTest/Start", false);
+ *             // Schedule test command...
+ *         }
+ *     }
+ * }
+ *
+ */ +public abstract class TestManagerBase { + + /** + * Initializes SmartDashboard controls for this test. + * Should be called once during robot initialization. + * Subclasses must implement this method. + */ + protected static void initializeDashboard() { + throw new UnsupportedOperationException("Subclass must implement initializeDashboard()"); + } + + /** + * Checks SmartDashboard for test trigger and starts test if requested. + * Should be called periodically (e.g., from subsystem.periodic()). + * Subclasses must implement this method. + */ + protected static void checkAndStartTest() { + throw new UnsupportedOperationException("Subclass must implement checkAndStartTest()"); + } +} +``` + +#### Optional 2: Add Simulation Recording + +This is a more advanced feature that you might want later: + +**Location:** Create new file `src/main/java/frc/robot/sim/SimulationRecorder.java` + +```java +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.sim; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import java.util.ArrayList; +import java.util.List; +import java.io.FileWriter; +import java.io.IOException; + +/** + * Records simulation state history for playback and analysis. + * Useful for debugging simulation behavior and generating test data. + */ +public class SimulationRecorder { + + /** + * A single frame of simulation data. + */ + public static record SimulationFrame( + double timestamp, + Pose2d pose, + ChassisSpeeds speeds, + double[] moduleAngles, + double[] moduleSpeeds + ) {} + + private final List frames = new ArrayList<>(); + private boolean recording = false; + + /** + * Starts recording simulation frames. + */ + public void startRecording() { + recording = true; + frames.clear(); + } + + /** + * Stops recording simulation frames. + */ + public void stopRecording() { + recording = false; + } + + /** + * Records a single simulation frame. + */ + public void recordFrame(Pose2d pose, ChassisSpeeds speeds, + double[] moduleAngles, double[] moduleSpeeds) { + if (!recording) { + return; + } + + double timestamp = Timer.getFPGATimestamp(); + frames.add(new SimulationFrame(timestamp, pose, speeds, moduleAngles, moduleSpeeds)); + } + + /** + * Exports recorded frames to CSV file for analysis. + * @param filename The output filename (e.g., "simulation_log.csv") + */ + public void exportToCSV(String filename) { + try (FileWriter writer = new FileWriter(filename)) { + // Write header + writer.write("timestamp,pose_x,pose_y,pose_rotation,vx,vy,omega," + + "mod0_angle,mod1_angle,mod2_angle,mod3_angle," + + "mod0_speed,mod1_speed,mod2_speed,mod3_speed\n"); + + // Write data + for (SimulationFrame frame : frames) { + writer.write(String.format("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f," + + "%.3f,%.3f,%.3f,%.3f," + + "%.3f,%.3f,%.3f,%.3f\n", + frame.timestamp, + frame.pose.getX(), frame.pose.getY(), frame.pose.getRotation().getDegrees(), + frame.speeds.vxMetersPerSecond, frame.speeds.vyMetersPerSecond, + frame.speeds.omegaRadiansPerSecond, + frame.moduleAngles[0], frame.moduleAngles[1], + frame.moduleAngles[2], frame.moduleAngles[3], + frame.moduleSpeeds[0], frame.moduleSpeeds[1], + frame.moduleSpeeds[2], frame.moduleSpeeds[3] + )); + } + + System.out.println("Exported " + frames.size() + " frames to " + filename); + } catch (IOException e) { + System.err.println("Failed to export simulation recording: " + e.getMessage()); + } + } + + /** + * Gets the number of recorded frames. + */ + public int getFrameCount() { + return frames.size(); + } +} +``` + +### 11.6 Further Reading + +**WPILib Documentation:** + +The official WPILib documentation is available at: https://docs.wpilib.org/ + +Key sections relevant to this architecture: +- **Command-based Programming** - Search for "Command-Based Programming" in the docs +- **Robot Simulation** - Search for "Robot Simulation" in the docs +- **Swerve Drive Kinematics** - Search for "Swerve Drive Kinematics" in the docs +- **Unit Testing** - Search for "Unit Testing" in the docs + +**Note:** WPILib documentation URLs change periodically. If specific links are needed, navigate from the main documentation site using the search function or table of contents. + +--- + +## Summary + +**Your current implementation is excellent.** The architecture is sound, the separation of concerns is clean, and the design choices are correct. The only changes needed are: +1. ✅ `pointInDirection()` method - Already fixed +2. Add a few clarifying comments (see Recommended Code Changes) +3. Optionally add diagnostics for debugging + +**No major refactoring needed!** 🎉 + +--- + +**Document prepared by:** AI Assistant (Claude Sonnet 4.5) +**Last Updated:** February 15, 2026 diff --git a/docs/unit-tests-with-sim.md b/docs/unit-tests-with-sim.md new file mode 100644 index 0000000..23fb0ae --- /dev/null +++ b/docs/unit-tests-with-sim.md @@ -0,0 +1,479 @@ +# Unit Testing with WPILib HAL Simulation + +## Overview + +WPILib's HAL (Hardware Abstraction Layer) provides a simulation backend that replaces physical hardware with in-memory state. When robot code runs on a desktop JVM instead of a roboRIO, every call to a HAL function (reading an encoder, commanding a motor, checking the DriverStation) goes through this simulation layer. + +This same mechanism works inside JUnit tests. By calling `HAL.initialize(500, 0)` before the test code runs, the simulation backend is activated, and all WPILib hardware objects operate against simulated state. No GUI, no network socket, and no physical hardware are involved — the tests run headless and deterministically. + +### What this enables + +- **Subsystem logic tests** — verify that a subsystem responds correctly to sensor inputs you control. +- **Command lifecycle tests** — exercise `initialize()` / `execute()` / `isFinished()` / `end()` without a running `TimedRobot` loop. +- **DriverStation state tests** — programmatically set enabled/disabled/autonomous/teleop mode and verify that robot code reacts accordingly. +- **CI/CD integration** — all of the above runs in a headless environment on any OS (Linux, macOS, Windows) via `./gradlew build`. + +### What this does NOT cover + +- Full closed-loop simulation with physics models (e.g. `DCMotorSim`, `SwerveDriveSimulation`). Those run in the `simulationPeriodic()` loop of a running robot program, not in JUnit tests. +- Vendor-specific hardware simulation (CTRE Phoenix, REV). Vendor sim classes exist but are separate from the WPILib HAL sim discussed here. + +--- + +## Prerequisites — `build.gradle` Configuration + +The project's `build.gradle` already contains everything needed to run HAL-sim-based tests. The relevant lines are: + +```gradle +// Desktop support must be enabled +def includeDesktopSupport = true + +dependencies { + // ...desktop native libraries for simulation... + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + + // JUnit 5 + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// THIS IS THE CRITICAL LINE — it adds the desktop native libraries +// (including the HAL simulation JNI) to the test classpath so that +// HAL.initialize() works inside JUnit tests. +wpi.java.configureTestTasks(test) +``` + +**Key point:** `wpi.java.configureTestTasks(test)` copies the platform-specific native libraries (`.so` / `.dylib` / `.dll`) onto the JUnit test classpath. Without this line, `HAL.initialize()` would throw an `UnsatisfiedLinkError` because the JNI bindings wouldn't be available. + +No additional Gradle configuration is required. + +--- + +## Writing Tests — Patterns and Examples + +### 1. Minimal HAL initialization + +The simplest approach — used when you need WPILib math or path-planning classes that internally touch the HAL, but you aren't simulating specific hardware. + +```java +package frc.robot.example; + +import static org.junit.jupiter.api.Assertions.*; + +import edu.wpi.first.hal.HAL; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; + +class MinimalHALTest { + + @BeforeAll + static void initHAL() { + // Initializes the HAL simulation backend. + // 500 = timeout in ms (standard value), 0 = default mode. + HAL.initialize(500, 0); + } + + @Test + void testSomethingThatTouchesHAL() { + // WPILib classes that internally call HAL functions will work here. + // Example: PathPlannerPath, Rotation2d, kinematics classes, etc. + assertTrue(true); + } +} +``` + +This is the pattern used by the existing `TrajectoryGenerationTest` in this project: + +```java +@BeforeAll +static void initHAL() { + // HAL must be initialized for PathPlannerPath internals to work. + HAL.initialize(500, 0); +} +``` + +### 2. Testing with hardware simulation objects + +When you want to test code that reads sensors or drives motors, you use the `*Sim` classes from `edu.wpi.first.wpilibj.simulation`. These give you setter methods to inject simulated sensor values and getter methods to observe what the robot code commanded. + +```java +package frc.robot.example; + +import static org.junit.jupiter.api.Assertions.*; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +class EncoderSimTest { + + private Encoder encoder; + private EncoderSim encoderSim; + + @BeforeAll + static void initHAL() { + HAL.initialize(500, 0); + } + + @BeforeEach + void setup() { + encoder = new Encoder(0, 1); // Create real Encoder object + encoderSim = new EncoderSim(encoder); // Wrap it with the sim companion + } + + @AfterEach + void teardown() { + encoder.close(); // Release HAL port allocations + } + + @Test + void testEncoderReadsInjectedDistance() { + encoderSim.setDistance(2.5); // Inject a simulated distance reading + + assertEquals(2.5, encoder.getDistance(), 0.001, + "Encoder.getDistance() should return the value injected via EncoderSim"); + } + + @Test + void testEncoderReadsInjectedRate() { + encoderSim.setRate(1.2); + + assertEquals(1.2, encoder.getRate(), 0.001, + "Encoder.getRate() should return the value injected via EncoderSim"); + } +} +``` + +### 3. Testing with DriverStation state + +`DriverStationSim` lets you programmatically set the robot's mode and enabled state. This is useful for testing code paths that depend on `DriverStation.isEnabled()`, `DriverStation.isAutonomous()`, etc. + +```java +package frc.robot.example; + +import static org.junit.jupiter.api.Assertions.*; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; + +class DriverStationSimTest { + + @BeforeAll + static void initHAL() { + HAL.initialize(500, 0); + } + + @Test + void testCanSetRobotToAutonomousEnabled() { + DriverStationSim.setAutonomous(true); + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); // Flush changes to the DriverStation + + assertTrue(DriverStation.isAutonomousEnabled(), + "DriverStation should report autonomous-enabled after DriverStationSim sets it"); + } + + @Test + void testCanSetRobotToDisabled() { + DriverStationSim.setEnabled(false); + DriverStationSim.notifyNewData(); + + assertFalse(DriverStation.isEnabled(), + "DriverStation should report disabled"); + } +} +``` + +**Important:** Call `DriverStationSim.notifyNewData()` after changing DriverStation state. This flushes the simulated values so that `DriverStation` queries see the updated state. + +### 4. Testing WPILib Commands with the CommandScheduler + +When testing `Command`-based logic, you need to manage the `CommandScheduler` lifecycle. The scheduler is a singleton with global state that persists across tests unless explicitly reset. + +```java +package frc.robot.example; + +import static org.junit.jupiter.api.Assertions.*; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +class CommandSchedulerTest { + + @BeforeAll + static void initHAL() { + HAL.initialize(500, 0); + } + + @BeforeEach + void setup() { + // Reset the CommandScheduler to a clean state before each test. + // This removes all registered subsystems, default commands, + // and scheduled commands from previous tests. + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().unregisterAllSubsystems(); + } + + @AfterEach + void teardown() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().unregisterAllSubsystems(); + } + + @Test + void testInstantCommandRunsOnce() { + int[] counter = {0}; + Command cmd = new InstantCommand(() -> counter[0]++); + + cmd.schedule(); + CommandScheduler.getInstance().run(); // Execute one scheduler cycle + + assertEquals(1, counter[0], "InstantCommand should have executed once"); + assertTrue(cmd.isFinished(), "InstantCommand should be finished after one cycle"); + } +} +``` + +**Lifecycle pattern summary:** + +| Annotation | Action | +|---------------|-----------------------------------------------| +| `@BeforeAll` | `HAL.initialize(500, 0)` — once per test class | +| `@BeforeEach` | Reset `CommandScheduler`, create hardware objects | +| `@AfterEach` | Cancel commands, unregister subsystems, `close()` hardware objects | + +### 5. Available simulation classes + +The `edu.wpi.first.wpilibj.simulation` package includes simulation companions for most WPILib hardware classes: + +| Sim Class | Corresponding Hardware | Key Methods | +|-----------------------|-------------------------------|------------------------------------------| +| `EncoderSim` | `Encoder` | `setDistance()`, `setRate()`, `setCount()` | +| `AnalogInputSim` | `AnalogInput` | `setVoltage()` | +| `DigitalInputSim` | `DigitalInput` | `setValue()` | +| `DIOSim` | `DigitalOutput` | `setValue()` | +| `PWMSim` | `PWMMotorController` subclasses | `getSpeed()`, `getPosition()` | +| `DriverStationSim` | `DriverStation` | `setEnabled()`, `setAutonomous()`, `notifyNewData()` | +| `RoboRioSim` | `RobotController` | `setVInVoltage()` (battery voltage) | +| `AnalogGyroSim` | `AnalogGyro` | `setAngle()`, `setRate()` | +| `BatterySim` | (utility) | `calculateDefaultBatteryLoadedVoltage()` | + +**Note on vendor hardware:** CTRE and REV provide their own simulation support. For CTRE Phoenix 6, see `TalonFXSimState`. For REV, see `SparkMaxSim` / `SparkFlexSim`. These are separate from the WPILib `*Sim` classes. + +--- + +## Running Tests + +### Locally + +```bash +# Run all tests (this is what 'build' does — compile + test) +./gradlew build + +# Run only tests (skip compilation if already built) +./gradlew test + +# Run a single test class +./gradlew test --tests "frc.robot.auto.TrajectoryGenerationTest" + +# Run with verbose output (shows individual test pass/fail) +./gradlew test --info + +# Re-run tests even if nothing changed +./gradlew test --rerun +``` + +Test reports are generated at `build/reports/tests/test/index.html`. + +### Platform compatibility + +Tests using HAL simulation run on all three desktop platforms: + +| Platform | Native Library | Status | +|----------------|---------------|--------| +| Windows x86_64 | `.dll` | Supported | +| macOS x86_64 | `.dylib` | Supported | +| macOS arm64 | `.dylib` | Supported | +| Linux x86_64 | `.so` | Supported | + +The `configureTestTasks` Gradle helper detects the current platform and loads the correct native library automatically. + +--- + +## GitHub Actions CI/CD + +### Current project configuration + +The project's existing CI workflow (`.github/workflows/ci.yml`) already runs `./gradlew build`, which includes the `test` task: + +```yaml +- name: Build with Gradle + run: ./gradlew build +``` + +This means **HAL-sim-based JUnit tests already run in CI** with no additional configuration needed. The workflow runs on `ubuntu-latest`, which provides a Linux x86_64 environment where the HAL native libraries load normally. + +### Why it works headless + +- The `halsim_gui` extension (Sim GUI) is **not loaded** during JUnit tests. GUI extensions are only loaded by `simulateJava` / `simulateNative` tasks. +- The `halsim_ds_socket` extension (DriverStation socket) is also **not loaded**. These extensions are configured in the `wpi.sim.*` block of `build.gradle`, which only applies to simulation tasks, not the `test` task. +- JUnit tests use only the base HAL simulation backend — a lightweight, in-process, headless simulation of the hardware abstraction layer. No display server, no network listeners, no GUI toolkit. + +### Workflow recommendations + +If you want to add a dedicated test-results step to the CI workflow, you can extend it: + +```yaml +jobs: + build: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Set up JDK 17 + uses: actions/setup-java@v4 + with: + java-version: '17' + distribution: 'temurin' + + - name: Cache Gradle packages + uses: actions/cache@v4 + with: + path: | + ~/.gradle/caches + ~/.gradle/wrapper + key: ${{ runner.os }}-gradle-${{ hashFiles('**/*.gradle*', '**/gradle-wrapper.properties') }} + restore-keys: | + ${{ runner.os }}-gradle- + + - name: Make gradlew executable + run: chmod +x ./gradlew + + - name: Build with Gradle + run: ./gradlew build + + # Optional: publish JUnit XML results as a check annotation + - name: Publish test results + uses: EnricoMi/publish-unit-test-result-action@v2 + if: always() + with: + files: build/test-results/test/**/*.xml + + - name: Upload build artifacts + uses: actions/upload-artifact@v4 + if: success() + with: + name: build-artifacts + path: | + build/libs/*.jar + build/reports/** + retention-days: 7 +``` + +The `publish-unit-test-result-action` step parses the JUnit XML output and adds pass/fail annotations directly to the pull request. + +--- + +## Troubleshooting + +### `UnsatisfiedLinkError` when running tests + +**Cause:** The HAL native libraries are not on the test classpath. + +**Fix:** Verify that `build.gradle` contains: +```gradle +wpi.java.configureTestTasks(test) +``` +This line must appear **after** the `test { ... }` block. + +### `HAL not initialized` errors + +**Cause:** `HAL.initialize(500, 0)` was not called before the test created WPILib hardware objects. + +**Fix:** Add a `@BeforeAll` method: +```java +@BeforeAll +static void initHAL() { + HAL.initialize(500, 0); +} +``` + +### Port allocation conflicts between tests + +**Cause:** Two tests allocate the same HAL port (e.g., DIO channel 0) without releasing it. + +**Fix:** Call `.close()` on hardware objects in `@AfterEach`: +```java +@AfterEach +void teardown() { + encoder.close(); + motor.close(); +} +``` + +### `CommandScheduler` state leaking between tests + +**Cause:** The `CommandScheduler` is a singleton. Registered subsystems, default commands, and scheduled commands persist across tests within the same JVM. + +**Fix:** Reset the scheduler in `@BeforeEach` and/or `@AfterEach`: +```java +@BeforeEach +void setup() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().unregisterAllSubsystems(); +} +``` + +### Tests pass locally but fail in CI + +Common causes: +- **Platform mismatch:** The CI environment is Linux x86_64. If your local machine is macOS arm64, the native libraries are different. Both should work, but vendor libraries (CTRE, REV) may have platform-specific gaps. +- **Non-deterministic timing:** Avoid `Thread.sleep()` or real-time waits in tests. Use `CommandScheduler.getInstance().run()` to advance the scheduler deterministically. +- **File path assumptions:** CI clones to a different directory. Use classpath resources instead of absolute paths. + +--- + +## Summary + +| Topic | Key Detail | +|----------------------|-------------------------------------------------------------------| +| Initialization | `HAL.initialize(500, 0)` in `@BeforeAll` | +| Gradle setup | `wpi.java.configureTestTasks(test)` — already present in project | +| Running tests | `./gradlew build` or `./gradlew test` | +| CI/CD | Works out of the box — `./gradlew build` in GitHub Actions | +| GUI required? | No — tests are headless | +| Platform support | Windows, macOS (Intel + Apple Silicon), Linux | +| Cleanup pattern | `.close()` hardware in `@AfterEach`, reset `CommandScheduler` | + +--- + +## References + +- [WPILib Simulation Documentation](https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/index.html) +- [WPILib Unit Testing Documentation](https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/unit-testing.html) +- [`edu.wpi.first.wpilibj.simulation` API Javadoc](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj/simulation/package-summary.html) +- [`DriverStationSim` API Javadoc](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.html) diff --git a/src/main/java/frc/lib/SendableChooserUtil.java b/src/main/java/frc/lib/SendableChooserUtil.java new file mode 100644 index 0000000..5562946 --- /dev/null +++ b/src/main/java/frc/lib/SendableChooserUtil.java @@ -0,0 +1,90 @@ +package frc.lib; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import java.util.function.Function; + +/** + * Utility for creating pre-populated {@link SendableChooser} instances from enums. + * + *

Eliminates the boilerplate of iterating over enum constants, calling + * {@code setDefaultOption}/{@code addOption}, and handling empty enums. + * + *

Usage examples: + * 

{@code
+ * // Uses toString() for display names, first constant is default
+ * SendableChooser chooser = SendableChooserUtil.fromEnum(AutoMode.class);
+ *
+ * // Specific default value, toString() for display names
+ * SendableChooser chooser =
+ *     SendableChooserUtil.fromEnum(AutoMode.class, AutoMode.DriveTestAuto);
+ *
+ * // Custom display name function with specific default
+ * SendableChooser chooser =
+ *     SendableChooserUtil.fromEnum(DiagnosticTestRegistry.class,
+ *         DiagnosticTestRegistry.SWERVE_ANGLE_DRIFT,
+ *         DiagnosticTestRegistry::getDisplayName);
+ * }
+ */ +public final class SendableChooserUtil { + + private SendableChooserUtil() {} // Prevent instantiation + + /** + * Creates a {@link SendableChooser} populated with all constants of the given enum. + * + *

The first enum constant (ordinal 0) is used as the default selection. + * Display names are generated using each constant's {@code toString()} method. + * + * @param The enum type + * @param enumClass The enum class to populate from + * @return A new SendableChooser containing all enum constants + */ + public static > SendableChooser fromEnum(Class enumClass) { + E[] constants = enumClass.getEnumConstants(); + return fromEnum(enumClass, constants.length > 0 ? constants[0] : null); + } + + /** + * Creates a {@link SendableChooser} populated with all constants of the given enum, + * with a specific default value. + * + *

Display names are generated using each constant's {@code toString()} method. + * + * @param The enum type + * @param enumClass The enum class to populate from + * @param defaultValue The constant to mark as the default selection + * @return A new SendableChooser containing all enum constants + */ + public static > SendableChooser fromEnum( + Class enumClass, E defaultValue) { + return fromEnum(enumClass, defaultValue, Enum::toString); + } + + /** + * Creates a {@link SendableChooser} populated with all constants of the given enum, + * with a specific default value and a custom function for generating display names. + * + *

This is the most flexible overload — use it when enum constants need custom + * labels in the SmartDashboard dropdown (e.g., {@code getDisplayName()} instead of + * {@code toString()}). + * + * @param The enum type + * @param enumClass The enum class to populate from + * @param defaultValue The constant to mark as the default selection + * @param displayNameFunc Function that maps each enum constant to its display name + * @return A new SendableChooser containing all enum constants + */ + public static > SendableChooser fromEnum( + Class enumClass, E defaultValue, Function displayNameFunc) { + SendableChooser chooser = new SendableChooser<>(); + for (E constant : enumClass.getEnumConstants()) { + String displayName = displayNameFunc.apply(constant); + if (constant == defaultValue) { + chooser.setDefaultOption(displayName, constant); + } else { + chooser.addOption(displayName, constant); + } + } + return chooser; + } +} diff --git a/src/main/java/frc/lib/test/DiagnosticTest.java b/src/main/java/frc/lib/test/DiagnosticTest.java new file mode 100644 index 0000000..550ab15 --- /dev/null +++ b/src/main/java/frc/lib/test/DiagnosticTest.java @@ -0,0 +1,115 @@ +package frc.lib.test; + +/** + * Interface that defines a common contract for all diagnostic tests. + * + *

Diagnostic tests should implement this interface in addition to extending {@link edu.wpi.first.wpilibj2.command.Command}. + * This interface provides metadata and result reporting capabilities while allowing tests to remain Commands + * for integration with WPILib's CommandScheduler. + * + *

All methods except {@link #getTestName()} have default implementations, making it easy to create simple tests + * that only need to provide a name. More complex tests can override the default methods to provide descriptions, + * parameter initialization, and result reporting. + * + *

Example Usage: + * 

{@code
+ * public class MyTest extends Command implements DiagnosticTest {
+ *     @Override
+ *     public String getTestName() {
+ *         return "My Test";
+ *     }
+ *     
+ *     @Override
+ *     public void initializeParameters() {
+ *         TestDashboard.putParamDouble(this, "Speed", 0.5);
+ *         TestDashboard.putParamInt(this, "Cycles", 10);
+ *     }
+ *     
+ *     @Override
+ *     public void initialize() {
+ *         double speed = TestDashboard.getParamDouble(this, "Speed", 0.5);
+ *         int cycles = TestDashboard.getParamInt(this, "Cycles", 10);
+ *     }
+ * }
+ * }
+ */ +public interface DiagnosticTest { + + /** + * Gets the display name for this test. + * This name will be shown in the SmartDashboard test selector dropdown. + * + * @return The display name of the test + */ + String getTestName(); + + /** + * Gets an optional description of what this test does. + * Can be overridden to provide helpful information about the test's purpose. + * + * @return A description of the test, or "No description available" if not overridden + */ + default String getTestDescription() { + return "No description available"; + } + + /** + * Initializes SmartDashboard parameters for this test. + * + *

This method is called by the framework when a test is selected in the dashboard, + * allowing the test to set up its parameter UI before execution. Tests should use + * {@link TestDashboard} to initialize parameters with default values. + * + *

This method has a default empty implementation. Override it to set up parameters: + * 

{@code
+     * @Override
+     * public void initializeParameters() {
+     *     TestDashboard.putParamDouble(this, "Speed", 0.5);
+     *     TestDashboard.putParamInt(this, "Cycles", 10);
+     * }
+     * }
+ */ + default void initializeParameters() { + // Override to set up SmartDashboard parameters + } + + /** + * Gets the result of this test after it has completed. + * + *

This method is called by the framework after a test finishes (either normally or interrupted) + * to retrieve the test results for display in SmartDashboard. Tests should create and store + * a {@link DiagnosticTestResult} in their {@link edu.wpi.first.wpilibj2.command.Command#end(boolean) end()} + * method and return it here. + * + *

This method has a default implementation that returns null. Override it to return results: + * 

{@code
+     * private DiagnosticTestResult result;
+     * 
+     * @Override
+     * public void end(boolean interrupted) {
+     *     if (interrupted) {
+     *         result = DiagnosticTestResult.cancelled("Test was interrupted");
+     *     } else {
+     *         Map metrics = new HashMap<>();
+     *         metrics.put("cycles", MetricValue.of(completedCycles));
+     *         result = DiagnosticTestResult.passWithMetrics("Test completed", metrics);
+     *     }
+     * }
+     * 
+     * @Override
+     * public DiagnosticTestResult getResult() {
+     *     return result;
+     * }
+     * }
+ * + * @return The test result, or null if the test hasn't completed yet or doesn't provide results + */ + /* + + TO BE ADDED LATER + + default DiagnosticTestResult getResult() { + return null; // Override to return test results + } + */ +} diff --git a/src/main/java/frc/lib/test/DiagnosticTestManager.java b/src/main/java/frc/lib/test/DiagnosticTestManager.java new file mode 100644 index 0000000..e0fca2a --- /dev/null +++ b/src/main/java/frc/lib/test/DiagnosticTestManager.java @@ -0,0 +1,450 @@ +package frc.lib.test; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.lib.SendableChooserUtil; +import frc.robot.RobotContainer; +import java.util.HashMap; +import java.util.Map; + +/** + * Manages the lifecycle and SmartDashboard integration for diagnostic tests. + * + *

This manager provides a unified interface for running diagnostic tests: + *

    + *
  • Test selection via dropdown (SendableChooser)
    • + *
  • Parameter initialization when tests are selected
    • + *
  • Test execution via a proxy {@link TestRunnerCommand} published as a dashboard button
    • + *
  • Status monitoring and display
    • + *
+ * + *

Test instances are created once in the constructor and reused across runs. + * All test commands support reuse because they fully reset state in + * {@code initialize()} and read fresh parameters from SmartDashboard each run. + * + *

Usage: + *

    + *
  1. Create instance in {@code Robot.testInit()}
    2. + *
  2. Call {@code periodic()} in {@code Robot.testPeriodic()}
    3. + *
  3. Call {@code cleanup()} in {@code Robot.testEnd()}
    4. + *
+ * + *

SmartDashboard Layout: + * 

+ * DiagnosticTests/
+ *   ├── TestSelector/             (SendableChooser - dropdown)
+ *   ├── StartTest/                (Command button - clickable in Elastic and SimGUI)
+ *   ├── CurrentTest/              (String - name of running test or "None")
+ *   ├── CurrentTest Description/  (String - description of the selected test)
+ *   ├── CurrentTest Status/       (String - Idle, Running, Complete, Cancelled, Error)
+ *   └── Message/                  (String - status messages and error information)
+ *
+ * + *

Button Behavior: The "StartTest" entry is a {@link TestRunnerCommand} published + * via {@code SmartDashboard.putData()}. Elastic and SimGUI render it as a native Command + * toggle button. When clicked, it schedules the currently selected test. When clicked again + * (or the test completes), the button resets. + */ +public class DiagnosticTestManager { + + private static final String DASHBOARD_PREFIX = "DiagnosticTests/"; + private static final String KEY_TEST_SELECTOR = DASHBOARD_PREFIX + "TestSelector"; + private static final String KEY_START_TEST = DASHBOARD_PREFIX + "StartTest"; + private static final String KEY_CURRENT_TEST = DASHBOARD_PREFIX + "CurrentTest"; + private static final String KEY_TEST_STATUS = DASHBOARD_PREFIX + "CurrentTest Status"; + private static final String KEY_MESSAGE = DASHBOARD_PREFIX + "Message"; + private static final String KEY_DESCRIPTION = DASHBOARD_PREFIX + "CurrentTest Description"; + + private final RobotContainer robotContainer; + private final SendableChooser testChooser; + private final Map testInstances = new HashMap<>(); + private final TestRunnerCommand runTestCommand; + + private Command activeTest = null; + private DiagnosticTestRegistry lastSelectedTest = null; + private DiagnosticTestRegistry suppressedSelectionWarning = null; // Tracks which selection we've already warned about during a running test + private TestStatus currentStatus = TestStatus.IDLE; + + /** + * Internal enum for tracking test status. + */ + private enum TestStatus { + IDLE("Idle"), + RUNNING("Running"), + COMPLETE("Complete"), + ERROR("Error"), + CANCELLED("Cancelled"); + + private final String displayName; + + TestStatus(String displayName) { + this.displayName = displayName; + } + + @Override + public String toString() { + return displayName; + } + } + + /** + * Creates a new DiagnosticTestManager. + * + *

Creates all test instances once and stores them for reuse. Also creates + * the proxy {@link TestRunnerCommand} that will be published as a dashboard button. + * + * @param robotContainer The robot container providing access to subsystems + */ + public DiagnosticTestManager(RobotContainer robotContainer) { + this.robotContainer = robotContainer; + this.testChooser = SendableChooserUtil.fromEnum( + DiagnosticTestRegistry.class, + DiagnosticTestRegistry.values()[0], + DiagnosticTestRegistry::getDisplayName); + + // Create all test instances once (persistent — reused across runs) + // * iterate over all test entries in the DiagnosticTestRegistry enum + for (DiagnosticTestRegistry entry : DiagnosticTestRegistry.values()) { + try { + Command test = entry.createTest(robotContainer); + testInstances.put(entry, test); + } catch (Exception e) { + System.err.println("Failed to create test: " + entry.getDisplayName() + + ": " + e.getMessage()); + // Test won't be available, but other tests still work + } + } + + // Create proxy command (published as a button in initializeDashboard()) + this.runTestCommand = new TestRunnerCommand(this::getSelectedTestInstance); + + initializeDashboard(); + } + + /** + * Initializes the SmartDashboard UI for test selection and execution. + * Called once during construction. + */ + private void initializeDashboard() { + // testChooser is already populated by SendableChooserUtil.fromEnum() in constructor + SmartDashboard.putData(KEY_TEST_SELECTOR, testChooser); + + // Publish proxy command as a clickable button (works in Elastic and SimGUI) + SmartDashboard.putData(KEY_START_TEST, runTestCommand); + + // Initialize status display + SmartDashboard.putString(KEY_CURRENT_TEST, "None"); + SmartDashboard.putString(KEY_TEST_STATUS, TestStatus.IDLE.toString()); + SmartDashboard.putString(KEY_MESSAGE, "Select a test and click Start Test to begin"); + SmartDashboard.putString(KEY_DESCRIPTION, ""); + } + + /** + * Periodic update method. Should be called every 20ms in {@code Robot.testPeriodic()}. + * + *

This method: + *

    + *
  • Monitors test selection dropdown for changes
    • + *
  • Initializes parameters when a test is selected
    • + *
  • Monitors active test status and updates display
    • + *
  • Detects unexpected test failures and handles errors
    • + *
+ * + *

Note: Start/cancel actions are handled by the {@link TestRunnerCommand} proxy + * via the CommandScheduler, not by polling a boolean. + */ + public void periodic() { + // Check chooser and ensure Current Test field is synchronized with selection + updateTestSelection(); + + // Monitor active test status and detect failures + updateTestStatus(); + } + + /** + * Checks the chooser selection and ensures the Current Test field and test selection are synchronized. + * + *

This method centralizes all logic related to monitoring the test selector chooser: + *

    + *
  • Updates the Current Test field to match the chooser selection
    • + *
  • Detects when the selection has changed
    • + *
  • Initializes parameters for newly selected tests
    • + *
+ * + *

This should be called periodically (e.g., from {@code periodic()}) to keep the UI + * synchronized with the chooser state. + */ + private void updateTestSelection() { + // Get current selection and update Current Test field + DiagnosticTestRegistry selectedTest = testChooser.getSelected(); + if (selectedTest != null) { + SmartDashboard.putString(KEY_CURRENT_TEST, selectedTest.getDisplayName()); + } else { + SmartDashboard.putString(KEY_CURRENT_TEST, "None"); + } + + // Check if selection has changed (enums use == for identity comparison) + if (selectedTest != null && selectedTest != lastSelectedTest) { + // Selection changed - check if we can allow the change + + // Don't allow selection change if a test is currently running + // Ignore the change and keep showing the running test + if (isTestRunning()) { + // Print warning once per selection change (suppress repeats) + if (selectedTest != suppressedSelectionWarning) { + System.out.println("Warning: Cannot change test selection while a test is running. " + + "Please wait for the current test to complete or cancel it first."); + suppressedSelectionWarning = selectedTest; + } + + // Keep "Current Test" showing the running test (not the new selection) + String runningTestName = getActiveTestName(); + if (!runningTestName.equals("No Active Test")) { + SmartDashboard.putString(KEY_CURRENT_TEST, runningTestName); + } + + // Do NOT update lastSelectedTest here — when the test completes, + // the selection change will be detected and parameters will be initialized + return; + } + + // Get test display name + String displayName = selectedTest.getDisplayName(); + // Get the selected test command + Command testCommand = testInstances.get(selectedTest); + + if (testCommand == null) { + // Test instance failed to create during construction + SmartDashboard.putString(KEY_DESCRIPTION, ""); + SmartDashboard.putString(KEY_MESSAGE, "Test creation failed: " + displayName); + System.err.println("Test creation failed: " + displayName); + } else if (testCommand instanceof DiagnosticTest) { + DiagnosticTest diagnosticTest = (DiagnosticTest) testCommand; + diagnosticTest.initializeParameters(); + SmartDashboard.putString(KEY_DESCRIPTION, diagnosticTest.getTestDescription()); + System.out.println("Initialized parameters for: " + displayName); + SmartDashboard.putString(KEY_MESSAGE, "Test selected: " + displayName + ". Click Start Test to begin."); + } else { + // Test doesn't implement DiagnosticTest yet (e.g., during Phase 2 migration) + SmartDashboard.putString(KEY_DESCRIPTION, ""); + System.out.println("Note: " + displayName + " does not implement DiagnosticTest interface yet"); + SmartDashboard.putString(KEY_MESSAGE, "Test selected: " + displayName + ". Click Start Test to begin."); + } + + lastSelectedTest = selectedTest; + } else if (selectedTest == null && lastSelectedTest != null) { + // Selection was cleared (shouldn't normally happen, but handle it) + lastSelectedTest = null; + SmartDashboard.putString(KEY_DESCRIPTION, ""); + // Update message when selection is cleared (only if no test is running) + if (!isTestRunning()) { + SmartDashboard.putString(KEY_MESSAGE, "Select a test and click Start Test to begin"); + } + } + } + + /** + * Supplier method for {@link TestRunnerCommand}. Returns the currently selected + * persistent test instance, or null if no valid test is available. + * + *

Called by the proxy's {@code initialize()} when the user clicks the button. + * This method is a pure lookup — it does not modify manager state. The manager + * detects the newly scheduled test in {@link #updateTestStatus()} on the next + * {@link #periodic()} call (same cycle — no gap). + * + * @return The persistent test command to schedule, or null if unavailable + */ + private Command getSelectedTestInstance() { + DiagnosticTestRegistry selected = testChooser.getSelected(); + if (selected == null) { + SmartDashboard.putString(KEY_MESSAGE, + "No test selected. Select a test from the dropdown."); + return null; + } + + Command test = testInstances.get(selected); + if (test == null) { + SmartDashboard.putString(KEY_MESSAGE, + "Test creation failed: " + selected.getDisplayName()); + return null; + } + + return test; + } + + /** + * Monitors the active test and updates status display. + * + *

Detects three kinds of transitions: + *

    + *
  • New test started: No active test, but the selected test instance is now + * scheduled (started by the proxy). Sets {@code activeTest} and transitions to RUNNING.
    • + *
  • Test completed/cancelled: Active test is no longer scheduled. Transitions + * to COMPLETE or CANCELLED based on {@code isFinished()}.
    • + *
  • Error: Exception while monitoring the active test. Transitions to ERROR.
    • + *
+ */ + private void updateTestStatus() { + // Note: Current Test field is kept in sync by updateTestSelection() called from periodic() + + // If no test is currently being tracked as running, check if the proxy started one. + // This handles: first start (activeTest==null), re-run after completion (activeTest + // kept for status display but not scheduled), and switching to a different test. + if (!isTestRunning()) { + DiagnosticTestRegistry selected = testChooser.getSelected(); + if (selected != null) { + Command test = testInstances.get(selected); + if (test != null && CommandScheduler.getInstance().isScheduled(test)) { + // Proxy scheduled this test — begin tracking it + activeTest = test; + suppressedSelectionWarning = null; + } + } + } + + // No active test to monitor — ensure IDLE status and return early + if (activeTest == null) { + if (currentStatus != TestStatus.IDLE) { + currentStatus = TestStatus.IDLE; + SmartDashboard.putString(KEY_TEST_STATUS, currentStatus.toString()); + // Message is handled by updateTestSelection() based on current selection state + } + return; + } + + // Check if test is still scheduled and valid + boolean isScheduled = false; + boolean isFinished = false; + + try { + isScheduled = CommandScheduler.getInstance().isScheduled(activeTest); + isFinished = activeTest.isFinished(); + } catch (Exception e) { + // Test encountered an exception - mark as error + System.err.println("Unexpected error monitoring test: " + e.getMessage()); + e.printStackTrace(); + + // Cancel the test (defensive cleanup) + // - activeTest cannot be null here + try { + activeTest.cancel(); + } catch (Exception cancelException) { + System.err.println("Error cancelling test after monitoring failure: " + cancelException.getMessage()); + } + + currentStatus = TestStatus.ERROR; + String testName = getActiveTestName(); + SmartDashboard.putString(KEY_TEST_STATUS, currentStatus.toString()); + SmartDashboard.putString(KEY_MESSAGE, "Test error: " + e.getMessage() + + " (Test: " + testName + ")"); + // Clear active test reference since it's in an error state + activeTest = null; + suppressedSelectionWarning = null; + return; + } + + if (!isScheduled && currentStatus == TestStatus.RUNNING) { + // Test is no longer scheduled - determine why + String testName = getActiveTestName(); + if (isFinished) { + // Test completed normally + currentStatus = TestStatus.COMPLETE; + SmartDashboard.putString(KEY_MESSAGE, "Test completed: " + testName); + } else { + // Test is not scheduled and not finished — it was cancelled + // (e.g., by the proxy's end(interrupted) or by CommandScheduler due to subsystem conflict) + currentStatus = TestStatus.CANCELLED; + SmartDashboard.putString(KEY_MESSAGE, "Test cancelled: " + testName); + } + SmartDashboard.putString(KEY_TEST_STATUS, currentStatus.toString()); + // Note: Current Test field is kept in sync by updateTestSelection() called from periodic() + + // Keep activeTest reference so completion status is displayed until a new test starts + } else if (isScheduled && currentStatus != TestStatus.RUNNING) { + // Test is running (either just started via proxy, or resumed unexpectedly) + currentStatus = TestStatus.RUNNING; + String testName = getActiveTestName(); + SmartDashboard.putString(KEY_TEST_STATUS, currentStatus.toString()); + SmartDashboard.putString(KEY_MESSAGE, "Test running: " + testName); + System.out.println("Started test: " + testName); + } + } + + /** + * Cleans up resources and cancels any active test. + * Should be called in {@code Robot.testEnd()}. + */ + public void cleanup() { + // Cancel the proxy command (which will also cancel the inner test if running) + if (CommandScheduler.getInstance().isScheduled(runTestCommand)) { + runTestCommand.cancel(); + } + + // Cancel any active test (defensive — proxy's end() should have done this) + if (activeTest != null) { + try { + if (CommandScheduler.getInstance().isScheduled(activeTest)) { + activeTest.cancel(); + } + } catch (Exception e) { + System.err.println("Error cancelling test during cleanup: " + e.getMessage()); + e.printStackTrace(); + } + activeTest = null; + suppressedSelectionWarning = null; + } + + // Release all test command instances so they (and their subsystem references) can be GC'd. + testInstances.clear(); + + // Break the reference chain: NetworkTables → TestRunnerCommand → supplier → this manager. + // Nulling the supplier allows this DiagnosticTestManager (and everything it owns) to be + // garbage collected once Robot.testExit() sets m_testManager = null. + // The TestRunnerCommand shell remains in NetworkTables (no WPILib API to remove Sendables), + // but with a null supplier any stale button click is a safe no-op. + runTestCommand.cleanup(); + + // Clear SmartDashboard entries when exiting test mode by setting to default/empty values + // NetworkTables entries persist until overwritten, so we set them to empty values + // They'll be recreated with proper values on next testInit() + SmartDashboard.putString(KEY_CURRENT_TEST, ""); + SmartDashboard.putString(KEY_DESCRIPTION, ""); + SmartDashboard.putString(KEY_TEST_STATUS, ""); + SmartDashboard.putString(KEY_MESSAGE, ""); + + currentStatus = TestStatus.IDLE; + lastSelectedTest = null; + } + + /** + * Checks whether a test is currently running (scheduled in the CommandScheduler). + * + * @return true if a test is actively running, false otherwise + */ + private boolean isTestRunning() { + return activeTest != null && CommandScheduler.getInstance().isScheduled(activeTest); + } + + /** + * Gets the display name of the active test. + * + *

If the test implements {@link DiagnosticTest}, returns {@link DiagnosticTest#getTestName()}. + * Otherwise, returns the simple class name as a fallback. + * + * @return The test name, or "No Active Test" if activeTest is null + */ + private String getActiveTestName() { + if (activeTest == null) { + return "No Active Test"; + } + + if (activeTest instanceof DiagnosticTest) { + return ((DiagnosticTest) activeTest).getTestName(); + } else { + // Fallback to class name for tests that don't implement DiagnosticTest yet + return activeTest.getClass().getSimpleName(); + } + } +} diff --git a/src/main/java/frc/lib/test/DiagnosticTestRegistry.java b/src/main/java/frc/lib/test/DiagnosticTestRegistry.java new file mode 100644 index 0000000..0e18fe8 --- /dev/null +++ b/src/main/java/frc/lib/test/DiagnosticTestRegistry.java @@ -0,0 +1,142 @@ +package frc.lib.test; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.test.LedStateTestCommand; +import frc.robot.test.OdometryResetTestCommand; +import frc.robot.test.SwerveAlignmentTestCommand; +import frc.robot.test.SwerveAngleDriftTestCommand; +import frc.robot.test.SwerveStraightLineTestCommand; + +/** + * Central registry of available diagnostic tests. + * + *

This enum-based registry provides compile-time safety and makes all available tests + * visible in one place. Each enum value represents a test and provides: + *

    + *
  • Display name for the SmartDashboard UI
    • + *
  • Factory method to create test instances
    • + *
+ * + *

Note: Test descriptions are provided by each test's + * {@link DiagnosticTest#getTestDescription()} method, not by the registry. + * + *

To add a new test: + *

    + *
  1. Create a test class that extends {@link edu.wpi.first.wpilibj2.command.Command} + * and implements {@link DiagnosticTest}
    2. + *
  2. Add a new enum value to this registry
    3. + *
  3. Implement the factory method to create your test instance
    4. + *
+ * + *

Example: + * 

{@code
+ * MY_NEW_TEST("My New Test") {
+ *     @Override
+ *     public Command createTest(RobotContainer robotContainer) {
+ *         return new MyNewTestCommand(robotContainer.getSwerveSubsystem());
+ *     }
+ * }
+ * }
+ */ +public enum DiagnosticTestRegistry { + + /** + * Swerve angle drift test. + * Tests encoder drift by rotating a swerve module through multiple cycles + * and comparing relative encoder to absolute encoder measurements. + */ + SWERVE_ANGLE_DRIFT("Swerve Angle Drift Test") { + @Override + public Command createTest(RobotContainer robotContainer) { + return new SwerveAngleDriftTestCommand(robotContainer.getSwerveSubsystem()); + } + }, + + /** + * Swerve Alignment Test - Commands all modules to the same angle and measures accuracy. + * Reveals angle offset calibration errors that cause drift during driving. + */ + SWERVE_ALIGNMENT("Swerve Alignment Test") { + @Override + public Command createTest(RobotContainer robotContainer) { + return new SwerveAlignmentTestCommand(robotContainer.getSwerveSubsystem()); + } + }, + + /** + * Swerve Straight Line Test - Drives the robot straight with known inputs. + * Bypasses the joystick to test whether the drive code, motors, and mechanics + * allow the robot to drive straight. + */ + SWERVE_STRAIGHT_LINE("Swerve Straight Line Test") { + @Override + public Command createTest(RobotContainer robotContainer) { + return new SwerveStraightLineTestCommand(robotContainer.getSwerveSubsystem()); + } + }, + + /** + * LED State Test - Diagnostic test for CandleSubsystem. + * Allows independent testing of LED states without other robot systems. + * Useful for verifying LED hardware functionality and visual feedback. + */ + LED_STATE_TEST("LED State Test") { + @Override + public Command createTest(RobotContainer robotContainer) { + return new LedStateTestCommand(robotContainer.getCandleSubsystem()); + } + }, + + /** + * Odometry Reset Test - Demonstrates the pose reset bug (issue 8.1/8.2). + * Resets the robot's pose and observes whether the heading stays correct + * across subsequent cycles. Reveals bugs where the odometry baseline or + * SimulationManager stale data corrupt the heading. + */ + ODOMETRY_RESET("Odometry Reset Test") { + @Override + public Command createTest(RobotContainer robotContainer) { + return new OdometryResetTestCommand(robotContainer.getSwerveSubsystem()); + } + }; + + private final String displayName; + + /** + * Creates a new registry entry. + * + * @param displayName The name to display in the SmartDashboard dropdown + */ + DiagnosticTestRegistry(String displayName) { + this.displayName = displayName; + } + + /** + * Gets the display name for this test. + * This name will be shown in the SmartDashboard test selector dropdown. + * + * @return The display name + */ + public String getDisplayName() { + return displayName; + } + + /** + * Factory method to create an instance of this test. + * + *

Each enum value must implement this method to create its specific test instance. + * The test should be configured with dependencies from the RobotContainer, but + * parameters should be read from SmartDashboard in the test's {@code initialize()} + * method (after {@link DiagnosticTest#initializeParameters()} has been called). + * + *

Note: Return type is {@link Command} for now to allow tests that haven't + * yet implemented {@link DiagnosticTest} (e.g., during Phase 2 migration). Once all + * tests implement the interface, this should be changed to return {@link DiagnosticTest}. + * + * @param robotContainer The robot container providing access to subsystems + * @return A new instance of the diagnostic test command + */ + public abstract Command createTest(RobotContainer robotContainer); + +} diff --git a/src/main/java/frc/lib/test/TestDashboard.java b/src/main/java/frc/lib/test/TestDashboard.java new file mode 100644 index 0000000..4358181 --- /dev/null +++ b/src/main/java/frc/lib/test/TestDashboard.java @@ -0,0 +1,295 @@ +package frc.lib.test; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Helper class for managing SmartDashboard interactions in diagnostic tests. + * + *

This class eliminates the need for {@code PARAM_PREFIX} and {@code RESULT_PREFIX} constants + * by automatically constructing prefixes from the test instance using {@link DiagnosticTest#getTestName()}. + * + *

Key Features: + *

    + *
  • Automatic prefix construction: {@code "DiagnosticTests/" + testName + "/Parameters/"} or {@code "/Results/"}
    • + *
  • Type-safe methods: No manual type casting required
    • + *
  • Symmetric naming: Clear distinction between params (inputs) and results (outputs)
    • + *
  • Simplified SendableChooser handling: Built-in null checking and default values
    • + *
+ * + *

Usage Example: + * 

{@code
+ * public class MyTest extends Command implements DiagnosticTest {
+ *     @Override
+ *     public String getTestName() {
+ *         return "My Test";
+ *     }
+ *     
+ *     @Override
+ *     public void initializeParameters() {
+ *         // No PARAM_PREFIX constant needed!
+ *         TestDashboard.putParamInt(this, "ModuleNumber", 0);
+ *         TestDashboard.putParamDouble(this, "Angle", 90.0);
+ *     }
+ *     
+ *     @Override
+ *     public void initialize() {
+ *         // Read parameters - prefix automatically constructed
+ *         int module = TestDashboard.getParamInt(this, "ModuleNumber", 0);
+ *         double angle = TestDashboard.getParamDouble(this, "Angle", 90.0);
+ *     }
+ *     
+ *     @Override
+ *     public void execute() {
+ *         // Publish results - no RESULT_PREFIX constant needed!
+ *         TestDashboard.putResultInt(this, "Config/Module", module);
+ *         TestDashboard.putResultDouble(this, "Summary/TotalDrift", totalDrift);
+ *     }
+ * }
+ * }
+ * + *

Prefix Format: + *

    + *
  • Params: {@code "DiagnosticTests/Test: " + testName + "/Parameters/" + paramName}
    • + *
  • Results: {@code "DiagnosticTests/Test: " + testName + "/Results/" + resultName}
    • + *
+ * + *

The "Test: " prefix groups all test-related parameter and result fields together in SmartDashboard. + * + *

Result names can include path separators (e.g., {@code "Config/Module"}) to organize results into groups. + */ +public class TestDashboard { + + private static final String BASE_PREFIX = "DiagnosticTests/"; + private static final String PARAMS_SUFFIX = "/Parameters/"; + private static final String RESULTS_SUFFIX = "/Results/"; + + /** + * Enum to distinguish between params (inputs) and results (outputs). + * Used internally for prefix construction. + */ + private enum DataType { + PARAM, // Input params (read/write) + RESULT // Output results (write-only) + } + + /** + * Constructs the prefix for a test based on the test name and data type. + * + *

The test name is prefixed with "Test: " to group all test-related fields together + * in SmartDashboard (e.g., "DiagnosticTests/Test: My Test/Parameters/"). + * + * @param test The test instance (must not be null) + * @param type Whether this is a PARAM or RESULT + * @return The constructed prefix (e.g., "DiagnosticTests/Test: My Test/Parameters/") + * @throws IllegalArgumentException if test is null + * @throws IllegalStateException if test name is null or empty + */ + private static String getPrefix(DiagnosticTest test, DataType type) { + if (test == null) { + throw new IllegalArgumentException("Test instance cannot be null"); + } + String testName = test.getTestName(); + if (testName == null || testName.isEmpty()) { + throw new IllegalStateException("Test name cannot be null or empty. Implement getTestName() properly."); + } + String suffix = (type == DataType.PARAM) ? PARAMS_SUFFIX : RESULTS_SUFFIX; + return BASE_PREFIX + "Test: " + testName + suffix; + } + + // ============================================================================ + // Param Methods (Inputs - Read/Write) + // ============================================================================ + + /** + * Puts an integer param value to SmartDashboard. + * + * @param test The test instance + * @param paramName The name of the param + * @param defaultValue The default value to set + */ + public static void putParamInt(DiagnosticTest test, String paramName, int defaultValue) { + SmartDashboard.putNumber(getPrefix(test, DataType.PARAM) + paramName, defaultValue); + } + + /** + * Gets an integer param value from SmartDashboard. + * + * @param test The test instance + * @param paramName The name of the param + * @param defaultValue The default value to return if not found + * @return The param value, or defaultValue if not found + */ + public static int getParamInt(DiagnosticTest test, String paramName, int defaultValue) { + return (int) SmartDashboard.getNumber(getPrefix(test, DataType.PARAM) + paramName, defaultValue); + } + + /** + * Puts a double param value to SmartDashboard. + * + * @param test The test instance + * @param paramName The name of the param + * @param defaultValue The default value to set + */ + public static void putParamDouble(DiagnosticTest test, String paramName, double defaultValue) { + SmartDashboard.putNumber(getPrefix(test, DataType.PARAM) + paramName, defaultValue); + } + + /** + * Gets a double param value from SmartDashboard. + * + * @param test The test instance + * @param paramName The name of the param + * @param defaultValue The default value to return if not found + * @return The param value, or defaultValue if not found + */ + public static double getParamDouble(DiagnosticTest test, String paramName, double defaultValue) { + return SmartDashboard.getNumber(getPrefix(test, DataType.PARAM) + paramName, defaultValue); + } + + /** + * Puts a boolean param value to SmartDashboard. + * + * @param test The test instance + * @param paramName The name of the param + * @param defaultValue The default value to set + */ + public static void putParamBoolean(DiagnosticTest test, String paramName, boolean defaultValue) { + SmartDashboard.putBoolean(getPrefix(test, DataType.PARAM) + paramName, defaultValue); + } + + /** + * Gets a boolean param value from SmartDashboard. + * + * @param test The test instance + * @param paramName The name of the param + * @param defaultValue The default value to return if not found + * @return The param value, or defaultValue if not found + */ + public static boolean getParamBoolean(DiagnosticTest test, String paramName, boolean defaultValue) { + return SmartDashboard.getBoolean(getPrefix(test, DataType.PARAM) + paramName, defaultValue); + } + + /** + * Puts a string param value to SmartDashboard. + * + * @param test The test instance + * @param paramName The name of the param + * @param defaultValue The default value to set + */ + public static void putParamString(DiagnosticTest test, String paramName, String defaultValue) { + SmartDashboard.putString(getPrefix(test, DataType.PARAM) + paramName, defaultValue); + } + + /** + * Gets a string param value from SmartDashboard. + * + * @param test The test instance + * @param paramName The name of the param + * @param defaultValue The default value to return if not found + * @return The param value, or defaultValue if not found + */ + public static String getParamString(DiagnosticTest test, String paramName, String defaultValue) { + return SmartDashboard.getString(getPrefix(test, DataType.PARAM) + paramName, defaultValue); + } + + /** + * Puts a SendableChooser param to SmartDashboard. + * + * @param test The test instance + * @param paramName The name of the param + * @param chooser The SendableChooser to put (must not be null) + */ + public static void putParamChooser(DiagnosticTest test, String paramName, SendableChooser chooser) { + SmartDashboard.putData(getPrefix(test, DataType.PARAM) + paramName, chooser); + } + + /** + * Gets a SendableChooser param from SmartDashboard. + * + *

The type parameter {@code T} is inferred from usage context. For example: + * 

{@code
+     * SendableChooser chooser = TestDashboard.getParamChooser(this, "LedState");
+     * }
+ * + * @param The value type stored in the chooser + * @param test The test instance + * @param paramName The name of the param + * @return The SendableChooser, or null if not found + */ + @SuppressWarnings("unchecked") + public static SendableChooser getParamChooser(DiagnosticTest test, String paramName) { + return (SendableChooser) SmartDashboard.getData(getPrefix(test, DataType.PARAM) + paramName); + } + + /** + * Gets the selected value from a SendableChooser param. + * Handles null checking and returns the default value if the chooser or selection is null. + * + *

The type parameter {@code T} is inferred from the {@code defaultValue}. For example: + * 

{@code
+     * LedStates state = TestDashboard.getParamChooserSelected(this, "LedState", LedStates.None);
+     * }
+ * + * @param The value type stored in the chooser + * @param test The test instance + * @param paramName The name of the param + * @param defaultValue The default value to return if chooser is null or no selection + * @return The selected value, or defaultValue if not available + */ + public static T getParamChooserSelected(DiagnosticTest test, String paramName, T defaultValue) { + SendableChooser chooser = getParamChooser(test, paramName); + if (chooser != null && chooser.getSelected() != null) { + return chooser.getSelected(); + } + return defaultValue; + } + + // ============================================================================ + // Result Methods (Outputs - Write-Only) + // ============================================================================ + + /** + * Puts an integer result value to SmartDashboard. + * + * @param test The test instance + * @param resultName The name of the result (can include path separators, e.g., "Config/Module") + * @param value The value to set + */ + public static void putResultInt(DiagnosticTest test, String resultName, int value) { + SmartDashboard.putNumber(getPrefix(test, DataType.RESULT) + resultName, value); + } + + /** + * Puts a double result value to SmartDashboard. + * + * @param test The test instance + * @param resultName The name of the result (can include path separators, e.g., "Summary/TotalDrift") + * @param value The value to set + */ + public static void putResultDouble(DiagnosticTest test, String resultName, double value) { + SmartDashboard.putNumber(getPrefix(test, DataType.RESULT) + resultName, value); + } + + /** + * Puts a boolean result value to SmartDashboard. + * + * @param test The test instance + * @param resultName The name of the result (can include path separators) + * @param value The value to set + */ + public static void putResultBoolean(DiagnosticTest test, String resultName, boolean value) { + SmartDashboard.putBoolean(getPrefix(test, DataType.RESULT) + resultName, value); + } + + /** + * Puts a string result value to SmartDashboard. + * + * @param test The test instance + * @param resultName The name of the result (can include path separators, e.g., "Status/Message") + * @param value The value to set + */ + public static void putResultString(DiagnosticTest test, String resultName, String value) { + SmartDashboard.putString(getPrefix(test, DataType.RESULT) + resultName, value); + } +} diff --git a/src/main/java/frc/lib/test/TestRunnerCommand.java b/src/main/java/frc/lib/test/TestRunnerCommand.java new file mode 100644 index 0000000..cc29e7b --- /dev/null +++ b/src/main/java/frc/lib/test/TestRunnerCommand.java @@ -0,0 +1,118 @@ +package frc.lib.test; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import java.util.function.Supplier; + +/** + * Proxy command that bridges the Elastic dashboard button to test execution. + * + *

Published once via SmartDashboard.putData(), this command appears as a + * clickable button in Elastic (and SimGUI). When clicked, it schedules + * the currently selected test. When clicked again (or the test completes), + * the button resets. + * + *

This command has no subsystem requirements, so it runs concurrently + * with the actual test command without scheduling conflicts. + * + *

Lifecycle: + *

    + *
  • Button click ("Start Test"): Elastic sets {@code running=true} → + * CommandScheduler schedules this proxy → {@code initialize()} fires → + * supplier returns selected test → proxy schedules it → + * button label changes to "Cancel Test"
    • + *
  • Test completes naturally: Inner test's {@code isFinished()} returns true → + * scheduler ends it → proxy detects it's gone → proxy ends → + * button label reverts to "Start Test"
    • + *
  • Button click ("Cancel Test"): Elastic sets {@code running=false} → + * scheduler cancels the proxy → {@code end(true)} fires → + * proxy cancels inner test → button label reverts to "Start Test"
    • + *
  • No test selected: Supplier returns null → {@code isFinished()} returns + * true immediately → proxy ends → button resets
    • + *
+ * + *

Dynamic button label: Calls {@code setName()} in {@code initialize()} + * and {@code end()} to toggle the button label. This works because WPILib's + * {@code Command.initSendable()} registers the {@code .name} property with a getter + * that the {@code SendableBuilder} polls periodically — name changes propagate + * to NetworkTables automatically. + */ +public class TestRunnerCommand extends Command { + + private static final String LABEL_START = "Start Test"; + private static final String LABEL_CANCEL = "Cancel Test"; + + private Supplier selectedTestSupplier; + private Command runningTest; + + /** + * Creates a new TestRunnerCommand. + * + * @param selectedTestSupplier Supplier that returns the persistent test + * instance to run. Returns null if no test is selected or available. + * Called once per button click (in initialize()). + */ + public TestRunnerCommand(Supplier selectedTestSupplier) { + this.selectedTestSupplier = selectedTestSupplier; + setName(LABEL_START); // Initial button label shown in Elastic + } + + @Override + public void initialize() { + // Guard: only allow tests to run in Test mode. + // The TestRunnerCommand persists in NetworkTables after exiting test mode, + // so it can still be clicked from teleop/autonomous/disabled. Reject those. + if (!DriverStation.isTest() || selectedTestSupplier == null) { + if (!DriverStation.isTest()) { + System.out.println("Warning: Diagnostic tests can only run in Test mode. " + + "Ignoring Start Test request."); + } + runningTest = null; + return; // isFinished() returns true immediately → proxy ends → button resets + } + + runningTest = selectedTestSupplier.get(); + if (runningTest != null) { + CommandScheduler.getInstance().schedule(runningTest); + setName(LABEL_CANCEL); // Button label changes to "Cancel Test" + } + } + + @Override + public void execute() { + // Nothing — just waiting for the inner test to finish + } + + @Override + public boolean isFinished() { + // Finished when: no test was selected, or inner test is done + return runningTest == null + || !CommandScheduler.getInstance().isScheduled(runningTest); + } + + @Override + public void end(boolean interrupted) { + if (interrupted && runningTest != null + && CommandScheduler.getInstance().isScheduled(runningTest)) { + runningTest.cancel(); + } + runningTest = null; + setName(LABEL_START); // Button label reverts to "Start Test" + } + + /** + * Releases the supplier reference so the owning {@link DiagnosticTestManager} + * (and everything it points to) can be garbage collected. + * + *

This command remains registered in NetworkTables as a Sendable (no WPILib API + * to remove it), but with a null supplier any subsequent button click is a safe + * no-op — {@code initialize()} will set {@code runningTest = null} and the proxy + * ends immediately. + * + *

Called by {@link DiagnosticTestManager#cleanup()} when exiting test mode. + */ + public void cleanup() { + selectedTestSupplier = null; + } +} diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index 75dae1c..f563d08 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -4,7 +4,8 @@ package frc.robot.Auto; - +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Subsystems.SwerveSubsystem; @@ -19,7 +20,6 @@ public DriveTestAuto (SwerveSubsystem drive) { } } */ - public class DriveTestAuto extends SequentialCommandGroup { @@ -30,3 +30,4 @@ public DriveTestAuto (SwerveSubsystem drive){ ); } } + diff --git a/src/main/java/frc/robot/Auto/EightLemonAuto.java b/src/main/java/frc/robot/Auto/EightLemonAuto.java index 1391f58..5f99ce3 100644 --- a/src/main/java/frc/robot/Auto/EightLemonAuto.java +++ b/src/main/java/frc/robot/Auto/EightLemonAuto.java @@ -9,7 +9,7 @@ import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.SwerveSubsystem; -//With PATHPLANNER + public class EightLemonAuto extends SequentialCommandGroup { public EightLemonAuto (SwerveSubsystem drive, ShooterSubsystem shooter, IntakeSubsystem intake){ addCommands( diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e3fa22d..fb6c212 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -96,7 +96,16 @@ public static final class SwerveConstants{ public static final double angleConversionFactor = 360.0 / angleGearRatio; /* Swerve Profiling Values */ - public static final double maxSpeed = 3; // meters per second + public static final double maxSpeed = 3; // meters per second — software speed limit for teleop + + // Physical max speed at the wheel, derived from the motor's free speed through the gearbox. + // This is what the motor can physically achieve, NOT a software limit. + // Used by PathPlanner's ModuleConfig to model motor physics (torque, current, acceleration). + // freeSpeedRadPerSec (after gear reduction) × wheel radius + public static final double maxDriveVelocityMPS = + DCMotor.getNeoVortex(1).withReduction(driveGearRatio).freeSpeedRadPerSec + * (wheelDiameter / 2.0); + public static final double maxAngularVelocity = maxSpeed/driveBaseRadius; //radians per second how fast the robot spin /* Neutral Modes */ @@ -142,7 +151,7 @@ public static final class AutoConstants { private static boolean dashboardInitialized = false; public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(SwerveConstants.wheelDiameter/2, - SwerveConstants.maxSpeed, + SwerveConstants.maxDriveVelocityMPS, // physical max speed, NOT the software speed limit (maxSpeed) 1.2, DCMotor.getNeoVortex(1).withReduction(SwerveConstants.driveGearRatio), SwerveConstants.driveContinuousCurrentLimit, @@ -201,20 +210,7 @@ public static final class FieldConstants { public static final Translation2d HUB_CENTER = new Translation2d(4.61,4.03); - /** - * If true, the robot will behave as if it is always on the Blue alliance (no field mirroring), - * even when connected to FMS / Driver Station reports Red. - * - * WARNING: Enabling this for real matches while actually on Red will make autos/field-oriented - * behavior mirror incorrectly. - */ - public static final boolean FORCE_BLUE_ALLIANCE = true; - public static boolean isRedAlliance(){ - if (FORCE_BLUE_ALLIANCE) { - return false; - } - // Default to Blue when alliance is unknown (common in sim/practice). return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; } @@ -241,7 +237,7 @@ public static Pose2d flipForAlliance(Pose2d pose){ public static final class ShooterConstants { public static final int SHOOTER_ID = 70; //Placeholder ID public static final int FEEDER_ID = 61; //Feeder ID - public static final int HOOD_ID = 62; //Hood ID (NEED CHANGE) + public static final int HOOD_ID = 62; //Hood ID public static final double SHOOTER_SPEED = 0.5; //Placeholder speed public static final double FEEDER_SPEED = 0.5; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a9f81d2..1bb98f6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,23 +4,48 @@ package frc.robot; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; - - +import frc.robot.sim.SimulationManager; +import frc.lib.test.DiagnosticTestManager; +/** + * Main robot class that extends TimedRobot. This is the entry point for the robot program + * and manages the robot lifecycle across different modes (disabled, autonomous, teleop, test). + * + *

The Robot class: + *

    + *
  • Creates and initializes the RobotContainer which sets up subsystems and command bindings
    • + *
  • Runs the CommandScheduler every 20ms to execute active commands and check button bindings
    • + *
  • Handles mode transitions (autonomous, teleop, test) and manages command lifecycle
    • + *
+ */ public class Robot extends TimedRobot { private Command m_autonomousCommand; private final RobotContainer m_robotContainer; - private final RobotSimulation m_robotSimulation; + private DiagnosticTestManager m_testManager; + + // Simulation support + private SimulationManager simManager; + /** + * Constructs the Robot. Initializes the RobotContainer which creates subsystems + * (subsystems configure themselves) and sets up command bindings. + */ public Robot() { m_robotContainer = new RobotContainer(); - m_robotSimulation = new RobotSimulation(m_robotContainer); } + @Override + public void robotInit() { + } + /** + * Called every 20ms during all robot modes. Runs the CommandScheduler which + * executes active commands, checks button/trigger bindings, and updates subsystems. + */ @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); @@ -69,21 +94,47 @@ public void teleopExit() {} public void testInit() { // Cancel all commands when entering test mode. CommandScheduler.getInstance().cancelAll(); + // Initialize diagnostic test manager + m_testManager = new DiagnosticTestManager(m_robotContainer); } @Override - public void testPeriodic() {} + public void testPeriodic() { + // Update diagnostic test manager (handles test selection, execution, and status monitoring) + if (m_testManager != null) { + m_testManager.periodic(); + } + } @Override - public void testExit() {} + public void testExit() { + // Cleanup diagnostic test manager + if (m_testManager != null) { + m_testManager.cleanup(); + m_testManager = null; + } + } @Override public void simulationInit() { - m_robotSimulation.simulationInit(); + // Suppress joystick-not-found warnings in sim (no physical controller). + // Opt-in by adding the following to your ~/.gradle/init.gradle file: + // allprojects { + // tasks.withType(JavaExec) { jvmArgs '-Dsim.silenceJoystick=true' } + // } + if (Boolean.parseBoolean(System.getProperty("sim.silenceJoystick", "false"))) { + DriverStation.silenceJoystickConnectionWarning(true); + } + + // Initialize simulation manager for driver practice simulation + simManager = new SimulationManager(m_robotContainer.getSwerveSubsystem()); } @Override public void simulationPeriodic() { - m_robotSimulation.simulationPeriodic(); + // Run simulation manager (handles both normal simulation and API testing) + if (simManager != null) { + simManager.simulationPeriodic(); + } } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9a18dcb..326b43a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,13 +23,10 @@ import frc.robot.Constants.ShooterConstants; import frc.robot.Command.AutoAlign; import frc.robot.Command.TeleopSwerve; +import frc.robot.Subsystems.CandleSubsystem; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; public class RobotContainer { @@ -57,12 +54,23 @@ public class RobotContainer { //ShooterSubsystem for shooter private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + + //CandleSubsystem for LED control + private final CandleSubsystem m_candle = new CandleSubsystem(); + /** + * Constructs the RobotContainer. Creates subsystems (which configure themselves) + * and sets up command bindings to map controller inputs to commands. + */ public RobotContainer() { AutoConstants.initDashboard(); configureBindings(); } + /** + * Configures command bindings for controller inputs. + * Maps buttons and triggers to commands and sets the default drive command. + */ private void configureBindings() { // Y Button = Zero gyro (reset heading to 0° or 180° based on alliance) @@ -141,13 +149,23 @@ private void configureBindings() { ); } + /** + * Determines if the driver has requested speed reduction for precise positioning + * or delicate tasks. + * @return Speed multiplier + */ private double getSpeedMultiplier(){ // getHID() accesses the underlying XboxController to read button states directly. // CommandXboxController doesn't provide a method for stick button presses, so we use // the HID (Human Interface Device) object's getRawButton() method instead. return driveController.getHID().getRawButton(Button.kLeftStick.value)? 0.85: 1; } - + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ public Command getAutonomousCommand() { AutoConstants.AutoMode selected = AutoConstants.getSelectedAutoMode(); @@ -161,8 +179,30 @@ public Command getAutonomousCommand() { }; } - public SwerveSubsystem getDriveSubsystem() { + // ============================================================================ + // Simulation and Test Support Methods + // The following methods are provided for simulation and diagnostic test + // support. They are not used by production robot code. + // ============================================================================ + + /** + * Gets the swerve subsystem instance. + * Used for simulation and test code that needs access to the + * swerve subsystem. + * + * @return The SwerveSubsystem instance + */ + public SwerveSubsystem getSwerveSubsystem() { return m_drive; } + /** + * Gets the candle subsystem instance. + * Used for test code that needs access to the LED subsystem. + * + * @return The CandleSubsystem instance + */ + public CandleSubsystem getCandleSubsystem() { + return m_candle; + } } diff --git a/src/main/java/frc/robot/RobotSimulation.java b/src/main/java/frc/robot/RobotSimulation.java deleted file mode 100644 index de37084..0000000 --- a/src/main/java/frc/robot/RobotSimulation.java +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.SwerveConstants; -import frc.robot.Subsystems.SwerveSubsystem; - -/** - * Desktop simulation hooks. - * - *

This class is called from {@link Robot#simulationInit()} and {@link Robot#simulationPeriodic()} - * so the "WPILib: Simulate Robot Code" action in VS Code can run your robot with a simple physics - * model. - */ -public class RobotSimulation { - private final SwerveSubsystem drive; - private double lastTimestampSeconds = Timer.getFPGATimestamp(); - - public RobotSimulation(RobotContainer robotContainer) { - this.drive = robotContainer.getDriveSubsystem(); - } - - public void simulationInit() { - if (!RobotBase.isSimulation()) { - return; - } - - // Leave the robot disabled by default so the Sim GUI Driver Station can control mode - // (Disabled / Auto / Teleop). - DriverStationSim.setDsAttached(true); - DriverStationSim.setEnabled(false); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(false); - DriverStationSim.notifyNewData(); - drive.simulationReset(); - lastTimestampSeconds = Timer.getFPGATimestamp(); - } - - public void simulationPeriodic() { - if (!RobotBase.isSimulation()) { - return; - } - - final double now = Timer.getFPGATimestamp(); - final double dtSeconds = MathUtil.clamp(now - lastTimestampSeconds, 0.0, 0.05); - lastTimestampSeconds = now; - - drive.simulationUpdate(dtSeconds); - - var speeds = drive.getLastCommandedSpeeds(); - double driveFraction = - Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) / SwerveConstants.maxSpeed; - double rotateFraction = - Math.abs(speeds.omegaRadiansPerSecond) / SwerveConstants.maxAngularVelocity; - double estimatedCurrentAmps = 8.0 + 80.0 * MathUtil.clamp(driveFraction, 0.0, 1.0) - + 40.0 * MathUtil.clamp(rotateFraction, 0.0, 1.0); - - RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(estimatedCurrentAmps)); - } -} diff --git a/src/main/java/frc/robot/Subsystems/CandleSubsystem.java b/src/main/java/frc/robot/Subsystems/CandleSubsystem.java index 9e9f1de..e61902e 100644 --- a/src/main/java/frc/robot/Subsystems/CandleSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/CandleSubsystem.java @@ -28,12 +28,12 @@ public CandleSubsystem() { CANdleConfiguration configAll = new CANdleConfiguration(); //Builds a configuration preset for the lights // Apply default configuration using Phoenix 6 API configAll.LED.BrightnessScalar = 0.25; - + candle.getConfigurator().apply(configAll); //Implements our awesome settings using Phoenix 6 API // Initialize LEDs to off - LightConfig(AnimationType.Off, LED_START_INDEX, LED_END_INDEX, Colors.Black); + lightConfig(AnimationType.Off, LED_START_INDEX, LED_END_INDEX, Colors.Black); } @@ -83,7 +83,7 @@ public enum LedStates { //Some Color Presets - public void ChangeColor(Colors Color, int R, int G, int B, int W) { + public void changeColor(Colors Color, int R, int G, int B, int W) { switch(Color) { default: case Red: @@ -114,46 +114,46 @@ public void ChangeColor(Colors Color, int R, int G, int B, int W) { } - public void ChangeState(LedStates State) { - ClearAnimations(); - LightConfig(AnimationType.Off, LED_START_INDEX, LED_END_INDEX, Colors.Black); + public void changeState(LedStates State) { + clearAnimations(); + lightConfig(AnimationType.Off, LED_START_INDEX, LED_END_INDEX, Colors.Black); switch(State) { default: case None: break; case OutOfRange: - LightConfig(AnimationType.Solid, LED_START_INDEX, LED_END_INDEX, Colors.Red); + lightConfig(AnimationType.Solid, LED_START_INDEX, LED_END_INDEX, Colors.Red); break; case InRange: - LightConfig(AnimationType.Solid, 0, 20, Colors.Yellow); + lightConfig(AnimationType.Solid, 0, 20, Colors.Yellow); break; case Aligned: - LightConfig(AnimationType.Solid, 21, 40, Colors.Orange); + lightConfig(AnimationType.Solid, 21, 40, Colors.Orange); break; case ShooterUpToSpeed: - LightConfig(AnimationType.ColorFlow, 41, 60, Colors.Yellow); + lightConfig(AnimationType.ColorFlow, 41, 60, Colors.Yellow); break; case ReadyToShoot: - LightConfig(AnimationType.Solid, LED_START_INDEX, LED_END_INDEX, Colors.Green); + lightConfig(AnimationType.Solid, LED_START_INDEX, LED_END_INDEX, Colors.Green); break; case HopperFull: - LightConfig(AnimationType.ColorFlow, LED_START_INDEX, LED_END_INDEX, Colors.Blue); + lightConfig(AnimationType.ColorFlow, LED_START_INDEX, LED_END_INDEX, Colors.Blue); break; case InRangeAligned: - LightConfig(AnimationType.Solid, 0, 20, Colors.Yellow); - LightConfig(AnimationType.Solid, 21, 40, Colors.Orange); + lightConfig(AnimationType.Solid, 0, 20, Colors.Yellow); + lightConfig(AnimationType.Solid, 21, 40, Colors.Orange); break; case InRangeShooterSpeed: - LightConfig(AnimationType.Solid, 0, 20, Colors.Yellow); - LightConfig(AnimationType.ColorFlow, 41, 60, Colors.Yellow); + lightConfig(AnimationType.Solid, 0, 20, Colors.Yellow); + lightConfig(AnimationType.ColorFlow, 41, 60, Colors.Yellow); break; case ShooterSpeedAligned: - LightConfig(AnimationType.Solid, 21, 40, Colors.Orange); - LightConfig(AnimationType.ColorFlow, 41, 60, Colors.Yellow); + lightConfig(AnimationType.Solid, 21, 40, Colors.Orange); + lightConfig(AnimationType.ColorFlow, 41, 60, Colors.Yellow); break; } } - public void ClearAnimations() { + public void clearAnimations() { for (int i = 0; i < 8; ++i) { candle.setControl(new EmptyAnimation(i)); } @@ -162,9 +162,8 @@ public void ClearAnimations() { - public void LightConfig(AnimationType type, int kSlot1StartIdx, int kSlot1EndIdx, Colors NewColor) { - ChangeColor(NewColor, 0,0,0,0); - + public void lightConfig(AnimationType type, int kSlot1StartIdx, int kSlot1EndIdx, Colors NewColor) { + changeColor(NewColor, 0,0,0,0); switch (type) { default: case ColorFlow: @@ -178,13 +177,13 @@ public void LightConfig(AnimationType type, int kSlot1StartIdx, int kSlot1EndIdx ); break; case Off: - ChangeColor(Colors.Black, 0,0,0,0); + changeColor(Colors.Black, 0,0,0,0); candle.setControl( new SolidColor(kSlot1StartIdx + 7, kSlot1EndIdx + 7).withColor(LedColor) ); break; } - } + } } diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 9af8284..2f5ddca 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -19,7 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; -import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -43,12 +43,6 @@ public class SwerveSubsystem extends SubsystemBase { private SwerveModule[] mSwerveMods; private Field2d field; - private ChassisSpeeds lastCommandedSpeeds = new ChassisSpeeds(); - - private double simYawDegrees = 0.0; - private final double[] simWheelPositionsMeters = new double[4]; - private final Rotation2d[] simWheelAngles = - new Rotation2d[] {new Rotation2d(), new Rotation2d(), new Rotation2d(), new Rotation2d()}; private final StructArrayPublisher swerveDataPublisher = NetworkTableInstance.getDefault() @@ -56,6 +50,8 @@ public class SwerveSubsystem extends SubsystemBase { private final StructArrayPublisher desiredSwerveDataPublisher = NetworkTableInstance.getDefault() .getStructArrayTopic("Desired Swerve States", SwerveModuleState.struct).publish(); + private final StructPublisher robotPose = NetworkTableInstance.getDefault() + .getStructTopic("Robot Pose", Pose2d.struct).publish(); /** Creates a new SwerveSubsystem. */ public SwerveSubsystem() { @@ -81,25 +77,6 @@ public SwerveSubsystem() { configurePathPlanner(); } - public void simulationReset() { - if (!RobotBase.isSimulation()) { - return; - } - - simYawDegrees = getYaw().getDegrees(); - for (int i = 0; i < 4; i++) { - simWheelPositionsMeters[i] = 0.0; - simWheelAngles[i] = new Rotation2d(); - } - - pigeon.setYaw(simYawDegrees); - SwerveModulePosition[] positions = new SwerveModulePosition[4]; - for (int i = 0; i < 4; i++) { - positions[i] = new SwerveModulePosition(0.0, simWheelAngles[i]); - } - odometry.resetPosition(Rotation2d.fromDegrees(simYawDegrees), positions, new Pose2d()); - } - private void configurePathPlanner(){ AutoBuilder.configure(this::getPose, @@ -176,9 +153,8 @@ public void drive(double xInput, double yInput, double rotationInput, boolean is } driveFromChassisSpeeds(desiredSpeeds, true); } - + public void driveFromChassisSpeeds(ChassisSpeeds driveSpeeds, boolean isOpenLoop){ - lastCommandedSpeeds = driveSpeeds; SwerveModuleState[] desiredStates = SwerveConstants.swerveKinematics.toSwerveModuleStates(driveSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.maxSpeed); @@ -193,10 +169,6 @@ public ChassisSpeeds getChassisSpeeds(){ return SwerveConstants.swerveKinematics.toChassisSpeeds(getStates()); } - public ChassisSpeeds getLastCommandedSpeeds() { - return lastCommandedSpeeds; - } - public Pose2d getPose() { return odometry.getEstimatedPosition(); } @@ -249,7 +221,7 @@ public Rotation2d getYaw() { public void resyncModuleEncoders(){ if(!DriverStation.isDisabled()){ DriverStation.reportWarning - ("Attempted to resync swerve module encoders while robot is enabled. Disable before resyncing", + ("Attempted to resync swerve module encoders while robot is enabled. Disable before resyncing", false); //NEED CONFIRM return; } @@ -258,7 +230,7 @@ public void resyncModuleEncoders(){ } } - public void saveModuleOffsets(){ + public void saveModuleOffsets(){ saveModuleOffsets(new Rotation2d()); } public void saveModuleOffsets(Rotation2d desiredAngle){ @@ -273,16 +245,14 @@ public void saveModuleOffsets(Rotation2d desiredAngle){ } } - - @Override public void periodic() { - if (!RobotBase.isSimulation()) { - odometry.update(getYaw(), getPositions()); - updateOdometryWithVision("limelight-a"); - updateOdometryWithVision("limelight-b"); - } + odometry.update(getYaw(), getPositions()); + updateOdometryWithVision("limelight-a"); + updateOdometryWithVision("limelight-b"); field.setRobotPose(getPose()); + // required by AdvantageScope - to visualize the robot pose without "spinning" + robotPose.set(getPose()); SmartDashboard.putNumber("Pigeon Yaw", pigeon.getYaw().getValueAsDouble()); @@ -299,35 +269,72 @@ public void periodic() { SmartDashboard.putNumber( "Mod " + mod.moduleNumber + " New Cancoder Offset", canCoderDegrees < 0 ? 360 + canCoderDegrees : canCoderDegrees); + } + swerveDataPublisher.set(getStates()); } - swerveDataPublisher.set(getStates()); -} + + // ============================================================================ + // Simulation and Test Support Methods + // The following methods are provided for simulation and diagnostic test + // support. They are not used by production robot code. + // ============================================================================ /** - * Simple swerve simulation: integrates the last commanded chassis speeds into wheel positions and - * a yaw angle, then updates odometry from those simulated sensors. + * Gets the desired module states. Used by simulation to track robot motion. + * Reads desired states from each module (modules store their own desired state). + * @return Array of desired swerve module states */ - public void simulationUpdate(double dtSeconds) { - if (!RobotBase.isSimulation()) { - return; + public SwerveModuleState[] getDesiredStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (SwerveModule mod : mSwerveMods) { + states[mod.moduleNumber] = mod.getDesiredState(); } + return states; + } + + public Field2d getField() { + return field; + } - ChassisSpeeds speeds = DriverStation.isDisabled() ? new ChassisSpeeds() : lastCommandedSpeeds; + public Pigeon2 getPigeon() { + return pigeon; + } - simYawDegrees += Math.toDegrees(speeds.omegaRadiansPerSecond * dtSeconds); - pigeon.setYaw(simYawDegrees); + public SwerveDrivePoseEstimator getOdometry() { + return odometry; + } - SwerveModuleState[] states = SwerveConstants.swerveKinematics.toSwerveModuleStates(speeds); - SwerveDriveKinematics.desaturateWheelSpeeds(states, SwerveConstants.maxSpeed); + public SwerveDriveKinematics getKinematics() { + return Constants.SwerveConstants.swerveKinematics; + } - SwerveModulePosition[] positions = new SwerveModulePosition[4]; - for (int i = 0; i < 4; i++) { - simWheelPositionsMeters[i] += states[i].speedMetersPerSecond * dtSeconds; - simWheelAngles[i] = states[i].angle; - positions[i] = new SwerveModulePosition(simWheelPositionsMeters[i], simWheelAngles[i]); + /** + * Gets a specific swerve module by its module number. + * Required for diagnostic tests to access individual modules for testing and diagnostics. + * + * @param moduleNumber The module number (0-3) + * @return The SwerveModule instance, or null if moduleNumber is invalid + */ + public SwerveModule getModule(int moduleNumber) { + if (moduleNumber >= 0 && moduleNumber < mSwerveMods.length) { + return mSwerveMods[moduleNumber]; } - - odometry.update(Rotation2d.fromDegrees(simYawDegrees), positions); + return null; } + /** + * Gets all swerve modules as an array. + * + *

Returns a defensive copy to prevent modification of the subsystem's internal + * module array structure. The {@link SwerveModule} instances inside the returned + * array are the same objects (their state remains mutable, which is intended). + * + *

Java arrays are always mutable, so this method returns a copy to prevent + * callers from replacing array elements (e.g., {@code getModules()[0] = null}). + * + * @return A copy of the array containing all swerve modules + */ + public SwerveModule[] getModules() { + return mSwerveMods.clone(); + } } diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java index b8c1e35..7708a3e 100644 --- a/src/main/java/frc/robot/SwerveModule.java +++ b/src/main/java/frc/robot/SwerveModule.java @@ -45,6 +45,9 @@ public class SwerveModule { private final boolean driveInvert; private final boolean angleInvert; + // Store desired state for simulation access + private SwerveModuleState desiredState; + private SparkMax angleMotor; private SparkFlex driveMotor; @@ -62,6 +65,19 @@ public class SwerveModule { //creates a feedforward for the swerve drive. feedforward does 90% of the work, estimating stuff //PID fixes the error + /** + * Constructs a swerve module with the specified module number and configuration data. + * Initializes and configures the angle encoder (CANcoder), angle motor (SparkMax), + * and drive motor (SparkFlex) according to the provided constants. + * + * @param moduleNumber The module identifier (typically 0-3 for a 4-module swerve drive) + * @param moduleConstants ModuleData record containing: + * - driveMotorID: CAN ID of the drive motor (SparkFlex) + * - angleMotorID: CAN ID of the angle motor (SparkMax) + * - encoderID: CAN ID of the absolute angle encoder (CANcoder) + * - angleOffset: Calibration offset in degrees to align encoder zero with module zero + * - location: Physical position of the module relative to robot center (Translation2d) + */ public SwerveModule(int moduleNumber, ModuleData moduleConstants){ this.moduleNumber = moduleNumber; this.angleOffsetPreferenceKey = "Swerve/Module" + moduleNumber + "/AngleOffsetDegrees"; @@ -76,13 +92,26 @@ public SwerveModule(int moduleNumber, ModuleData moduleConstants){ Preferences.getDouble(angleOffsetPreferenceKey, moduleConstants.angleOffset()); angleOffset = Rotation2d.fromDegrees(normalizeDegrees(storedOffset)); - + /* Angle Encoder Configuration + * The CANcoder is an absolute encoder that provides the module's angle even after power loss. + * It's used to calibrate the integrated encoder on startup. + */ // Create CANcoder instance with the encoder CAN ID from module constants angleEncoder = new CANcoder(moduleConstants.encoderID()); // Apply default configuration to the CANcoder (factory reset to known state) angleEncoder.getConfigurator().apply(new CANcoderConfiguration()); + // Set update frequency to 1 Hz (once per second) for absolute position readings. + // The CANcoder (absolute encoder) is only used once during robot startup to calibrate + // the integrated encoder (see resetToAbsolute() in configAngleMotor()). During normal + // operation, getAngle() reads from the integrated encoder every 20ms loop cycle, not + // the CANcoder. A low CANcoder update frequency reduces CAN bus traffic since we only + // need the absolute position once at startup, not continuously. angleEncoder.getAbsolutePosition().setUpdateFrequency(1); + /* Angle Motor Configuration + * The angle motor rotates the swerve module to the desired orientation. + * It uses a SparkMax with integrated encoder for position control. + */ // Create SparkMax motor controller for angle rotation (brushless motor) angleMotor = new SparkMax(moduleConstants.angleMotorID(), MotorType.kBrushless); // Get the integrated encoder (relative encoder) from the motor controller @@ -92,6 +121,10 @@ public SwerveModule(int moduleNumber, ModuleData moduleConstants){ // Configure motor settings (current limits, PID, encoder conversion, etc.) configAngleMotor(); + /* Drive Motor Configuration + * The drive motor provides forward/backward motion for the swerve module. + * It uses a SparkFlex with integrated encoder for velocity control. + */ // Create SparkFlex motor controller for drive motion (brushless motor) driveMotor = new SparkFlex(moduleConstants.driveMotorID(), MotorType.kBrushless); // Get the integrated encoder (relative encoder) from the motor controller @@ -103,35 +136,138 @@ public SwerveModule(int moduleNumber, ModuleData moduleConstants){ // Initialize lastAngle to current module angle for optimization calculations lastAngle = getState().angle; + + // Initialize desired state to zero state - used for simulation + desiredState = new SwerveModuleState(0, new Rotation2d()); } + /** + * Sets the module to the desired state (speed and angle). + *

+ * This is the main method for controlling the swerve module. It optimizes the desired + * state to minimize rotation distance, then sets both the wheel angle and drive speed. + *

+ * IMPORTANT FOR SIMULATION/TESTING: This method stores the optimized state in + * the {@code desiredState} field, which is read by {@link frc.robot.sim.SimulationManager} + * to simulate robot motion. All control commands (driving, testing, autonomous) must + * flow through this method to ensure simulation works correctly. + *

+ * Control Flow: + *

    + *
  • Normal driving: TeleopSwerve → SwerveSubsystem.drive() → this method
    • + *
  • Test commands: TestCommand → this method (directly)
    • + *
  • Autonomous: Auto command → SwerveSubsystem → this method
    • + *
+ *

+ * Note: Simulation automatically works for all the above control flows since + * {@link frc.robot.sim.SimulationManager} reads the stored desired state from this method. + * + * @param desiredState The target module state (speed in m/s and wheel angle) + * @param isOpenLoop If true, uses open loop control for drive motor; if false, uses closed loop velocity control + */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { // Optimize the desired state to minimize rotation (flip wheel 180° if needed) SwerveModuleState optimizedState = optimize(desiredState, getAngle()); + // Store desired state for simulation access + this.desiredState = optimizedState; // Set the wheel angle to the optimized direction setAngle(optimizedState); // Set the drive motor speed (open loop or closed loop based on parameter) setSpeed(optimizedState, isOpenLoop); } + + /** + * Gets the current state of the swerve module. + * @return SwerveModuleState containing the current drive velocity (in meters per second) + * and module angle (Rotation2d) + */ public SwerveModuleState getState(){ return new SwerveModuleState(driveEncoder.getVelocity(), getAngle()); } + + /** + * Gets the current position of the swerve module. + * @return SwerveModulePosition containing the current drive encoder position (in meters) + * and module angle (Rotation2d) + */ public SwerveModulePosition getPosition(){ return new SwerveModulePosition(driveEncoder.getPosition(), getAngle()); - } + } + + /** + * Gets the raw drive encoder position in encoder units (not converted to meters). + * @return Raw encoder position value from the drive motor's integrated encoder + */ public double getRawDriveEncoder(){ return driveEncoder.getPosition(); } + + /** + * Gets the raw turn encoder position in encoder units (not converted to degrees). + * @return Raw encoder position value from the angle motor's integrated encoder + */ public double getRawTurnEncoder(){ return integratedAngleEncoder.getPosition(); } + + /** + * Gets the current **absolute encoder** (CANcoder) position. + *

+ * The CANcoder is an **absolute encoder** that retains its position even after power loss. + * This method reads the raw absolute position from the CANcoder and converts it to a + * Rotation2d representing the module's wheel angle. + *

+ * Used primarily during module initialization in {@link #resetToAbsolute()} to calibrate + * the integrated encoder. Also used for debugging/logging to display the absolute encoder + * value on SmartDashboard for diagnostics. + * + * @return The current absolute encoder position as a Rotation2d + */ public Rotation2d getCanCoder(){ return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValue().in(Units.Rotations)); } + + /** + * Checks if encoder data from both motors is valid (no errors). + *

+ * Encoder errors can occur when: + *

    + *
  • CAN bus communication fails (disconnected cable, CAN bus overload, electrical interference)
    • + *
  • Motor controller configuration errors (invalid parameters, failed configuration write)
    • + *
  • Encoder hardware failure (damaged encoder, loose connections, sensor malfunction)
    • + *
  • Motor controller fault conditions (overcurrent, overvoltage, thermal shutdown)
    • + *
+ *

+ * When errors are detected, consider: + *

    + *
  • Logging the error to SmartDashboard or Driver Station for diagnostics
    • + *
  • Disabling the affected module to prevent unpredictable behavior
    • + *
  • Using fallback behavior (e.g., last known good encoder value, or disabling that module)
    • + *
  • Attempting recovery (re-initialization, reconfiguration, or recalibration)
    • + *
+ *

+ * Note: This method is currently not called anywhere in the codebase. Consider adding + * periodic error checking in {@code SwerveSubsystem.periodic()} to monitor module health. + * + * @return true if both drive motor and angle motor have no errors, false otherwise + */ public boolean isEncoderDataValid(){ return driveMotor.getLastError() == REVLibError.kOk && angleMotor.getLastError() == REVLibError.kOk; } + /** + * Optimizes the desired module state to minimize rotation distance. + *

+ * Swerve modules can achieve the same direction of travel by rotating the wheel + * 180 degrees and reversing the drive speed. This method checks if the required + * rotation is greater than 90 degrees, and if so, flips the wheel direction + * and reverses speed to reduce the rotation needed. This minimizes wear and + * improves response time. + * + * @param desiredState The target module state (speed and angle) + * @param currentAngle The current module wheel angle + * @return Optimized module state that achieves the same direction with minimal rotation + */ private SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle){ // Calculate the angular difference between desired and current angle double difference = desiredState.angle.getDegrees() - currentAngle.getDegrees(); @@ -157,12 +293,32 @@ private SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d cu return new SwerveModuleState (speed, Rotation2d.fromDegrees(direction)); } + /** + * Sets the drive motor speed to achieve the desired velocity. + *

+ * This is a private helper method used by {@link #setDesiredState(SwerveModuleState, boolean)}. + * Use {@code setDesiredState()} to control the module - do not call this method directly. + *

+ * Supports two control modes: + *

    + *
  • Open loop: Direct percent output control (no feedback, less accurate)
    • + *
  • Closed loop: Velocity control with PID and feedforward (uses encoder feedback, more accurate)
    • + *
+ * + * @param desiredState The target module state containing the desired speed in meters per second + * @param isOpenLoop If true, uses open loop control; if false, uses closed loop velocity control + */ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){ if (isOpenLoop) { + // Open loop: Convert desired speed to percent output (-1.0 to 1.0) + // No encoder feedback - motor runs at fixed percentage regardless of actual speed double percentOutput = desiredState.speedMetersPerSecond / Constants.SwerveConstants.maxSpeed; driveMotor.set(percentOutput); } else{ + // Closed loop: Use PID controller with feedforward for accurate velocity control + // Feedforward estimates motor output needed for desired speed (90% of work) + // PID controller corrects for any error between desired and actual speed driveController.setReference( desiredState.speedMetersPerSecond, ControlType.kVelocity, @@ -171,6 +327,18 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){ } } + /** + * Sets the wheel angle to the desired direction. + *

+ * This is a private helper method used by {@link #setDesiredState(SwerveModuleState, boolean)}. + * Use {@code setDesiredState()} to control the module - do not call this method directly. + *

+ * When the robot is moving very slowly (≤1% of max speed), the wheel angle is kept + * at the last position to prevent unnecessary rotation and reduce wear. When moving + * at significant speed, the wheel rotates to the desired angle. + * + * @param desiredState The target module state containing the desired wheel angle + */ private void setAngle(SwerveModuleState desiredState){ // If speed is very low (≤1% of max), keep last angle to avoid unnecessary rotation // This prevents jittery behavior and reduces wear when robot is barely moving @@ -182,17 +350,45 @@ private void setAngle(SwerveModuleState desiredState){ lastAngle = angle; } - + /** + * Gets the current wheel angle from the **integrated encoder**. + *

+ * This method reads the angle motor's integrated encoder position and converts it + * to a Rotation2d representing the current wheel orientation. + * + * @return The current wheel angle as a Rotation2d + */ private Rotation2d getAngle(){ return Rotation2d.fromDegrees(integratedAngleEncoder.getPosition()); } - + /** + * Points the wheel in a specific direction without changing drive speed. + *

+ * This method rotates the wheel to the specified angle (in degrees) while keeping + * the drive motor stopped. Useful for testing, calibration, or positioning the wheel + * without moving the robot. + *

+ * This method internally uses {@link #setDesiredState(SwerveModuleState, boolean)} + * to ensure simulation and test code can track the commanded state. This maintains + * consistency with the simulation architecture where all module commands flow through + * {@code setDesiredState()}. + * + * @param degrees The target wheel angle in degrees (0-360) + */ public void pointInDirection(double degrees){ - angleController.setReference(degrees, ControlType.kPosition); - lastAngle = Rotation2d.fromDegrees(degrees); + // Use setDesiredState to maintain consistency with simulation + // Speed = 0.0 (wheel doesn't drive), angle = desired direction, closed loop control + setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(degrees)), false); } + /** + * Configures the angle motor (SparkMax) with all necessary settings for position control. + * Called once during module initialization in the constructor. Configures current limits, + * motor inversion, brake mode, encoder conversion factors, PID values, and voltage + * compensation. After configuration, calibrates the integrated encoder to the absolute + * encoder (CANcoder) position. + */ private void configAngleMotor(){ SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); // Factory reset is commented out - only needed if motor needs to be reset to defaults @@ -223,6 +419,18 @@ private void configAngleMotor(){ resetToAbsolute(); } + /** + * Calibrates the integrated encoder to match the absolute encoder (CANcoder) position. + *

+ * This method reads the absolute encoder position, subtracts the calibration offset + * (angleOffset), and sets the integrated encoder to this value. This ensures the + * integrated encoder starts at the correct position even after power loss, since the + * absolute encoder retains its position while the integrated encoder resets to zero. + *

+ * Called once during module initialization in {@link #configAngleMotor()} after motor + * configuration is complete. This establishes the starting position for the integrated + * encoder, which is then used for all subsequent angle readings during normal operation. + */ private void resetToAbsolute() { double absolutePosition = getCanCoder().getDegrees() - angleOffset.getDegrees(); integratedAngleEncoder.setPosition(absolutePosition); //may need to change @@ -234,10 +442,18 @@ public void resyncToAbsolute(){ resetToAbsolute(); } + /*Saves the current CANcoder reading as the zero reference for this module. + Run this while the wheels are physically pointing straight to capture the + correct offset and persist it in WPILib Preferences. + */ public void saveCanCoderZero(){ saveCanCoderOffset(Rotation2d.fromDegrees(0.0)); } + /** + * Saves the current CANcoder reading as a reference for a desired heading. + * @param desiredAngle The field-relative angle that the wheel is currently aiming at. + */ public void saveCanCoderOffset(Rotation2d desiredAngle){ double absolute = getCanCoder().getDegrees(); double newOffset = normalizeDegrees(absolute - desiredAngle.getDegrees()); @@ -254,6 +470,12 @@ private double normalizeDegrees(double degrees){ return normalized; } + /** + * Configures the drive motor (SparkFlex) with all necessary settings for velocity control. + * Called once during module initialization in the constructor. Configures current limits, + * motor inversion, brake mode, encoder conversion factors, PID values, and voltage + * compensation. After configuration, resets the drive encoder position to zero. + */ private void configDriveMotor(){ SparkFlexConfig sparkFlexConfig = new SparkFlexConfig(); // Factory reset is commented out - only needed if motor needs to be reset to defaults @@ -284,4 +506,56 @@ private void configDriveMotor(){ // Reset encoder position to zero (sets starting position for odometry) driveEncoder.setPosition(0.0); } + + // ============================================================================ + // Simulation and Test Support Methods + // The following methods are provided for simulation and diagnostic test + // support. They are not used by production robot code. + // ============================================================================ + + /** + * Gets the CANcoder for simulation access. + * @return The CANcoder instance + */ + public CANcoder getCanCoderDevice() { + return angleEncoder; + } + + /** + * Gets the drive encoder for simulation access. + * @return The drive encoder (RelativeEncoder from SparkFlex) + */ + public RelativeEncoder getDriveEncoder() { + return driveEncoder; + } + + /** + * Gets the angle encoder for simulation access. + * @return The angle encoder (RelativeEncoder from SparkMax) + */ + public RelativeEncoder getAngleEncoder() { + return integratedAngleEncoder; + } + + /** + * Gets the desired state of the swerve module (what it's trying to achieve). + * Used by simulation to track commanded module states. + * @return The desired SwerveModuleState + */ + public SwerveModuleState getDesiredState() { + return desiredState; + } + + /** + * Gets the angle offset used to calibrate this module. + *

+ * This is the offset subtracted from the CANcoder reading to determine the module's + * zero position. It may come from Constants.java or from a stored Preference. + * Useful for diagnostic tests to verify encoder calibration. + * + * @return The angle offset as a Rotation2d + */ + public Rotation2d getAngleOffset() { + return angleOffset; + } } diff --git a/src/main/java/frc/robot/sim/SimulationManager.java b/src/main/java/frc/robot/sim/SimulationManager.java new file mode 100644 index 0000000..300d6ea --- /dev/null +++ b/src/main/java/frc/robot/sim/SimulationManager.java @@ -0,0 +1,151 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.sim; + +import com.ctre.phoenix6.sim.CANcoderSimState; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.SwerveModule; +import frc.robot.Subsystems.SwerveSubsystem; + +/** + * A self-contained simulation manager for driver practice. + * Updates Field2d and fakes gyro/encoder readings for swerve robots. + * Completely optional — can be removed without touching your real code. + */ +public class SimulationManager { + + private final SwerveSubsystem swerveSubsystem; + + // Internal simulated pose + private Pose2d simPose = new Pose2d(); + private double lastTime = 0; + + // Disabled-state tracking: used to detect the enabled→disabled transition + // so we can zero out stale module desired states once (edge-triggered). + // Starts true because the robot boots into disabled mode. + private boolean wasDisabled = true; + + /** + * Creates a new SimulationManager. + * @param swerveSubsystem The swerve subsystem to simulate + */ + public SimulationManager(SwerveSubsystem swerveSubsystem) { + this.swerveSubsystem = swerveSubsystem; + this.lastTime = Timer.getFPGATimestamp(); + } + + /** + * Call this in Robot.java simulationPeriodic(). + * Updates simulated sensors based on module states and integrates robot motion. + */ + public void simulationPeriodic() { + // Calculate time delta for physics integration (distance = velocity × time) + // This enables frame-rate independent motion simulation + double currentTime = Timer.getFPGATimestamp(); + double dt = currentTime - lastTime; + lastTime = currentTime; + + // Clamp dt to [0, 50ms]. Zero or negative dt produces no motion (correct). + // 50ms upper bound (2.5× the nominal 20ms loop) limits pose jumps during + // GC pauses or debugger breakpoints — the robot loses time rather than teleporting. + dt = MathUtil.clamp(dt, 0.0, 0.05); + + // Disabled-state guard: when the robot transitions from enabled to disabled, + // the CommandScheduler stops running commands but each module's desiredState + // field retains its last commanded velocity. Without this guard, SimulationManager + // would keep integrating those stale speeds, causing the simulated robot to drift. + // We clear once on the transition edge — zeroing drive speed while preserving + // wheel angles (realistic: wheels stop spinning but hold their orientation). + boolean isDisabled = DriverStation.isDisabled(); + if (isDisabled && !wasDisabled) { + for (SwerveModule module : swerveSubsystem.getModules()) { + Rotation2d currentAngle = module.getDesiredState().angle; + module.setDesiredState(new SwerveModuleState(0.0, currentAngle), false); + } + } + wasDisabled = isDisabled; + + // Step 1: Get desired module states (from normal driving or individual module commands) + SwerveModuleState[] desiredStates = swerveSubsystem.getDesiredStates(); + + // Convert to chassis speeds for robot pose/gyro updates + // In test mode with single module, this will be zero (correct - robot doesn't move) + ChassisSpeeds desiredChassisSpeeds = (desiredStates != null && desiredStates.length == 4) + ? swerveSubsystem.getKinematics().toChassisSpeeds(desiredStates) + : new ChassisSpeeds(); + + // Step 2: Update robot pose by adding distance traveled (velocity × time) to current position + // Chassis speeds represent overall robot motion (one body moving as a unit) + // Motors don't actually move in sim, so we use desired speeds for simulation + simPose = simPose.exp(new Twist2d( + desiredChassisSpeeds.vxMetersPerSecond * dt, // X distance = X velocity × time + desiredChassisSpeeds.vyMetersPerSecond * dt, // Y distance = Y velocity × time + desiredChassisSpeeds.omegaRadiansPerSecond * dt // Angular distance = angular velocity × time + )); + + // Step 3: Update simulated Pigeon2 gyro + swerveSubsystem.getPigeon().getSimState().setRawYaw(simPose.getRotation().getDegrees()); + + // Step 4: Update simulated module encoders + // Use desired states directly (not chassis speeds) to handle individual module commands + updateModuleEncoders(desiredStates, dt); + + // Note: Odometry and Field2d are updated by SwerveSubsystem.periodic(), which runs + // automatically for both real robot and simulation. No need to update them here! + // The periodic() method reads the sensors we just simulated (gyro, encoders) and + // updates odometry and Field2d accordingly. + } + + /** + * Updates simulated module encoders based on desired module states. + * For each module, updates encoder positions to match the desired states. + * This handles both normal driving (all modules coordinated) and test mode (individual module control). + */ + private void updateModuleEncoders(SwerveModuleState[] desiredStates, double dt) { + SwerveModule[] modules = swerveSubsystem.getModules(); + for (int i = 0; i < modules.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState desiredState = desiredStates[i]; + + if (desiredState == null) { + continue; + } + + // Update drive encoder position: distance = velocity × time + // * The encoder position represents distance traveled along the ground (in meters) + // Each wheel can have a different speed (e.g., outside wheels move faster when turning) + // * Wheel's speed * elapsed time = wheel's distance traveled + RelativeEncoder driveEncoder = module.getDriveEncoder(); + double currentPosition = driveEncoder.getPosition(); // Current distance in meters + double deltaMeters = desiredState.speedMetersPerSecond * dt; // Distance traveled = wheel speed × time + double newPosition = currentPosition + deltaMeters; // Total distance traveled + + // Update drive encoder in simulation + // REV encoders: use setPosition() directly (works in simulation) + driveEncoder.setPosition(newPosition); + + // Update angle encoder position (module rotation) + // The angle encoder position is in degrees (due to conversion factor) + RelativeEncoder angleEncoder = module.getAngleEncoder(); + double desiredAngleDegrees = desiredState.angle.getDegrees(); + + // Update angle encoder to match desired angle + angleEncoder.setPosition(desiredAngleDegrees); + + // Update CANcoder simulation (absolute encoder on the steering axis) + // Wheel steering angle expressed in rotations (e.g. 0.25 = 90°, 0.5 = 180°) + CANcoderSimState cancoderSimState = module.getCanCoderDevice().getSimState(); + cancoderSimState.setRawPosition(desiredState.angle.getRotations()); + } + } +} diff --git a/src/main/java/frc/robot/test/LedStateTestCommand.java b/src/main/java/frc/robot/test/LedStateTestCommand.java new file mode 100644 index 0000000..0423c47 --- /dev/null +++ b/src/main/java/frc/robot/test/LedStateTestCommand.java @@ -0,0 +1,146 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.test; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.SendableChooserUtil; +import frc.lib.test.DiagnosticTest; +import frc.lib.test.TestDashboard; +import frc.robot.Subsystems.CandleSubsystem; + +/** + * Diagnostic test for the CandleSubsystem LED states. + * + *

This test allows developers to independently test the CandleSubsystem by setting + * any LED state for a specified duration. This is useful for: + *

    + *
  • Verifying LED hardware functionality
    • + *
  • Testing LED state configurations without other robot systems
    • + *
  • Validating visual feedback during development
    • + *
  • Debugging LED-related issues
    • + *
+ * + *

The test sets the selected LED state, runs for the specified duration, then + * automatically turns the LEDs off. This provides a safe, isolated way to test + * the CandleSubsystem independent of other robot subsystems. + */ +public class LedStateTestCommand extends Command implements DiagnosticTest { + + private final CandleSubsystem candleSubsystem; + private SendableChooser ledStateChooser; + + // Test parameters (read from SmartDashboard in initialize()) + private CandleSubsystem.LedStates selectedLedState; + private double duration; + + // Test state + private double startTime; + private double actualDuration; + + /** + * Creates a new LED State Test command. + * + * @param candleSubsystem The candle subsystem to control + */ + public LedStateTestCommand(CandleSubsystem candleSubsystem) { + this.candleSubsystem = candleSubsystem; + + addRequirements(candleSubsystem); + } + + @Override + public String getTestName() { + return "LED State Test"; + } + + @Override + public String getTestDescription() { + return "Tests CandleSubsystem LED states independently. Sets the selected LED state for a " + + "specified duration, then turns LEDs off. Useful for verifying LED hardware functionality " + + "and testing visual feedback without other robot systems."; + } + + @Override + public void initializeParameters() { + // Set up duration parameter first + TestDashboard.putParamDouble(this, "Duration", 3.0); + + // Set up SendableChooser dropdown for LedStates enum — stores enum values directly + ledStateChooser = SendableChooserUtil.fromEnum(CandleSubsystem.LedStates.class); + TestDashboard.putParamChooser(this, "LedState", ledStateChooser); + } + + @Override + public void initialize() { + // Read parameters from SmartDashboard + // Note: We retrieve the chooser from SmartDashboard because initializeParameters() + // was called on a different (throwaway) instance. The chooser on SmartDashboard + // contains the user's selection. + // The chooser stores enum values directly — no string-to-enum conversion needed. + selectedLedState = TestDashboard.getParamChooserSelected( + this, + "LedState", + CandleSubsystem.LedStates.None + ); + + duration = TestDashboard.getParamDouble(this, "Duration", 3.0); + + // Validate duration + if (duration <= 0) { + System.err.println("Warning: Duration must be positive. Using default 3.0 seconds."); + duration = 3.0; + } + + // Initialize test state + startTime = Timer.getFPGATimestamp(); + actualDuration = 0.0; + + // Set LED state + candleSubsystem.changeState(selectedLedState); + + // Initialize result display + TestDashboard.putResultString(this, "Status", "Running"); + TestDashboard.putResultString(this, "SelectedState", selectedLedState.name()); + TestDashboard.putResultDouble(this, "TargetDuration", duration); + + System.out.println("LED State Test started: State=" + selectedLedState.name() + ", Duration=" + duration + "s"); + } + + @Override + public void execute() { + // Test just waits - LED state is already set in initialize() + // Update actual duration for display + actualDuration = Timer.getFPGATimestamp() - startTime; + TestDashboard.putResultDouble(this, "ElapsedTime", actualDuration); + } + + @Override + public boolean isFinished() { + // Test completes when duration has elapsed + return (Timer.getFPGATimestamp() - startTime) >= duration; + } + + @Override + public void end(boolean interrupted) { + // Calculate actual duration + actualDuration = Timer.getFPGATimestamp() - startTime; + + // Turn LEDs off + candleSubsystem.changeState(CandleSubsystem.LedStates.None); + + // Update result display + if (interrupted) { + TestDashboard.putResultString(this, "Status", "Interrupted"); + System.out.println("LED State Test interrupted after " + String.format("%.2f", actualDuration) + "s"); + } else { + TestDashboard.putResultString(this, "Status", "Complete"); + System.out.println("LED State Test completed: Ran for " + String.format("%.2f", actualDuration) + "s"); + } + + TestDashboard.putResultDouble(this, "ActualDuration", actualDuration); + } +} diff --git a/src/main/java/frc/robot/test/OdometryResetTestCommand.java b/src/main/java/frc/robot/test/OdometryResetTestCommand.java new file mode 100644 index 0000000..7bca33a --- /dev/null +++ b/src/main/java/frc/robot/test/OdometryResetTestCommand.java @@ -0,0 +1,911 @@ +package frc.robot.test; + +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.test.DiagnosticTest; +import frc.lib.test.TestDashboard; +import frc.robot.Subsystems.SwerveSubsystem; + +/** + * Diagnostic test that demonstrates the odometry reset bug (issue 8.1/8.2) + * and quantifies its impact on closed-loop path following. + * + *

Phase 1 — Reset & Observe: Resets the robot pose to + * {@code (targetX, targetY, targetHeading)} using the same buggy code as + * {@code startAutoAt}, then observes whether the odometry heading is corrupted + * on subsequent cycles due to pigeon setYaw latency and stale SimulationManager + * pose. + * + *

Phase 2 — Closed-Loop L-Path: Resets the robot pose to a + * different heading ({@code driveHeading}) at the same position, then + * drives an L-shaped path using a simple proportional controller. Because the + * pigeon settled at {@code targetHeading} during Phase 1, the Phase 2 reset to + * {@code driveHeading} triggers genuine pigeon latency (e.g., 90° → 0°), + * causing heading corruption on the first drive cycle. + * + *

L-Path geometry (computed from driveHeading): + * 

+ *   Start → (targetX, targetY, driveHeading)
+ *     │
+ *     │ Leg 1: 1.0m "forward" (in direction of driveHeading)
+ *     ▼
+ *   WP1 → (same heading)
+ *     │
+ *     │ Leg 2: 1.0m "right" + 90° CW turn
+ *     ▼
+ *   WP2 → (driveHeading − 90°)
+ *
+ * + *

With default parameters (target=90°, drive=0°, position 5.0, 4.0): + * 

+ *   Phase 1: Reset to heading 90°, observe. Pigeon settles at 90°.
+ *   Phase 2: Reset to heading 0° (pigeon: 90° → 0° = real latency)
+ *
+ *   Start (5.0, 4.0) heading 0° (+X)
+ *     │
+ *     │ Leg 1: drive +X to (6.0, 4.0)
+ *     ▼
+ *   WP1 (6.0, 4.0) heading 0°
+ *     │
+ *     │ Leg 2: turn to −90°, drive −Y to (6.0, 3.0)
+ *     ▼
+ *   WP2 (6.0, 3.0) heading −90°
+ *
+ * + *

Multi-trial: The entire Phase 1 + Phase 2 sequence repeats for + * {@code NumberOfTrials} trials (default 10). After each drive phase, the pigeon + * is at {@code driveHeading}, so the next trial's Phase 1 reset to + * {@code targetHeading} is a genuine heading change with real pigeon latency. + * A summary table at the end shows per-trial corruption and deviation statistics. + * + * @see Section 8.1 and 8.2 + */ +public class OdometryResetTestCommand extends Command implements DiagnosticTest { + + // ============================================================================ + // Phase Management + // ============================================================================ + + private enum Phase { + OBSERVING, // Post-reset heading observation (N cycles) + DRIVING_LEG1, // Drive to waypoint 1 (forward) + DRIVING_LEG2, // Drive to waypoint 2 (turn + forward) + STOPPING, // Stop the robot + COMPLETE + } + + // ============================================================================ + // Tuning Constants + // ============================================================================ + + // Closed-loop proportional control + private static final double DRIVE_KP = 2.0; // position P gain (m/s per m error) + private static final double HEADING_KP = 3.0; // heading P gain (rad/s per rad error) + private static final double MAX_DRIVE_SPEED = 1.0; // m/s speed cap + private static final double MAX_ROT_SPEED = 2.0; // rad/s rotation cap + + // Waypoint arrival thresholds + private static final double POSITION_TOLERANCE = 0.05; // meters + private static final double HEADING_TOLERANCE_DEG = 3.0; // degrees + private static final double SETTLE_SPEED = 0.05; // m/s — robot must be nearly stopped + + // Timeouts and limits + private static final double LEG_TIMEOUT_SEC = 5.0; + private static final double LEG_LENGTH = 1.0; // meters per leg + private static final int MAX_OBSERVATION_CYCLES = 20; + private static final double HEADING_CORRUPTION_THRESHOLD = 5.0; // degrees + + // Drive sample storage + private static final int MAX_DRIVE_SAMPLES = 600; + + // Console output: print every Nth drive cycle (plus first, last, and anomalies) + private static final int DRIVE_PRINT_INTERVAL = 10; + private static final double DEVIATION_ALERT_THRESHOLD = 0.1; // meters + + // ============================================================================ + // Instance Fields + // ============================================================================ + + private final SwerveSubsystem swerveSubsystem; + + // Test parameters (read from SmartDashboard in initialize()) + private double targetX; + private double targetY; + private double targetHeadingDeg; + private double driveHeadingDeg; + private int observationCycles; + private boolean includeDrivePhase; + + // Phase management + private Phase currentPhase; + private double legStartTime; + + // Waypoints (computed from drive heading) + private Pose2d startPose; // Phase 1 reset pose (targetHeading) + private Pose2d drivePose; // Phase 2 reset pose (driveHeading) + private Pose2d wp1; + private Pose2d wp2; + + // --- Phase 1: Reset observation data --- + private int obsCycleCount; + private double pigeonYawBeforeReset; + private double odometryHeadingBeforeReset; + private double pigeonYawAfterReset; + private double odometryHeadingAfterReset; + private double[] obsPigeonYaw; + private double[] obsOdoHeading; + private double[] obsOdoX; + private double[] obsOdoY; + + // --- Phase 2: Drive sample data --- + private int driveSampleCount; + private int[] driveLeg; // 1 = leg1, 2 = leg2 + private double[] driveTime; // elapsed time since drive start + private double[] drivePoseX; + private double[] drivePoseY; + private double[] drivePoseHeading; + private double[] drivePigeonYaw; + private double[] driveCmdVx; // commanded field-relative vX + private double[] driveCmdVy; // commanded field-relative vY + private double[] driveCmdOmega; // commanded omega + private double[] driveLateralDev; // perpendicular distance from ideal path + + // Drive phase tracking + private double driveStartTime; + private boolean leg1Complete; + private double leg1FinalX, leg1FinalY, leg1FinalHeading; + private double leg1MaxDev; + private int leg1Cycles; + + // Phase 2 reset tracking (reset from targetHeading → driveHeading) + private double phase2PigeonYawBeforeReset; + private double phase2OdoHeadingBeforeReset; + private double phase2PigeonYawAfterReset; + private double phase2OdoHeadingAfterReset; + + // --- Multi-trial tracking --- + private int numberOfTrials; + private int currentTrial; // 0-based + private int[] trialCorruptedCycles; + private double[] trialMaxHeadingError; + private boolean[] trialPigeonHadLatency; + private double[] trialMaxLateralDev; + private double[] trialFinalPosError; + private double[] trialFinalHeadingError; + + // ============================================================================ + // Constructor + // ============================================================================ + + public OdometryResetTestCommand(SwerveSubsystem swerveSubsystem) { + this.swerveSubsystem = swerveSubsystem; + addRequirements(swerveSubsystem); + } + + // ============================================================================ + // DiagnosticTest Interface + // ============================================================================ + + @Override + public String getTestName() { + return "Odometry Reset Test"; + } + + @Override + public String getTestDescription() { + return "Multi-trial test: Phase 1 resets to TargetHeading and observes heading corruption. " + + "Phase 2 resets to DriveHeading and drives a closed-loop L-path. " + + "Repeats for NumberOfTrials trials to measure consistency."; + } + + @Override + public void initializeParameters() { + TestDashboard.putParamDouble(this, "TargetX", 5.0); + TestDashboard.putParamDouble(this, "TargetY", 4.0); + TestDashboard.putParamDouble(this, "TargetHeading", 90.0); + TestDashboard.putParamDouble(this, "DriveHeading", 0.0); + TestDashboard.putParamInt(this, "ObservationCycles", 10); + TestDashboard.putParamBoolean(this, "IncludeDrivePhase", true); + TestDashboard.putParamInt(this, "NumberOfTrials", 10); + } + + // ============================================================================ + // Command Lifecycle + // ============================================================================ + + @Override + public void initialize() { + // Read parameters + targetX = TestDashboard.getParamDouble(this, "TargetX", 5.0); + targetY = TestDashboard.getParamDouble(this, "TargetY", 4.0); + targetHeadingDeg = TestDashboard.getParamDouble(this, "TargetHeading", 90.0); + driveHeadingDeg = TestDashboard.getParamDouble(this, "DriveHeading", 0.0); + observationCycles = Math.min( + TestDashboard.getParamInt(this, "ObservationCycles", 10), + MAX_OBSERVATION_CYCLES); + includeDrivePhase = TestDashboard.getParamBoolean(this, "IncludeDrivePhase", true); + numberOfTrials = Math.max(1, TestDashboard.getParamInt(this, "NumberOfTrials", 10)); + + // Compute waypoints and poses (same for all trials) + computeWaypoints(); + startPose = new Pose2d(targetX, targetY, Rotation2d.fromDegrees(targetHeadingDeg)); + drivePose = new Pose2d(targetX, targetY, Rotation2d.fromDegrees(driveHeadingDeg)); + + // Allocate per-trial result arrays + trialCorruptedCycles = new int[numberOfTrials]; + trialMaxHeadingError = new double[numberOfTrials]; + trialPigeonHadLatency = new boolean[numberOfTrials]; + trialMaxLateralDev = new double[numberOfTrials]; + trialFinalPosError = new double[numberOfTrials]; + trialFinalHeadingError = new double[numberOfTrials]; + + // Print header + System.out.println(String.format("=== Odometry Reset Test Started (%d trials) ===", numberOfTrials)); + System.out.println(String.format("Phase 1 (observe) heading: %.2f°", targetHeadingDeg)); + System.out.println(String.format("Phase 2 (drive) heading: %.2f° (delta = %.2f°)", + driveHeadingDeg, normalizeHeadingError(targetHeadingDeg, driveHeadingDeg))); + System.out.println(String.format("Start position: (%.2f, %.2f)", targetX, targetY)); + System.out.println(String.format("WP1: (%.2f, %.2f, %.2f°)", wp1.getX(), wp1.getY(), wp1.getRotation().getDegrees())); + System.out.println(String.format("WP2: (%.2f, %.2f, %.2f°)", wp2.getX(), wp2.getY(), wp2.getRotation().getDegrees())); + System.out.println(String.format("Drive phase: %s | Observation cycles: %d", + includeDrivePhase ? "ENABLED" : "DISABLED", observationCycles)); + + // Dashboard config + TestDashboard.putResultDouble(this, "Config/TargetX", targetX); + TestDashboard.putResultDouble(this, "Config/TargetY", targetY); + TestDashboard.putResultDouble(this, "Config/TargetHeading", targetHeadingDeg); + TestDashboard.putResultDouble(this, "Config/DriveHeading", driveHeadingDeg); + TestDashboard.putResultInt(this, "Config/NumberOfTrials", numberOfTrials); + + // Start first trial + currentTrial = 0; + startTrial(); + } + + /** + * Initializes (or re-initializes) state for a new trial and performs the + * Phase 1 reset. Called once from {@code initialize()} and again from + * {@code STOPPING} for subsequent trials. + */ + private void startTrial() { + System.out.println(String.format("\n========== Trial %d/%d ==========", currentTrial + 1, numberOfTrials)); + + // Reset per-trial observation state + obsCycleCount = 0; + obsPigeonYaw = new double[observationCycles]; + obsOdoHeading = new double[observationCycles]; + obsOdoX = new double[observationCycles]; + obsOdoY = new double[observationCycles]; + + // Reset per-trial drive state + driveSampleCount = 0; + driveLeg = new int[MAX_DRIVE_SAMPLES]; + driveTime = new double[MAX_DRIVE_SAMPLES]; + drivePoseX = new double[MAX_DRIVE_SAMPLES]; + drivePoseY = new double[MAX_DRIVE_SAMPLES]; + drivePoseHeading = new double[MAX_DRIVE_SAMPLES]; + drivePigeonYaw = new double[MAX_DRIVE_SAMPLES]; + driveCmdVx = new double[MAX_DRIVE_SAMPLES]; + driveCmdVy = new double[MAX_DRIVE_SAMPLES]; + driveCmdOmega = new double[MAX_DRIVE_SAMPLES]; + driveLateralDev = new double[MAX_DRIVE_SAMPLES]; + + leg1Complete = false; + leg1MaxDev = 0; + leg1Cycles = 0; + + // Record state BEFORE reset + pigeonYawBeforeReset = swerveSubsystem.getPigeon().getYaw().getValueAsDouble(); + odometryHeadingBeforeReset = swerveSubsystem.getPose().getRotation().getDegrees(); + + // ---- PERFORM THE PHASE 1 RESET (same buggy code as startAutoAt) ---- + Pigeon2 pigeon = swerveSubsystem.getPigeon(); + SwerveDrivePoseEstimator odometry = swerveSubsystem.getOdometry(); + SwerveModulePosition[] positions = swerveSubsystem.getPositions(); + + pigeon.setYaw(startPose.getRotation().getDegrees()); + odometry.resetPosition(startPose.getRotation(), positions, startPose); + // ---- END BUGGY CODE ---- + + // Record state after reset + pigeonYawAfterReset = pigeon.getYaw().getValueAsDouble(); + odometryHeadingAfterReset = swerveSubsystem.getPose().getRotation().getDegrees(); + + System.out.println(String.format("Phase 1: Reset to %.2f° — pigeon before=%.2f° after=%.2f° odo before=%.2f° after=%.2f°", + targetHeadingDeg, pigeonYawBeforeReset, pigeonYawAfterReset, + odometryHeadingBeforeReset, odometryHeadingAfterReset)); + + currentPhase = Phase.OBSERVING; + TestDashboard.putResultString(this, "Status/Message", + String.format("Trial %d/%d — Observing...", currentTrial + 1, numberOfTrials)); + } + + @Override + public void execute() { + switch (currentPhase) { + case OBSERVING: + executeObserving(); + break; + case DRIVING_LEG1: + executeDriveLeg(1, wp1); + break; + case DRIVING_LEG2: + executeDriveLeg(2, wp2); + break; + case STOPPING: + swerveSubsystem.drive(0, 0, 0, false); + recordTrialResults(); + currentTrial++; + if (currentTrial < numberOfTrials) { + startTrial(); + } else { + currentPhase = Phase.COMPLETE; + } + break; + case COMPLETE: + break; + } + } + + @Override + public boolean isFinished() { + return currentPhase == Phase.COMPLETE; + } + + @Override + public void end(boolean interrupted) { + // Always stop the robot + swerveSubsystem.drive(0, 0, 0, false); + + int completedTrials = currentTrial; + + if (interrupted) { + System.out.println(String.format( + "\n=== Odometry Reset Test INTERRUPTED (completed %d/%d trials) ===", + completedTrials, numberOfTrials)); + TestDashboard.putResultString(this, "Status/Message", "Interrupted"); + } + + // Print summary table if any trials completed + if (completedTrials > 0) { + printMultiTrialSummary(completedTrials); + } + } + + // ============================================================================ + // Phase Execution Methods + // ============================================================================ + + private void executeObserving() { + if (obsCycleCount >= observationCycles) { + // Print observation summary before transitioning + System.out.println("--- Observation complete ---"); + printObservationSummary(); + + if (includeDrivePhase) { + // The pigeon is now settled at targetHeadingDeg from Phase 1. + // Phase 2 uses a DIFFERENT heading (driveHeadingDeg), so the + // setYaw(driveHeading) call triggers real pigeon latency + // (e.g., 90° → 0° transition), causing heading corruption on + // the first drive cycle. + System.out.println(String.format( + "\n--- Phase 2: Reset to driveHeading=%.2f° (pigeon is at ~%.2f°) ---", + driveHeadingDeg, targetHeadingDeg)); + performPoseReset(); + System.out.println(String.format( + "Phase 2 reset: pigeonYaw before=%.2f° after=%.2f° odoHeading before=%.2f° after=%.2f°", + phase2PigeonYawBeforeReset, phase2PigeonYawAfterReset, + phase2OdoHeadingBeforeReset, phase2OdoHeadingAfterReset)); + + boolean pigeonHasLatency = Math.abs(phase2PigeonYawAfterReset - driveHeadingDeg) > HEADING_CORRUPTION_THRESHOLD; + System.out.println(String.format(" Pigeon latency: %s (pigeon=%.2f° vs target=%.2f°)", + pigeonHasLatency + ? "YES — pigeon has NOT caught up (corruption expected on drive cycle 1)" + : "NO — pigeon already updated (no corruption expected)", + phase2PigeonYawAfterReset, driveHeadingDeg)); + + System.out.println("\n--- Starting closed-loop L-path drive ---"); + System.out.println(String.format("Leg 1: Drive to WP1 (%.2f, %.2f, %.2f°)", + wp1.getX(), wp1.getY(), wp1.getRotation().getDegrees())); + currentPhase = Phase.DRIVING_LEG1; + driveStartTime = Timer.getFPGATimestamp(); + legStartTime = driveStartTime; + TestDashboard.putResultString(this, "Status/Message", "Driving Leg 1..."); + } else { + // No drive phase — record results and advance to next trial + recordTrialResults(); + currentTrial++; + if (currentTrial < numberOfTrials) { + startTrial(); + } else { + currentPhase = Phase.COMPLETE; + } + } + return; + } + + // Record observation sample + double pigeonYaw = swerveSubsystem.getPigeon().getYaw().getValueAsDouble(); + Pose2d odoPose = swerveSubsystem.getPose(); + double odoHeading = odoPose.getRotation().getDegrees(); + + obsPigeonYaw[obsCycleCount] = pigeonYaw; + obsOdoHeading[obsCycleCount] = odoHeading; + obsOdoX[obsCycleCount] = odoPose.getX(); + obsOdoY[obsCycleCount] = odoPose.getY(); + + double headingError = normalizeHeadingError(targetHeadingDeg, odoHeading); + + System.out.println(String.format( + " Cycle %2d: pigeonYaw=%7.2f° odoHeading=%7.2f° odoPos=(%.2f, %.2f) headingError=%6.2f°%s", + obsCycleCount + 1, pigeonYaw, odoHeading, + odoPose.getX(), odoPose.getY(), headingError, + headingError > HEADING_CORRUPTION_THRESHOLD ? " *** HEADING CORRUPTED ***" : "")); + + obsCycleCount++; + } + + /** + * Performs the same buggy pose reset as startAutoAt(), but using the + * drive heading (different from the observation heading). Since the pigeon + * is still at targetHeadingDeg from Phase 1, this triggers real pigeon + * latency during the setYaw() call. + */ + private void performPoseReset() { + Pigeon2 pigeon = swerveSubsystem.getPigeon(); + SwerveDrivePoseEstimator odometry = swerveSubsystem.getOdometry(); + SwerveModulePosition[] positions = swerveSubsystem.getPositions(); + + // Record state before reset + phase2PigeonYawBeforeReset = pigeon.getYaw().getValueAsDouble(); + phase2OdoHeadingBeforeReset = swerveSubsystem.getPose().getRotation().getDegrees(); + + // ---- SAME BUGGY CODE AS startAutoAt(), using driveHeading ---- + pigeon.setYaw(drivePose.getRotation().getDegrees()); + odometry.resetPosition(drivePose.getRotation(), positions, drivePose); + // ---- END BUGGY CODE ---- + + // Record state after reset (same cycle) + phase2PigeonYawAfterReset = pigeon.getYaw().getValueAsDouble(); + phase2OdoHeadingAfterReset = swerveSubsystem.getPose().getRotation().getDegrees(); + + // Dashboard + TestDashboard.putResultDouble(this, "Phase2Reset/PigeonBefore", phase2PigeonYawBeforeReset); + TestDashboard.putResultDouble(this, "Phase2Reset/PigeonAfter", phase2PigeonYawAfterReset); + TestDashboard.putResultDouble(this, "Phase2Reset/OdoHeadingBefore", phase2OdoHeadingBeforeReset); + TestDashboard.putResultDouble(this, "Phase2Reset/OdoHeadingAfter", phase2OdoHeadingAfterReset); + } + + private void executeDriveLeg(int legNumber, Pose2d waypoint) { + double now = Timer.getFPGATimestamp(); + + // Command drive toward waypoint + double[] cmd = computeDriveCommand(waypoint); + swerveSubsystem.drive(cmd[0], cmd[1], cmd[2], true); + + // Record sample + recordDriveSample(legNumber, cmd, now); + + // Check arrival or timeout + boolean arrived = isAtWaypoint(waypoint); + boolean timedOut = (now - legStartTime) > LEG_TIMEOUT_SEC; + + if (arrived || timedOut) { + if (timedOut && !arrived) { + System.out.println(String.format(" *** Leg %d TIMED OUT after %.1f sec ***", + legNumber, LEG_TIMEOUT_SEC)); + } + + if (legNumber == 1) { + // Record leg 1 final state + Pose2d finalPose = swerveSubsystem.getPose(); + leg1Complete = true; + leg1FinalX = finalPose.getX(); + leg1FinalY = finalPose.getY(); + leg1FinalHeading = finalPose.getRotation().getDegrees(); + leg1Cycles = countLegSamples(1); + leg1MaxDev = maxLateralDeviation(1); + + System.out.println(String.format( + " Leg 1 complete: pos=(%.3f, %.3f) heading=%.2f° maxDev=%.4f m cycles=%d %s", + leg1FinalX, leg1FinalY, leg1FinalHeading, leg1MaxDev, + leg1Cycles, arrived ? "ARRIVED" : "TIMEOUT")); + + // Transition to leg 2 + System.out.println(String.format(" Leg 2: Drive to WP2 (%.2f, %.2f, %.2f°)", + wp2.getX(), wp2.getY(), wp2.getRotation().getDegrees())); + currentPhase = Phase.DRIVING_LEG2; + legStartTime = now; + TestDashboard.putResultString(this, "Status/Message", "Driving Leg 2..."); + } else { + // Leg 2 complete — stop + System.out.println(String.format( + " Leg 2 complete: pos=(%.3f, %.3f) heading=%.2f° %s", + swerveSubsystem.getPose().getX(), + swerveSubsystem.getPose().getY(), + swerveSubsystem.getPose().getRotation().getDegrees(), + arrived ? "ARRIVED" : "TIMEOUT")); + currentPhase = Phase.STOPPING; + TestDashboard.putResultString(this, "Status/Message", "Stopping..."); + } + } + } + + // ============================================================================ + // Per-Trial Result Recording + // ============================================================================ + + /** + * Computes and stores summary results for the current trial, then prints + * a one-line summary to the console. + */ + private void recordTrialResults() { + int t = currentTrial; + + // Phase 1 results + int corrupted = 0; + double maxError = 0; + for (int i = 0; i < obsCycleCount; i++) { + double error = normalizeHeadingError(targetHeadingDeg, obsOdoHeading[i]); + if (error > maxError) maxError = error; + if (error > HEADING_CORRUPTION_THRESHOLD) corrupted++; + } + trialCorruptedCycles[t] = corrupted; + trialMaxHeadingError[t] = maxError; + + // Phase 2 results + if (includeDrivePhase && driveSampleCount > 0) { + trialPigeonHadLatency[t] = Math.abs(phase2PigeonYawAfterReset - driveHeadingDeg) > HEADING_CORRUPTION_THRESHOLD; + trialMaxLateralDev[t] = Math.max(maxLateralDeviation(1), maxLateralDeviation(2)); + Pose2d finalPose = swerveSubsystem.getPose(); + trialFinalPosError[t] = finalPose.getTranslation().getDistance(wp2.getTranslation()); + trialFinalHeadingError[t] = normalizeHeadingError( + wp2.getRotation().getDegrees(), + finalPose.getRotation().getDegrees()); + } + + // One-line summary + if (includeDrivePhase && driveSampleCount > 0) { + System.out.println(String.format( + " ► Trial %d: corruption=%d/%d pigeonLatency=%s maxDev=%.4fm posErr=%.4fm hdgErr=%.2f°", + currentTrial + 1, corrupted, obsCycleCount, + trialPigeonHadLatency[t] ? "YES" : "NO", + trialMaxLateralDev[t], trialFinalPosError[t], trialFinalHeadingError[t])); + } else { + System.out.println(String.format( + " ► Trial %d: corruption=%d/%d maxHdgError=%.2f°", + currentTrial + 1, corrupted, obsCycleCount, maxError)); + } + } + + // ============================================================================ + // Drive Control + // ============================================================================ + + /** + * Computes field-relative drive commands using proportional control. + * Returns [vx, vy, omega] in field frame. + */ + private double[] computeDriveCommand(Pose2d waypoint) { + Pose2d current = swerveSubsystem.getPose(); + + // Position error in field frame + double xError = waypoint.getX() - current.getX(); + double yError = waypoint.getY() - current.getY(); + + // Heading error (shortest path, in radians) + double headingErrorRad = waypoint.getRotation().minus(current.getRotation()).getRadians(); + + // Proportional control with speed limits + double vx = clamp(DRIVE_KP * xError, -MAX_DRIVE_SPEED, MAX_DRIVE_SPEED); + double vy = clamp(DRIVE_KP * yError, -MAX_DRIVE_SPEED, MAX_DRIVE_SPEED); + double omega = clamp(HEADING_KP * headingErrorRad, -MAX_ROT_SPEED, MAX_ROT_SPEED); + + return new double[]{vx, vy, omega}; + } + + /** + * Checks if the robot has arrived at the waypoint. + */ + private boolean isAtWaypoint(Pose2d waypoint) { + Pose2d current = swerveSubsystem.getPose(); + + double posError = current.getTranslation().getDistance(waypoint.getTranslation()); + double headingError = normalizeHeadingError( + waypoint.getRotation().getDegrees(), + current.getRotation().getDegrees()); + + // Also check that the robot is moving slowly (settled) + ChassisSpeeds speeds = swerveSubsystem.getChassisSpeeds(); + double speed = Math.sqrt( + speeds.vxMetersPerSecond * speeds.vxMetersPerSecond + + speeds.vyMetersPerSecond * speeds.vyMetersPerSecond); + + return posError < POSITION_TOLERANCE + && headingError < HEADING_TOLERANCE_DEG + && speed < SETTLE_SPEED; + } + + // ============================================================================ + // Data Recording + // ============================================================================ + + private void recordDriveSample(int legNumber, double[] cmd, double now) { + if (driveSampleCount >= MAX_DRIVE_SAMPLES) return; + + Pose2d pose = swerveSubsystem.getPose(); + double pigeonYaw = swerveSubsystem.getPigeon().getYaw().getValueAsDouble(); + double lateralDev = computeLateralDeviation(legNumber, pose); + + int i = driveSampleCount; + driveLeg[i] = legNumber; + driveTime[i] = now - driveStartTime; + drivePoseX[i] = pose.getX(); + drivePoseY[i] = pose.getY(); + drivePoseHeading[i] = pose.getRotation().getDegrees(); + drivePigeonYaw[i] = pigeonYaw; + driveCmdVx[i] = cmd[0]; + driveCmdVy[i] = cmd[1]; + driveCmdOmega[i] = cmd[2]; + driveLateralDev[i] = lateralDev; + + // Console output: first cycle, every Nth cycle, anomalies + int legCycle = countLegSamples(legNumber); + boolean shouldPrint = (legCycle == 1) + || (legCycle % DRIVE_PRINT_INTERVAL == 0) + || (lateralDev > DEVIATION_ALERT_THRESHOLD); + + if (shouldPrint) { + System.out.println(String.format( + " [L%d C%3d t=%.2fs] pos=(%.3f,%.3f) hdg=%.1f° pigeon=%.1f° cmd=(%.2f,%.2f,%.2f) dev=%.4fm%s", + legNumber, legCycle, driveTime[i], + drivePoseX[i], drivePoseY[i], drivePoseHeading[i], drivePigeonYaw[i], + driveCmdVx[i], driveCmdVy[i], driveCmdOmega[i], lateralDev, + lateralDev > DEVIATION_ALERT_THRESHOLD ? " *** HIGH DEVIATION ***" : "")); + } + + driveSampleCount++; + + // Real-time dashboard + TestDashboard.putResultDouble(this, "RealTime/PoseX", pose.getX()); + TestDashboard.putResultDouble(this, "RealTime/PoseY", pose.getY()); + TestDashboard.putResultDouble(this, "RealTime/Heading", pose.getRotation().getDegrees()); + TestDashboard.putResultDouble(this, "RealTime/LateralDev", lateralDev); + } + + // ============================================================================ + // Waypoint Computation + // ============================================================================ + + /** + * Computes the two waypoints for the L-shaped path based on the drive heading. + * The L-path uses {@code driveHeadingDeg} (not {@code targetHeadingDeg}) so the + * path geometry matches the heading the robot will be reset to for Phase 2. + *

+ * WP1: LEG_LENGTH meters "forward" from start (in the direction of driveHeading). + * WP2: LEG_LENGTH meters "right" from WP1 (90° CW turn). + */ + private void computeWaypoints() { + double headingRad = Math.toRadians(driveHeadingDeg); + + // Forward direction (direction robot will face for drive phase) + double fwdX = Math.cos(headingRad); + double fwdY = Math.sin(headingRad); + + // WP1: straight ahead from start + wp1 = new Pose2d( + targetX + LEG_LENGTH * fwdX, + targetY + LEG_LENGTH * fwdY, + Rotation2d.fromDegrees(driveHeadingDeg)); + + // Right direction (90° CW from heading) + // cos(θ - 90°) = sin(θ), sin(θ - 90°) = -cos(θ) + double rightX = Math.sin(headingRad); + double rightY = -Math.cos(headingRad); + + // WP2: right turn from WP1 + wp2 = new Pose2d( + wp1.getX() + LEG_LENGTH * rightX, + wp1.getY() + LEG_LENGTH * rightY, + Rotation2d.fromDegrees(driveHeadingDeg - 90.0)); + } + + // ============================================================================ + // Lateral Deviation + // ============================================================================ + + /** + * Computes perpendicular distance from the robot's current position to the + * ideal straight-line path for the given leg. + */ + private double computeLateralDeviation(int legNumber, Pose2d pose) { + if (legNumber == 1) { + // Ideal: straight line from (targetX, targetY) to WP1 + return pointToLineDistance( + pose.getX(), pose.getY(), + targetX, targetY, wp1.getX(), wp1.getY()); + } else { + // Ideal: straight line from WP1 to WP2 + return pointToLineDistance( + pose.getX(), pose.getY(), + wp1.getX(), wp1.getY(), wp2.getX(), wp2.getY()); + } + } + + /** + * Perpendicular distance from point (px,py) to the line through (x1,y1)-(x2,y2). + */ + private static double pointToLineDistance( + double px, double py, + double x1, double y1, double x2, double y2) { + double dx = x2 - x1; + double dy = y2 - y1; + double len = Math.sqrt(dx * dx + dy * dy); + if (len < 1e-6) { + return Math.sqrt((px - x1) * (px - x1) + (py - y1) * (py - y1)); + } + // |cross product| / length + return Math.abs((py - y1) * dx - (px - x1) * dy) / len; + } + + // ============================================================================ + // Reporting + // ============================================================================ + + private void printObservationSummary() { + double maxError = 0; + int corruptedCount = 0; + for (int i = 0; i < obsCycleCount; i++) { + double error = normalizeHeadingError(targetHeadingDeg, obsOdoHeading[i]); + if (error > maxError) maxError = error; + if (error > HEADING_CORRUPTION_THRESHOLD) corruptedCount++; + } + System.out.println(String.format(" Corrupted cycles: %d / %d maxHeadingError: %.2f°", + corruptedCount, obsCycleCount, maxError)); + } + + /** + * Prints the summary table, statistics, and verdict for all completed trials. + */ + private void printMultiTrialSummary(int completedTrials) { + System.out.println(String.format( + "\n=== MULTI-TRIAL SUMMARY (%d/%d trials completed) ===", completedTrials, numberOfTrials)); + + // Table + if (includeDrivePhase) { + System.out.println("Trial | Corrupted | MaxHdgErr | PigeonLat | MaxLatDev | FinalPosErr | FinalHdgErr"); + System.out.println("------+-----------+-----------+-----------+-----------+-------------+------------"); + for (int t = 0; t < completedTrials; t++) { + System.out.println(String.format( + " %2d | %2d/%-2d | %6.2f° | %3s | %7.4fm | %7.4fm | %6.2f°", + t + 1, trialCorruptedCycles[t], observationCycles, + trialMaxHeadingError[t], + trialPigeonHadLatency[t] ? "YES" : " NO", + trialMaxLateralDev[t], trialFinalPosError[t], trialFinalHeadingError[t])); + } + } else { + System.out.println("Trial | Corrupted | MaxHdgErr"); + System.out.println("------+-----------+----------"); + for (int t = 0; t < completedTrials; t++) { + System.out.println(String.format( + " %2d | %2d/%-2d | %6.2f°", + t + 1, trialCorruptedCycles[t], observationCycles, + trialMaxHeadingError[t])); + } + } + + // Statistics + int trialsWithCorruption = 0; + int trialsWithLatency = 0; + double totalCorrupted = 0; + double maxMaxDev = 0; + double totalMaxDev = 0; + double maxPosErr = 0; + + for (int t = 0; t < completedTrials; t++) { + if (trialCorruptedCycles[t] > 0) trialsWithCorruption++; + totalCorrupted += trialCorruptedCycles[t]; + if (includeDrivePhase) { + if (trialPigeonHadLatency[t]) trialsWithLatency++; + if (trialMaxLateralDev[t] > maxMaxDev) maxMaxDev = trialMaxLateralDev[t]; + totalMaxDev += trialMaxLateralDev[t]; + if (trialFinalPosError[t] > maxPosErr) maxPosErr = trialFinalPosError[t]; + } + } + + System.out.println("\n--- Statistics ---"); + System.out.println(String.format(" Trials with corruption: %d/%d (%.0f%%)", + trialsWithCorruption, completedTrials, 100.0 * trialsWithCorruption / completedTrials)); + System.out.println(String.format(" Average corrupted cycles: %.1f / %d", + totalCorrupted / completedTrials, observationCycles)); + + if (includeDrivePhase) { + System.out.println(String.format(" Trials with pigeon latency: %d/%d (%.0f%%)", + trialsWithLatency, completedTrials, 100.0 * trialsWithLatency / completedTrials)); + System.out.println(String.format(" Average max lateral dev: %.4fm", + totalMaxDev / completedTrials)); + System.out.println(String.format(" Worst max lateral dev: %.4fm", maxMaxDev)); + System.out.println(String.format(" Worst final pos error: %.4fm", maxPosErr)); + } + + // Verdict + System.out.println("\n=== VERDICT ==="); + System.out.println(String.format("Phase 1 (Reset): %s — %d/%d trials showed heading corruption", + trialsWithCorruption > 0 ? "BUG DETECTED" : "PASS", + trialsWithCorruption, completedTrials)); + + if (trialsWithCorruption > 0) { + System.out.println(" → odometry.resetPosition() received the DESIRED heading as gyro baseline"); + System.out.println(" instead of the ACTUAL pigeon reading (which hasn't updated yet)."); + } + + if (includeDrivePhase) { + boolean allPathsOk = true; + for (int t = 0; t < completedTrials; t++) { + if (trialMaxLateralDev[t] > 0.15 || trialFinalPosError[t] > 0.15) { + allPathsOk = false; + break; + } + } + System.out.println(String.format("Phase 2 (Path): %s — worst deviation=%.4fm, worst pos error=%.4fm", + allPathsOk ? "PASS" : "DEVIATION DETECTED", maxMaxDev, maxPosErr)); + } + + System.out.println("===================================\n"); + + // Dashboard + TestDashboard.putResultInt(this, "Summary/CompletedTrials", completedTrials); + TestDashboard.putResultInt(this, "Summary/TrialsWithCorruption", trialsWithCorruption); + TestDashboard.putResultString(this, "Summary/ResetVerdict", + trialsWithCorruption > 0 ? "BUG DETECTED" : "PASS"); + TestDashboard.putResultString(this, "Status/Message", + trialsWithCorruption > 0 ? "BUG DETECTED — see console" : "PASS"); + } + + // ============================================================================ + // Utility Methods + // ============================================================================ + + /** + * Returns the absolute heading error in [0, 180] degrees. + */ + private static double normalizeHeadingError(double targetDeg, double actualDeg) { + double error = Math.abs(targetDeg - actualDeg); + if (error > 180) error = 360 - error; + return error; + } + + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } + + /** + * Counts drive samples for a specific leg. + */ + private int countLegSamples(int legNumber) { + int count = 0; + for (int i = 0; i < driveSampleCount; i++) { + if (driveLeg[i] == legNumber) count++; + } + return count; + } + + /** + * Finds the maximum lateral deviation for a specific leg. + */ + private double maxLateralDeviation(int legNumber) { + double max = 0; + for (int i = 0; i < driveSampleCount; i++) { + if (driveLeg[i] == legNumber && driveLateralDev[i] > max) { + max = driveLateralDev[i]; + } + } + return max; + } +} diff --git a/src/main/java/frc/robot/test/SwerveAlignmentTestCommand.java b/src/main/java/frc/robot/test/SwerveAlignmentTestCommand.java new file mode 100644 index 0000000..fa4ce56 --- /dev/null +++ b/src/main/java/frc/robot/test/SwerveAlignmentTestCommand.java @@ -0,0 +1,424 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.test; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.test.DiagnosticTest; +import frc.lib.test.TestDashboard; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.SwerveModule; + +/** + * Diagnostic test that commands all four swerve modules to the same angle and measures + * how accurately each module reaches the target. + * + *

This test directly diagnoses angle offset calibration errors, which are the most + * common cause of swerve drive drift. If one module has a bad angleOffset or a failed + * resetToAbsolute() at startup, this test will reveal it clearly. + * + *

Test sequence: + *

    + *
  1. Command all 4 modules to a target angle (e.g., 0° = straight forward) with zero drive speed
    2. + *
  2. Wait for all modules to settle (within tolerance or timeout)
    3. + *
  3. Record the actual integrated encoder angle and absolute encoder (CANcoder) angle for each module
    4. + *
  4. Optionally repeat at additional angles (90°, 180°, 270°) to detect angle-dependent errors
    5. + *
  5. Report per-module angle errors and flag any module that deviates significantly
    6. + *
+ * + *

What this test reveals: + *

    + *
  • Incorrect angleOffset values (module points in wrong direction)
    • + *
  • Failed resetToAbsolute() at startup (integrated encoder not calibrated)
    • + *
  • Stale Preferences values overriding Constants.java
    • + *
  • Mechanical binding or friction preventing a module from reaching its target
    • + *
  • PID tuning issues (one module reaches target much slower than others)
    • + *
+ * + *

Interpreting results: + *

    + *
  • All modules within ±2°: Angle calibration is good — drift cause is elsewhere
    • + *
  • One module off by a consistent amount: Bad angleOffset for that module
    • + *
  • One module off by varying amounts at different test angles: Encoder or mechanical issue
    • + *
  • All modules off by similar amount: Systematic error (wrong conversion factor or gear ratio)
    • + *
+ */ +public class SwerveAlignmentTestCommand extends Command implements DiagnosticTest { + + private final SwerveSubsystem swerveSubsystem; + + // Test parameters (read from SmartDashboard in initialize()) + private double[] testAngles; // Angles to test (e.g., {0, 90, 180, 270}) + private double settleTimeSeconds; // Time to wait for modules to settle at each angle + private double toleranceDegrees; // Angle tolerance for "at position" check + + // Test state + private enum TestState { + COMMANDING, // Just commanded modules to a new angle + SETTLING, // Waiting for modules to settle + RECORDING, // Recording measurements at current angle + COMPLETE // All angles tested + } + + private TestState currentState; + private int currentAngleIndex; // Index into testAngles array + private double stateStartTime; + + // Results storage: [angleIndex][moduleNumber] + private double[][] integratedAngles; // Integrated encoder readings + private double[][] absoluteAngles; // CANcoder readings + private double[][] angleErrors; // Difference from target + private boolean[][] settledInTime; // Whether module reached target before timeout + + // Module references + private SwerveModule[] modules; + private static final int NUM_MODULES = 4; + + /** + * Creates a new SwerveAlignmentTestCommand. + * Parameters are read from SmartDashboard in the initialize() method. + * + * @param swerveSubsystem The swerve subsystem containing the modules + */ + public SwerveAlignmentTestCommand(SwerveSubsystem swerveSubsystem) { + this.swerveSubsystem = swerveSubsystem; + addRequirements(swerveSubsystem); + } + + // ============================================================================ + // DiagnosticTest Interface Implementation + // ============================================================================ + + @Override + public String getTestName() { + return "Swerve Alignment Test"; + } + + @Override + public String getTestDescription() { + return "Commands all four swerve modules to the same angle and measures how accurately " + + "each module reaches the target. Reveals angle offset calibration errors, " + + "failed encoder calibration, and module-specific issues that cause drift."; + } + + @Override + public void initializeParameters() { + TestDashboard.putParamBoolean(this, "TestMultipleAngles", true); + TestDashboard.putParamDouble(this, "SingleTestAngle", 0.0); + TestDashboard.putParamDouble(this, "SettleTime", 2.0); + TestDashboard.putParamDouble(this, "Tolerance", 2.0); + } + + // ============================================================================ + // Command Lifecycle + // ============================================================================ + + @Override + public void initialize() { + // Read parameters from SmartDashboard + boolean testMultiple = TestDashboard.getParamBoolean(this, "TestMultipleAngles", true); + double singleAngle = TestDashboard.getParamDouble(this, "SingleTestAngle", 0.0); + settleTimeSeconds = TestDashboard.getParamDouble(this, "SettleTime", 2.0); + toleranceDegrees = TestDashboard.getParamDouble(this, "Tolerance", 2.0); + + // Set up test angles + if (testMultiple) { + testAngles = new double[]{0.0, 90.0, 180.0, 270.0}; + } else { + testAngles = new double[]{singleAngle}; + } + + // Get module references + modules = new SwerveModule[NUM_MODULES]; + for (int i = 0; i < NUM_MODULES; i++) { + modules[i] = swerveSubsystem.getModule(i); + if (modules[i] == null) { + System.err.println("ERROR: Module " + i + " not found in swerve subsystem."); + currentState = TestState.COMPLETE; + return; + } + } + + // Initialize results storage + integratedAngles = new double[testAngles.length][NUM_MODULES]; + absoluteAngles = new double[testAngles.length][NUM_MODULES]; + angleErrors = new double[testAngles.length][NUM_MODULES]; + settledInTime = new boolean[testAngles.length][NUM_MODULES]; + + // Start first angle test + currentAngleIndex = 0; + commandCurrentAngle(); + + // Log test start + System.out.println("=== Swerve Alignment Test Started ==="); + System.out.println("Test Angles: " + formatAngles(testAngles)); + System.out.println("Settle Time: " + settleTimeSeconds + "s"); + System.out.println("Tolerance: " + toleranceDegrees + "°"); + System.out.println("-------------------------------------"); + + // Update dashboard + TestDashboard.putResultString(this, "Status", "Running"); + TestDashboard.putResultInt(this, "Config/NumAngles", testAngles.length); + } + + @Override + public void execute() { + if (currentState == TestState.COMPLETE) { + return; + } + + double elapsed = Timer.getFPGATimestamp() - stateStartTime; + + switch (currentState) { + case COMMANDING: + // Transition to settling immediately (command was sent in commandCurrentAngle) + currentState = TestState.SETTLING; + stateStartTime = Timer.getFPGATimestamp(); + break; + + case SETTLING: + // Wait for settle time to elapse + if (elapsed >= settleTimeSeconds) { + currentState = TestState.RECORDING; + recordMeasurements(); + } + // Update real-time display while settling + updateRealTimeDisplay(); + break; + + case RECORDING: + // Measurements recorded, move to next angle or finish + currentAngleIndex++; + if (currentAngleIndex < testAngles.length) { + commandCurrentAngle(); + } else { + currentState = TestState.COMPLETE; + printResults(); + } + break; + + default: + break; + } + } + + @Override + public boolean isFinished() { + return currentState == TestState.COMPLETE; + } + + @Override + public void end(boolean interrupted) { + if (interrupted) { + System.out.println("=== Swerve Alignment Test INTERRUPTED ==="); + TestDashboard.putResultString(this, "Status", "Interrupted"); + } else { + System.out.println("=== Swerve Alignment Test COMPLETED ==="); + TestDashboard.putResultString(this, "Status", "Complete"); + } + } + + // ============================================================================ + // Test Logic + // ============================================================================ + + /** + * Commands all four modules to the current test angle with zero drive speed. + */ + private void commandCurrentAngle() { + double angle = testAngles[currentAngleIndex]; + SwerveModuleState targetState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(angle)); + + for (int i = 0; i < NUM_MODULES; i++) { + modules[i].setDesiredState(targetState, false); + } + + currentState = TestState.COMMANDING; + stateStartTime = Timer.getFPGATimestamp(); + + System.out.println(String.format("\nCommanding all modules to %.1f°...", angle)); + TestDashboard.putResultDouble(this, "Progress/TargetAngle", angle); + TestDashboard.putResultInt(this, "Progress/AngleStep", + currentAngleIndex + 1); + } + + /** + * Records encoder measurements for all modules at the current test angle. + */ + private void recordMeasurements() { + double targetAngle = testAngles[currentAngleIndex]; + + System.out.println(String.format("Recording measurements at %.1f°:", targetAngle)); + System.out.println(String.format(" %-10s %-15s %-15s %-12s %-10s", + "Module", "Integrated(°)", "Absolute(°)", "Error(°)", "Settled?")); + System.out.println(String.format(" %-10s %-15s %-15s %-12s %-10s", + "------", "-------------", "-----------", "--------", "--------")); + + for (int i = 0; i < NUM_MODULES; i++) { + double integrated = modules[i].getRawTurnEncoder(); + double absolute = modules[i].getCanCoder().getDegrees(); + double error = Math.IEEEremainder(integrated - targetAngle, 360.0); + boolean settled = Math.abs(error) <= toleranceDegrees; + + integratedAngles[currentAngleIndex][i] = integrated; + absoluteAngles[currentAngleIndex][i] = absolute; + angleErrors[currentAngleIndex][i] = error; + settledInTime[currentAngleIndex][i] = settled; + + System.out.println(String.format(" Mod %-5d %-15.2f %-15.2f %-12.3f %-10s", + i, integrated, absolute, error, settled ? "YES" : "NO ⚠️")); + + // Publish per-module results for this angle + String prefix = String.format("Angle%.0f/Mod%d/", targetAngle, i); + TestDashboard.putResultDouble(this, prefix + "Integrated", integrated); + TestDashboard.putResultDouble(this, prefix + "Absolute", absolute); + TestDashboard.putResultDouble(this, prefix + "Error", error); + TestDashboard.putResultBoolean(this, prefix + "Settled", settled); + } + } + + /** + * Updates the real-time SmartDashboard display during settling. + */ + private void updateRealTimeDisplay() { + double targetAngle = testAngles[currentAngleIndex]; + for (int i = 0; i < NUM_MODULES; i++) { + double current = modules[i].getRawTurnEncoder(); + double error = Math.IEEEremainder(current - targetAngle, 360.0); + TestDashboard.putResultDouble(this, "RealTime/Mod" + i + "Error", error); + TestDashboard.putResultDouble(this, "RealTime/Mod" + i + "Angle", current); + } + double remaining = settleTimeSeconds - (Timer.getFPGATimestamp() - stateStartTime); + TestDashboard.putResultDouble(this, "RealTime/SettleRemaining", Math.max(0.0, remaining)); + } + + // ============================================================================ + // Results Reporting + // ============================================================================ + + /** + * Prints the final results summary with per-module analysis. + */ + private void printResults() { + System.out.println("\n=== ALIGNMENT TEST RESULTS ==="); + + // Per-module summary across all angles + System.out.println("\n--- Per-Module Summary ---"); + for (int mod = 0; mod < NUM_MODULES; mod++) { + double maxAbsError = 0.0; + double sumAbsError = 0.0; + int failCount = 0; + + for (int a = 0; a < testAngles.length; a++) { + double absError = Math.abs(angleErrors[a][mod]); + if (absError > maxAbsError) maxAbsError = absError; + sumAbsError += absError; + if (!settledInTime[a][mod]) failCount++; + } + + double avgAbsError = sumAbsError / testAngles.length; + String status; + if (maxAbsError <= toleranceDegrees) { + status = "✓ PASS"; + } else if (maxAbsError <= toleranceDegrees * 2) { + status = "⚠️ MARGINAL"; + } else { + status = "✗ FAIL"; + } + + System.out.println(String.format(" Module %d: %s (avg error: %.2f°, max error: %.2f°, failed: %d/%d angles)", + mod, status, avgAbsError, maxAbsError, failCount, testAngles.length)); + + // Publish summary results + TestDashboard.putResultDouble(this, "Summary/Mod" + mod + "/AvgError", avgAbsError); + TestDashboard.putResultDouble(this, "Summary/Mod" + mod + "/MaxError", maxAbsError); + TestDashboard.putResultString(this, "Summary/Mod" + mod + "/Status", status); + } + + // Cross-module comparison: are all modules pointing the same direction? + System.out.println("\n--- Cross-Module Comparison (do all modules agree?) ---"); + for (int a = 0; a < testAngles.length; a++) { + double minAngle = integratedAngles[a][0]; + double maxAngle = integratedAngles[a][0]; + for (int mod = 1; mod < NUM_MODULES; mod++) { + double angle = integratedAngles[a][mod]; + // Use IEEEremainder to handle wrapping when comparing + double diffFromFirst = Math.IEEEremainder(angle - integratedAngles[a][0], 360.0); + double adjusted = integratedAngles[a][0] + diffFromFirst; + if (adjusted < minAngle) minAngle = adjusted; + if (adjusted > maxAngle) maxAngle = adjusted; + } + double spread = maxAngle - minAngle; + + String spreadStatus; + if (spread <= 2.0) { + spreadStatus = "✓ Tight"; + } else if (spread <= 5.0) { + spreadStatus = "⚠️ Moderate"; + } else { + spreadStatus = "✗ Wide — modules disagree significantly!"; + } + + System.out.println(String.format(" At %.0f°: spread = %.2f° %s", + testAngles[a], spread, spreadStatus)); + + TestDashboard.putResultDouble(this, + String.format("Summary/Angle%.0f/Spread", testAngles[a]), spread); + } + + // Integrated vs Absolute comparison: is resetToAbsolute() working? + // At calibration: integrated = absolute - angleOffset + // So at any time: (absolute - angleOffset) should ≈ integrated + // The "calibration error" is how much the integrated encoder has drifted from + // what the absolute encoder says it should be. + System.out.println("\n--- Integrated vs Absolute Encoder Comparison ---"); + System.out.println(" Checks whether resetToAbsolute() calibrated correctly."); + System.out.println(" CalibError = integrated - (absolute - angleOffset) [should be ≈ 0°]"); + for (int mod = 0; mod < NUM_MODULES; mod++) { + // Use the first test angle for this comparison + double integ = integratedAngles[0][mod]; + double absol = absoluteAngles[0][mod]; + double offset = modules[mod].getAngleOffset().getDegrees(); + // What the integrated encoder SHOULD read based on the absolute encoder and offset + double expectedIntegrated = absol - offset; + // How far off the integrated encoder is from what the absolute encoder says + double calibError = Math.IEEEremainder(integ - expectedIntegrated, 360.0); + + String calibStatus; + if (Math.abs(calibError) <= 3.0) { + calibStatus = "✓"; + } else if (Math.abs(calibError) <= 10.0) { + calibStatus = "⚠️"; + } else { + calibStatus = "✗ resetToAbsolute() may have failed!"; + } + + System.out.println(String.format( + " Module %d: Integrated=%.2f°, Absolute=%.2f°, Offset=%.2f°, CalibError=%.2f° %s", + mod, integ, absol, offset, calibError, calibStatus)); + + TestDashboard.putResultDouble(this, "Summary/Mod" + mod + "/CalibError", calibError); + TestDashboard.putResultDouble(this, "Summary/Mod" + mod + "/AngleOffset", offset); + } + + System.out.println("\n==============================\n"); + } + + // ============================================================================ + // Utilities + // ============================================================================ + + private String formatAngles(double[] angles) { + StringBuilder sb = new StringBuilder("["); + for (int i = 0; i < angles.length; i++) { + if (i > 0) sb.append(", "); + sb.append(String.format("%.0f°", angles[i])); + } + sb.append("]"); + return sb.toString(); + } +} diff --git a/src/main/java/frc/robot/test/SwerveAngleDriftTestCommand.java b/src/main/java/frc/robot/test/SwerveAngleDriftTestCommand.java new file mode 100644 index 0000000..c605654 --- /dev/null +++ b/src/main/java/frc/robot/test/SwerveAngleDriftTestCommand.java @@ -0,0 +1,694 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.test; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.test.DiagnosticTest; +import frc.lib.test.TestDashboard; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.SwerveModule; + +/** + * Test command to detect encoder drift in swerve module angle motors. + * + * This command performs the following test sequence: + * 1. Commands the angle motor to turn to a specific position + * 2. Waits for the motor to reach the target position + * 3. Commands the motor back to zero + * 4. Waits for the motor to reach zero + * 5. Repeats this cycle N times + * 6. Compares the relative encoder to the absolute encoder at each cycle + * 7. Reports drift statistics + * + * This test helps identify if the relative (integrated) encoder is accumulating + * error over multiple cycles, which would indicate drift issues. + */ +public class SwerveAngleDriftTestCommand extends Command implements DiagnosticTest { + + private final SwerveSubsystem swerveSubsystem; + + // Test parameters (read from SmartDashboard in initialize()) + private int moduleNumber; + private double testAngleDegrees; + private int numberOfCycles; + private double angleToleranceDegrees; + private double maxWaitTimeSeconds; + private double minHoldTimeSeconds; // Minimum time to hold at each position (for visibility in simulation) + + // Test state machine - tracks where we are in the test cycle + private enum TestState { + MOVING_TO_TARGET, // Module is rotating to the test angle + AT_TARGET, // Module has reached test angle - holding for minimum time + MOVING_TO_ZERO, // Module is rotating back to zero + AT_ZERO, // Module has reached zero - holding for minimum time + COMPLETE // All cycles finished + } + + private TestState currentState = TestState.MOVING_TO_TARGET; + private int currentCycle = 0; // Current cycle number (0-indexed, displayed as cycle+1) + private double stateStartTime = 0.0; // Timestamp when current state started (for timeout detection) + private double positionReachedTime = 0.0; // Timestamp when we reached the current target position + private SwerveModule testModule; // The module being tested + + /** + * Record to store the results of a single test cycle. + * A complete cycle consists of: + * 1. Moving to the target angle and measuring drift + * 2. Moving back to zero and measuring drift + */ + public static record TestCycleResult( + int cycleNumber, + // Measurement at target angle + double relativeEncoderAtTarget, + double absoluteEncoderAtTarget, + double driftAtTarget, + boolean wasTimeoutAtTarget, + // Measurement at zero + double relativeEncoderAtZero, + double absoluteEncoderAtZero, + double driftAtZero, + boolean wasTimeoutAtZero + ) { + /** + * Creates a TestCycleResult with measurements at both target and zero positions. + */ + public static TestCycleResult create( + int cycleNumber, + double relativeAtTarget, double absoluteAtTarget, boolean timeoutAtTarget, + double relativeAtZero, double absoluteAtZero, boolean timeoutAtZero) { + double driftAtTarget = Math.IEEEremainder(relativeAtTarget - absoluteAtTarget, 360.0); + double driftAtZero = Math.IEEEremainder(relativeAtZero - absoluteAtZero, 360.0); + return new TestCycleResult( + cycleNumber, + relativeAtTarget, absoluteAtTarget, driftAtTarget, timeoutAtTarget, + relativeAtZero, absoluteAtZero, driftAtZero, timeoutAtZero + ); + } + } + + // Test results storage - one entry per complete cycle + // Each cycle contains measurements at both target and zero positions + private TestCycleResult[] testResults; + + // Temporary storage for current cycle measurements at target position + // These are stored when we reach the target, then combined with zero measurements + // to create the complete TestCycleResult when we reach zero + private double relativeAtTarget = 0.0; + private double absoluteAtTarget = 0.0; + private boolean timeoutAtTarget = false; + + /** + * Creates a new SwerveAngleDriftTestCommand. + * Parameters are read from SmartDashboard in the initialize() method. + * + * @param swerveSubsystem The swerve subsystem containing the modules + */ + public SwerveAngleDriftTestCommand(SwerveSubsystem swerveSubsystem) { + this.swerveSubsystem = swerveSubsystem; + addRequirements(swerveSubsystem); + } + + // ============================================================================ + // DiagnosticTest Interface Implementation + // ============================================================================ + + @Override + public String getTestName() { + return "Swerve Angle Drift Test"; + } + + @Override + public String getTestDescription() { + return "Tests encoder drift by rotating a swerve module through multiple cycles and comparing " + + "relative encoder to absolute encoder measurements. This test helps identify if the relative " + + "(integrated) encoder is accumulating error over multiple cycles, which would indicate drift issues."; + } + + @Override + public void initializeParameters() { + // Set up SmartDashboard parameters with default values + TestDashboard.putParamInt(this, "ModuleNumber", 0); + TestDashboard.putParamDouble(this, "Angle", 90.0); + TestDashboard.putParamInt(this, "NumberOfCycles", 10); + TestDashboard.putParamDouble(this, "AngleTolerance", 2.0); + TestDashboard.putParamDouble(this, "MaxWaitTime", 1.0); + TestDashboard.putParamDouble(this, "MinHoldTime", 0.5); + } + + /** + * Called once by CommandScheduler when the command is first scheduled/started. + * Sets up the test: validates parameters, initializes data structures, and begins the first cycle. + * + *

This is part of WPILib's Command framework lifecycle: + *

    + *
  • Called automatically when command is scheduled (e.g., via button press or SmartDashboard trigger)
    • + *
  • Runs once at the start of the command
    • + *
  • After this, execute() will be called repeatedly
    • + *
+ */ + @Override + public void initialize() { + // Read parameters from SmartDashboard + moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); + testAngleDegrees = TestDashboard.getParamDouble(this, "Angle", 90.0); + numberOfCycles = TestDashboard.getParamInt(this, "NumberOfCycles", 10); + angleToleranceDegrees = TestDashboard.getParamDouble(this, "AngleTolerance", 2.0); + maxWaitTimeSeconds = TestDashboard.getParamDouble(this, "MaxWaitTime", 1.0); + minHoldTimeSeconds = TestDashboard.getParamDouble(this, "MinHoldTime", 0.5); + + // Validate parameters + if (moduleNumber < 0 || moduleNumber > 3) { + System.err.println("ERROR: Invalid module number: " + moduleNumber + ". Must be 0-3."); + currentState = TestState.COMPLETE; + return; + } + if (numberOfCycles < 1) { + System.err.println("ERROR: Number of cycles must be at least 1. Got: " + numberOfCycles); + currentState = TestState.COMPLETE; + return; + } + if (testAngleDegrees < 0 || testAngleDegrees >= 360) { + System.err.println("WARNING: Test angle should be 0-360 degrees. Using: " + testAngleDegrees); + } + + // Get the module to test + testModule = swerveSubsystem.getModule(moduleNumber); + if (testModule == null) { + System.err.println("ERROR: Module " + moduleNumber + " not found in swerve subsystem."); + currentState = TestState.COMPLETE; + return; + } + + // Initialize test results storage (one entry per complete cycle) + testResults = new TestCycleResult[numberOfCycles]; + + // Reset temporary storage + relativeAtTarget = 0.0; + absoluteAtTarget = 0.0; + timeoutAtTarget = false; + + // Initialize test state machine + currentCycle = 0; // Start with cycle 0 (will display as cycle 1, also used as array index) + currentState = TestState.MOVING_TO_TARGET; // First action: move to test angle + stateStartTime = Timer.getFPGATimestamp(); // Record start time for timeout detection + positionReachedTime = 0.0; // Reset position reached time + + // Begin first cycle: command module to rotate to the test angle + // Use setDesiredState to match production code behavior (includes optimization logic) + testModule.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(testAngleDegrees)), false); + + // Log test start + System.out.println("=== Swerve Angle Drift Test Started ==="); + System.out.println("Module: " + moduleNumber); + System.out.println("Test Angle: " + testAngleDegrees + " degrees"); + System.out.println("Cycles: " + numberOfCycles); + System.out.println("Tolerance: " + angleToleranceDegrees + " degrees"); + System.out.println("Min Hold Time: " + minHoldTimeSeconds + " seconds"); + System.out.println("----------------------------------------"); + + // Update SmartDashboard with organized groups + // Config group (static test setup) + TestDashboard.putResultInt(this, "Config/Module", moduleNumber); + TestDashboard.putResultInt(this, "Config/TotalCycles", numberOfCycles); + // Progress group (current test progress) + TestDashboard.putResultInt(this, "Progress/CurrentCycle", currentCycle + 1); // Display 1-indexed cycle number + TestDashboard.putResultString(this, "Progress/State", currentState.toString()); + } + + /** + * Called repeatedly by CommandScheduler every 20ms while the command is active. + * Manages the test state machine: checks if module has reached target positions, + * records measurements, and transitions between states. + * + *

This is part of WPILib's Command framework lifecycle: + *

    + *
  • Called automatically by CommandScheduler.run() (which runs in Robot.robotPeriodic())
    • + *
  • Runs every 20ms (50 times per second) while command is scheduled
    • + *
  • Continues until isFinished() returns true or command is interrupted
    • + *
+ */ + @Override + public void execute() { + // This method runs every 20ms while the command is active + // It checks if the module has reached its target position and manages the test cycle + + if (currentState == TestState.COMPLETE) { + return; + } + + double currentTime = Timer.getFPGATimestamp(); + double elapsedTime = currentTime - stateStartTime; // Time spent in current state + + switch (currentState) { + case MOVING_TO_TARGET: + // Phase 1: Wait for module to reach the test angle (e.g., 90°) + // Once reached, transition to AT_TARGET state to hold for minimum time + if (isAtAngle(testModule, testAngleDegrees, angleToleranceDegrees)) { + // Reached target - transition to hold state + currentState = TestState.AT_TARGET; + positionReachedTime = currentTime; + recordTargetMeasurement(false); + System.out.println(String.format(" Cycle %d: Reached target angle %.2f° - holding for %.2f seconds...", + currentCycle + 1, testAngleDegrees, minHoldTimeSeconds)); + } else if (elapsedTime > maxWaitTimeSeconds) { + // Timeout - transition to hold state anyway + currentState = TestState.AT_TARGET; + positionReachedTime = currentTime; + recordTargetMeasurement(true); + } + break; + + case AT_TARGET: + // Hold at target position for minimum time (for visibility in simulation) + double holdTime = currentTime - positionReachedTime; + if (holdTime >= minHoldTimeSeconds) { + // Hold time complete - transition to moving to zero + transitionToZero(currentTime); + } + break; + + case MOVING_TO_ZERO: + // Phase 2: Wait for module to return to zero + // Once reached, transition to AT_ZERO state to hold for minimum time + if (isAtAngle(testModule, 0.0, angleToleranceDegrees)) { + // Reached zero - transition to hold state + currentState = TestState.AT_ZERO; + positionReachedTime = currentTime; + recordZeroMeasurement(false); + System.out.println(String.format(" Cycle %d: Reached zero - holding for %.2f seconds...", + currentCycle + 1, minHoldTimeSeconds)); + } else if (elapsedTime > maxWaitTimeSeconds) { + // Timeout - transition to hold state anyway + currentState = TestState.AT_ZERO; + positionReachedTime = currentTime; + recordZeroMeasurement(true); + } + break; + + case AT_ZERO: + // Hold at zero position for minimum time (for visibility in simulation) + holdTime = currentTime - positionReachedTime; + if (holdTime >= minHoldTimeSeconds) { + // Hold time complete - complete the cycle + completeCycle(currentTime); + } + break; + + default: + break; + } + + // Update NetworkTables with real-time encoder and angle information for simulator testing + updateNetworkTables(); + } + + /** + * Updates NetworkTables with real-time motor angle and encoder information. + * This allows monitoring the test progress in the simulator or on SmartDashboard. + */ + private void updateNetworkTables() { + if (testModule == null) { + return; + } + + // Get current encoder values + double currentRelativeAngle = testModule.getRawTurnEncoder(); + double currentAbsoluteAngle = testModule.getCanCoder().getDegrees(); + double currentDrift = Math.IEEEremainder(currentRelativeAngle - currentAbsoluteAngle, 360.0); + + // Determine target angle based on current state + double targetAngle = (currentState == TestState.MOVING_TO_TARGET) ? testAngleDegrees : 0.0; + double angleError = Math.IEEEremainder(currentRelativeAngle - targetAngle, 360.0); + + // Get current module state for additional information + SwerveModuleState moduleState = testModule.getState(); + + // Publish to NetworkTables (accessible via SmartDashboard or NetworkTables API) + // RealTime group (live measurements during execution - alphabetical order) + TestDashboard.putResultDouble(this, "RealTime/AngleMotorVelocity", moduleState.speedMetersPerSecond); + TestDashboard.putResultDouble(this, "RealTime/CurrentAngleEncoderAbsolute", currentAbsoluteAngle); + TestDashboard.putResultDouble(this, "RealTime/CurrentAngleEncoderDrift", currentDrift); + TestDashboard.putResultDouble(this, "RealTime/CurrentAngleEncoderRelative", currentRelativeAngle); + TestDashboard.putResultDouble(this, "RealTime/TargetAngle", targetAngle); + TestDashboard.putResultDouble(this, "RealTime/TargetRelativeError", angleError); + // Progress group (update state) + TestDashboard.putResultString(this, "Progress/State", currentState.toString()); + } + + /** + * Called once by CommandScheduler when the command ends (either normally or interrupted). + * Performs cleanup and final status updates. + * + *

This is part of WPILib's Command framework lifecycle: + *

    + *
  • Called automatically when isFinished() returns true OR when command is interrupted/cancelled
    • + *
  • Runs once at the end of the command
    • + *
  • The interrupted parameter indicates if command was cancelled (true) or completed normally (false)
    • + *
+ * + * @param interrupted true if command was cancelled/interrupted, false if it completed normally + */ + @Override + public void end(boolean interrupted) { + if (interrupted) { + System.out.println("=== Swerve Angle Drift Test INTERRUPTED ==="); + TestDashboard.putResultString(this, "Status/Message", "Interrupted"); + } else { + System.out.println("=== Swerve Angle Drift Test COMPLETED ==="); + TestDashboard.putResultString(this, "Status/Message", "Complete"); + } + } + + /** + * Called by CommandScheduler every 20ms to check if the command should end. + * When this returns true, the command will end and end() will be called. + * + *

This is part of WPILib's Command framework lifecycle: + *

    + *
  • Called automatically by CommandScheduler after each execute() call
    • + *
  • If returns true, command ends and end() is called
    • + *
  • If returns false, command continues and execute() is called again next cycle
    • + *
+ * + * @return true if command should end, false to continue running + */ + @Override + public boolean isFinished() { + return currentState == TestState.COMPLETE; + } + + // ============================================================================ + // Helper Methods + // ============================================================================ + + /** + * Checks if a swerve module is at the specified angle within tolerance. + * + * This method compares the current relative encoder position to the target angle, + * accounting for the circular nature of angles (e.g., 359° is close to 1°). + * + * @param module The swerve module to check + * @param targetDegrees The target angle in degrees (0-360) + * @param toleranceDegrees The acceptable error in degrees + * @return true if the module is within tolerance of the target angle + */ + private boolean isAtAngle(SwerveModule module, double targetDegrees, double toleranceDegrees) { + double currentDegrees = module.getRawTurnEncoder(); + double error = Math.abs(Math.IEEEremainder(currentDegrees - targetDegrees, 360.0)); + return error <= toleranceDegrees; + } + + // ============================================================================ + // State Transition Methods + // ============================================================================ + + /** + * Transitions the state machine to moving to zero position. + * + * @param currentTime Current timestamp for state transition + */ + private void transitionToZero(double currentTime) { + currentState = TestState.MOVING_TO_ZERO; + stateStartTime = currentTime; + // Use setDesiredState to match production code behavior (includes optimization logic) + testModule.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0)), false); + } + + /** + * Transitions to the next test cycle or completes the test if all cycles are done. + * + * @param currentTime Current timestamp for state transition + */ + private void transitionToNextCycle(double currentTime) { + currentState = TestState.MOVING_TO_TARGET; + stateStartTime = currentTime; + // Use setDesiredState to match production code behavior (includes optimization logic) + testModule.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(testAngleDegrees)), false); + // Update SmartDashboard with new cycle number (1-indexed for display) + TestDashboard.putResultInt(this, "Progress/CurrentCycle", currentCycle + 1); + } + + /** + * Handles cycle completion: checks if more cycles are needed or finishes the test. + * + * @param currentTime Current timestamp for state transition + */ + private void completeCycle(double currentTime) { + // Cycle complete! Increment cycle counter and check if we need to run more cycles or finish + currentCycle++; + if (currentCycle >= numberOfCycles) { + // All cycles completed - print final statistics and end test + currentState = TestState.COMPLETE; + printResults(); + } else { + // Start the next cycle + transitionToNextCycle(currentTime); + } + } + + // ============================================================================ + // Recording Methods + // ============================================================================ + + /** + * Records the target position measurement. + * + * @param wasTimeout true if this measurement was taken after a timeout + */ + private void recordTargetMeasurement(boolean wasTimeout) { + // Store encoder measurements temporarily - we'll create the complete cycle result + // when we also have the zero position measurement + relativeAtTarget = testModule.getRawTurnEncoder(); + absoluteAtTarget = testModule.getCanCoder().getDegrees(); + timeoutAtTarget = wasTimeout; + + // Print measurement results + printTargetMeasurement(wasTimeout); + } + + /** + * Records the zero position measurement and creates the complete cycle result. + * + * @param wasTimeout true if this measurement was taken after a timeout + */ + private void recordZeroMeasurement(boolean wasTimeout) { + // Get zero position measurements + double relativeAtZero = testModule.getRawTurnEncoder(); + double absoluteAtZero = testModule.getCanCoder().getDegrees(); + + // Record the complete cycle result (contains both target and zero measurements) + recordCycleResult(relativeAtZero, absoluteAtZero, wasTimeout); + + // Print measurement results + printZeroMeasurement(wasTimeout, relativeAtZero, absoluteAtZero); + } + + /** + * Records the complete cycle result with both target and zero measurements. + * + * @param relativeAtZero Relative encoder value at zero position + * @param absoluteAtZero Absolute encoder value at zero position + * @param wasTimeoutAtZero true if zero measurement was taken after a timeout + */ + private void recordCycleResult(double relativeAtZero, double absoluteAtZero, boolean wasTimeoutAtZero) { + // Create and store the complete cycle result (contains both target and zero measurements) + // currentCycle is used as the array index (0-indexed) and cycle number (1-indexed for display) + testResults[currentCycle] = TestCycleResult.create( + currentCycle + 1, // Cycle number (1-indexed for display) + relativeAtTarget, absoluteAtTarget, timeoutAtTarget, // Target position data + relativeAtZero, absoluteAtZero, wasTimeoutAtZero // Zero position data + ); + } + + // ============================================================================ + // Printing Methods + // ============================================================================ + + /** + * Prints the target position measurement results. + * + * @param wasTimeout true if this measurement was taken after a timeout + */ + private void printTargetMeasurement(boolean wasTimeout) { + double driftAtTarget = Math.IEEEremainder(relativeAtTarget - absoluteAtTarget, 360.0); + + if (wasTimeout) { + System.err.println(String.format( + "WARNING: Cycle %d timed out waiting to reach target angle %.2f° (within %.2f° tolerance)", + currentCycle + 1, testAngleDegrees, angleToleranceDegrees)); + TestDashboard.putResultString(this, "Status/Message", "Timeout at Target"); + } + + System.out.println(String.format( + "%sCycle %d: Reached target (%.2f°) - Drift: %.3f° (Rel: %.2f°, Abs: %.2f°)", + wasTimeout ? " " : "", currentCycle + 1, testAngleDegrees, driftAtTarget, + relativeAtTarget, absoluteAtTarget)); + + TestDashboard.putResultDouble(this, "LastCycle/DriftAtTarget", driftAtTarget); + } + + /** + * Prints the zero position measurement results and cycle completion summary. + * + * @param wasTimeout true if this measurement was taken after a timeout + * @param relativeAtZero Relative encoder value at zero position + * @param absoluteAtZero Absolute encoder value at zero position + */ + private void printZeroMeasurement(boolean wasTimeout, double relativeAtZero, double absoluteAtZero) { + double driftAtZero = Math.IEEEremainder(relativeAtZero - absoluteAtZero, 360.0); + + if (wasTimeout) { + System.err.println(String.format( + "WARNING: Cycle %d timed out waiting to reach zero (within %.2f° tolerance)", + currentCycle + 1, angleToleranceDegrees)); + TestDashboard.putResultString(this, "Status/Message", "Timeout at Zero"); + } + + System.out.println(String.format( + "%sCycle %d: Reached zero - Drift: %.3f° (Rel: %.2f°, Abs: %.2f°)", + wasTimeout ? " " : "", currentCycle + 1, driftAtZero, relativeAtZero, absoluteAtZero)); + System.out.println(String.format( + " Cycle %d complete%s - Target drift: %.3f°, Zero drift: %.3f°", + currentCycle + 1, wasTimeout ? " (with timeout)" : "", + testResults[currentCycle].driftAtTarget(), driftAtZero)); + + TestDashboard.putResultDouble(this, "LastCycle/DriftAtZero", driftAtZero); + } + + /** + * Calculates and prints drift statistics from the collected test results. + * + * Analyzes the complete cycle results to show: + * - Statistics for drift at target position (across all cycles) + * - Statistics for drift at zero position (across all cycles) + * - Total accumulated drift over the entire test + * - Warnings if drift exceeds acceptable thresholds + */ + private void printResults() { + System.out.println("\n=== DRIFT TEST RESULTS ==="); + System.out.println("Module: " + moduleNumber); + System.out.println("Test Angle: " + testAngleDegrees + " degrees"); + System.out.println("Cycles Completed: " + currentCycle); + + if (currentCycle == 0) { + System.out.println("No cycles completed."); + System.out.println("==========================\n"); + return; + } + + // Extract drift values from each cycle for separate analysis + // We analyze target and zero positions separately to see if drift patterns differ + double[] driftAtTarget = new double[currentCycle]; + double[] driftAtZero = new double[currentCycle]; + int timeoutCountAtTarget = 0; + int timeoutCountAtZero = 0; + + for (int i = 0; i < currentCycle; i++) { + if (testResults[i] != null) { + // Extract drift values from each complete cycle + driftAtTarget[i] = testResults[i].driftAtTarget(); + driftAtZero[i] = testResults[i].driftAtZero(); + // Count timeouts for reporting + if (testResults[i].wasTimeoutAtTarget()) timeoutCountAtTarget++; + if (testResults[i].wasTimeoutAtZero()) timeoutCountAtZero++; + } + } + + System.out.println("\n--- Drift at Target Position ---"); + printDriftStats(driftAtTarget, currentCycle); + if (timeoutCountAtTarget > 0) { + System.out.println(String.format(" (%d cycle(s) recorded after timeout at target)", timeoutCountAtTarget)); + } + + System.out.println("\n--- Drift at Zero Position ---"); + printDriftStats(driftAtZero, currentCycle); + if (timeoutCountAtZero > 0) { + System.out.println(String.format(" (%d cycle(s) recorded after timeout at zero)", timeoutCountAtZero)); + } + + // Calculate total accumulated drift over all cycles + // Uses zero position measurements because we return to zero each cycle, + // making it easier to see if drift accumulates over time + if (currentCycle > 0) { + double initialDrift = driftAtZero[0]; // Drift at zero in first cycle + double finalDrift = driftAtZero[currentCycle - 1]; // Drift at zero in last cycle + double totalDrift = finalDrift - initialDrift; // How much drift accumulated + + System.out.println("\n--- Total Drift Over Test ---"); + System.out.println(String.format("Initial Drift at Zero: %.3f°", initialDrift)); + System.out.println(String.format("Final Drift at Zero: %.3f°", finalDrift)); + System.out.println(String.format("Total Accumulated Drift: %.3f°", totalDrift)); + if (currentCycle > 1) { + System.out.println(String.format("Average Drift per Cycle: %.3f°", totalDrift / (currentCycle - 1))); + } + + // Update SmartDashboard + // Summary group (final statistics after test completes) + TestDashboard.putResultDouble(this, "Summary/AvgDriftPerCycle", currentCycle > 1 ? totalDrift / (currentCycle - 1) : 0.0); + TestDashboard.putResultDouble(this, "Summary/TotalDrift", totalDrift); + + // Warning if drift is significant + if (Math.abs(totalDrift) > 5.0) { + System.out.println("\n⚠️ WARNING: Significant drift detected! (>5°)"); + System.out.println(" Consider checking:"); + System.out.println(" - PID tuning (especially I term)"); + System.out.println(" - Motor controller configuration"); + System.out.println(" - Mechanical backlash or binding"); + System.out.println(" - Encoder calibration"); + } else if (Math.abs(totalDrift) > 2.0) { + System.out.println("\n⚠️ CAUTION: Moderate drift detected (>2°)"); + } else { + System.out.println("\n✓ Drift is within acceptable range (<2°)"); + } + } + + System.out.println("==========================\n"); + } + + /** + * Prints statistics for drift values. + * + * @param driftArray Array of drift values + * @param count Number of valid values in the array + */ + private void printDriftStats(double[] driftArray, int count) { + if (count == 0) { + System.out.println("No data collected"); + return; + } + + double min = driftArray[0]; + double max = driftArray[0]; + double sum = 0.0; + + for (int i = 0; i < count; i++) { + double drift = driftArray[i]; + if (drift < min) min = drift; + if (drift > max) max = drift; + sum += drift; + } + + double avg = sum / count; + + // Calculate standard deviation + double variance = 0.0; + for (int i = 0; i < count; i++) { + double diff = driftArray[i] - avg; + variance += diff * diff; + } + double stdDev = Math.sqrt(variance / count); + + System.out.println(String.format("Cycles: %d", count)); + System.out.println(String.format("Min: %.3f°", min)); + System.out.println(String.format("Max: %.3f°", max)); + System.out.println(String.format("Average: %.3f°", avg)); + System.out.println(String.format("Std Dev: %.3f°", stdDev)); + } + +} diff --git a/src/main/java/frc/robot/test/SwerveStraightLineTestCommand.java b/src/main/java/frc/robot/test/SwerveStraightLineTestCommand.java new file mode 100644 index 0000000..5b33595 --- /dev/null +++ b/src/main/java/frc/robot/test/SwerveStraightLineTestCommand.java @@ -0,0 +1,500 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.test; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.test.DiagnosticTest; +import frc.lib.test.TestDashboard; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.SwerveModule; + +/** + * Diagnostic test that commands the robot to drive straight forward with known inputs + * and measures how each module responds. + * + *

This test bypasses the joystick entirely, feeding exact ChassisSpeeds through the + * same code path as normal driving ({@code driveFromChassisSpeeds()}). By removing human + * input variability, it reveals whether the drift problem is caused by the drive code, + * motor/encoder configuration, or mechanical issues. + * + *

Test sequence: + *

    + *
  1. Command all modules to 0° (straight forward) and wait for them to settle
    2. + *
  2. Command a known ChassisSpeeds (e.g., 1 m/s forward, 0 strafe, 0 rotation)
    3. + *
  3. Record module states (angles and velocities) at regular intervals during the drive period
    4. + *
  4. Record gyro heading to detect rotation during straight-line driving
    5. + *
  5. Stop the drive and report per-module statistics
    6. + *
+ * + *

What this test reveals: + *

    + *
  • Module angle errors during driving (modules not all pointing the same direction)
    • + *
  • Module velocity mismatches (one module spinning faster/slower than others)
    • + *
  • Gyro heading drift during straight-line driving (robot rotating when it shouldn't be)
    • + *
  • Open-loop vs closed-loop differences (by toggling the control mode parameter)
    • + *
  • Whether kinematics module ordering matches physical module positions
    • + *
+ * + *

Interpreting results: + *

    + *
  • All module angles ≈ 0° and velocities match: Drive code is correct — problem is elsewhere
    • + *
  • One module angle offset: Bad angleOffset calibration for that module
    • + *
  • Velocities differ significantly in open-loop but match in closed-loop: Normal motor variation (use closed-loop)
    • + *
  • Gyro drifts during test: Robot is physically rotating — one or more modules are pushing sideways
    • + *
  • Module angles are correct but gyro drifts: Mechanical issue (uneven wheel wear, friction, weight)
    • + *
+ */ +public class SwerveStraightLineTestCommand extends Command implements DiagnosticTest { + + private final SwerveSubsystem swerveSubsystem; + + // Test parameters (read from SmartDashboard in initialize()) + private double driveSpeedMps; // Forward speed in m/s + private double settleTimeSeconds; // Time to wait for modules to align before driving + private double driveTimeSeconds; // Duration to drive straight + private double sampleIntervalSeconds; // How often to record measurements + private boolean useClosedLoop; // Closed-loop (true) or open-loop (false) drive control + + // Test state + private enum TestState { + ALIGNING, // Pre-aligning modules to 0° before driving + DRIVING, // Driving straight and recording measurements + STOPPING, // Stopped driving, recording final state + COMPLETE // Test finished + } + + private TestState currentState; + private double stateStartTime; + private double lastSampleTime; + private int sampleCount; + private double initialYaw; + + // Results storage (per sample) + private static final int MAX_SAMPLES = 500; // 10 seconds at 20ms = 500 samples + private double[] sampleTimes; + private double[][] sampleAngles; // [sampleIndex][moduleNumber] + private double[][] sampleVelocities; // [sampleIndex][moduleNumber] + private double[] sampleYaw; // Gyro heading at each sample + + // Module references + private SwerveModule[] modules; + private static final int NUM_MODULES = 4; + + /** + * Creates a new SwerveStraightLineTestCommand. + * Parameters are read from SmartDashboard in the initialize() method. + * + * @param swerveSubsystem The swerve subsystem to test + */ + public SwerveStraightLineTestCommand(SwerveSubsystem swerveSubsystem) { + this.swerveSubsystem = swerveSubsystem; + addRequirements(swerveSubsystem); + } + + // ============================================================================ + // DiagnosticTest Interface Implementation + // ============================================================================ + + @Override + public String getTestName() { + return "Swerve Straight Line Test"; + } + + @Override + public String getTestDescription() { + return "Commands the robot to drive straight forward with known inputs (bypassing the joystick) " + + "and measures each module's angle and velocity response. Reveals angle calibration errors, " + + "velocity mismatches, and heading drift that cause the robot to not drive straight."; + } + + @Override + public void initializeParameters() { + TestDashboard.putParamDouble(this, "DriveSpeed", 1.0); + TestDashboard.putParamDouble(this, "SettleTime", 1.5); + TestDashboard.putParamDouble(this, "DriveTime", 3.0); + TestDashboard.putParamDouble(this, "SampleInterval", 0.1); + TestDashboard.putParamBoolean(this, "UseClosedLoop", false); + } + + // ============================================================================ + // Command Lifecycle + // ============================================================================ + + @Override + public void initialize() { + // Read parameters from SmartDashboard + driveSpeedMps = TestDashboard.getParamDouble(this, "DriveSpeed", 1.0); + settleTimeSeconds = TestDashboard.getParamDouble(this, "SettleTime", 1.5); + driveTimeSeconds = TestDashboard.getParamDouble(this, "DriveTime", 3.0); + sampleIntervalSeconds = TestDashboard.getParamDouble(this, "SampleInterval", 0.1); + useClosedLoop = TestDashboard.getParamBoolean(this, "UseClosedLoop", false); + + // Validate + if (driveSpeedMps <= 0) { + System.err.println("ERROR: DriveSpeed must be positive. Got: " + driveSpeedMps); + currentState = TestState.COMPLETE; + return; + } + if (driveTimeSeconds <= 0) { + System.err.println("ERROR: DriveTime must be positive. Got: " + driveTimeSeconds); + currentState = TestState.COMPLETE; + return; + } + + // Get module references + modules = new SwerveModule[NUM_MODULES]; + for (int i = 0; i < NUM_MODULES; i++) { + modules[i] = swerveSubsystem.getModule(i); + if (modules[i] == null) { + System.err.println("ERROR: Module " + i + " not found in swerve subsystem."); + currentState = TestState.COMPLETE; + return; + } + } + + // Initialize results storage + int expectedSamples = Math.min(MAX_SAMPLES, + (int) (driveTimeSeconds / sampleIntervalSeconds) + 10); + sampleTimes = new double[expectedSamples]; + sampleAngles = new double[expectedSamples][NUM_MODULES]; + sampleVelocities = new double[expectedSamples][NUM_MODULES]; + sampleYaw = new double[expectedSamples]; + sampleCount = 0; + + // Record initial yaw + initialYaw = swerveSubsystem.getYaw().getDegrees(); + + // Start by aligning all modules to 0° (straight forward) + currentState = TestState.ALIGNING; + stateStartTime = Timer.getFPGATimestamp(); + lastSampleTime = 0.0; + + // Command all modules to 0° with zero speed + SwerveModuleState alignState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0)); + for (SwerveModule mod : modules) { + mod.setDesiredState(alignState, false); + } + + // Log test start + System.out.println("=== Swerve Straight Line Test Started ==="); + System.out.println("Drive Speed: " + driveSpeedMps + " m/s"); + System.out.println("Settle Time: " + settleTimeSeconds + "s"); + System.out.println("Drive Time: " + driveTimeSeconds + "s"); + System.out.println("Control Mode: " + (useClosedLoop ? "Closed-Loop" : "Open-Loop")); + System.out.println("Initial Yaw: " + String.format("%.2f°", initialYaw)); + System.out.println("-----------------------------------------"); + + // Update dashboard + TestDashboard.putResultString(this, "Status", "Aligning modules..."); + TestDashboard.putResultString(this, "Config/ControlMode", + useClosedLoop ? "Closed-Loop" : "Open-Loop"); + TestDashboard.putResultDouble(this, "Config/DriveSpeed", driveSpeedMps); + } + + @Override + public void execute() { + if (currentState == TestState.COMPLETE) { + return; + } + + double currentTime = Timer.getFPGATimestamp(); + double elapsed = currentTime - stateStartTime; + + switch (currentState) { + case ALIGNING: + // Wait for modules to settle at 0° before starting to drive + if (elapsed >= settleTimeSeconds) { + // Start driving + currentState = TestState.DRIVING; + stateStartTime = currentTime; + lastSampleTime = currentTime; + + // Command straight-forward ChassisSpeeds through the full drive pipeline + // This uses the SAME code path as teleop driving + ChassisSpeeds straightForward = new ChassisSpeeds(driveSpeedMps, 0.0, 0.0); + swerveSubsystem.driveFromChassisSpeeds(straightForward, !useClosedLoop); + + System.out.println("Modules aligned. Driving straight at " + + driveSpeedMps + " m/s..."); + TestDashboard.putResultString(this, "Status", "Driving..."); + + // Record first sample immediately + recordSample(currentTime); + } + break; + + case DRIVING: + // Continue commanding the drive (ChassisSpeeds needs to be sent every cycle) + ChassisSpeeds straightForward = new ChassisSpeeds(driveSpeedMps, 0.0, 0.0); + swerveSubsystem.driveFromChassisSpeeds(straightForward, !useClosedLoop); + + // Record samples at the specified interval + if (currentTime - lastSampleTime >= sampleIntervalSeconds) { + recordSample(currentTime); + lastSampleTime = currentTime; + } + + // Update real-time display + updateRealTimeDisplay(); + + // Check if drive time has elapsed + if (elapsed >= driveTimeSeconds) { + // Stop driving + ChassisSpeeds stop = new ChassisSpeeds(0.0, 0.0, 0.0); + swerveSubsystem.driveFromChassisSpeeds(stop, true); + + currentState = TestState.STOPPING; + System.out.println("Drive period complete. Recording final measurements..."); + TestDashboard.putResultString(this, "Status", "Analyzing..."); + } + break; + + case STOPPING: + // Done — print results + currentState = TestState.COMPLETE; + printResults(); + break; + + default: + break; + } + } + + @Override + public boolean isFinished() { + return currentState == TestState.COMPLETE; + } + + @Override + public void end(boolean interrupted) { + // Ensure motors are stopped + ChassisSpeeds stop = new ChassisSpeeds(0.0, 0.0, 0.0); + swerveSubsystem.driveFromChassisSpeeds(stop, true); + + if (interrupted) { + System.out.println("=== Swerve Straight Line Test INTERRUPTED ==="); + TestDashboard.putResultString(this, "Status", "Interrupted"); + } else { + System.out.println("=== Swerve Straight Line Test COMPLETED ==="); + TestDashboard.putResultString(this, "Status", "Complete"); + } + } + + // ============================================================================ + // Data Recording + // ============================================================================ + + /** + * Records a single sample of all module states and gyro heading. + */ + private void recordSample(double timestamp) { + if (sampleCount >= sampleTimes.length) { + return; // Buffer full + } + + sampleTimes[sampleCount] = timestamp - stateStartTime; // Time relative to drive start + + for (int i = 0; i < NUM_MODULES; i++) { + SwerveModuleState state = modules[i].getState(); + sampleAngles[sampleCount][i] = state.angle.getDegrees(); + sampleVelocities[sampleCount][i] = state.speedMetersPerSecond; + } + + sampleYaw[sampleCount] = swerveSubsystem.getYaw().getDegrees(); + sampleCount++; + } + + /** + * Updates real-time SmartDashboard display during driving. + */ + private void updateRealTimeDisplay() { + double currentYaw = swerveSubsystem.getYaw().getDegrees(); + double yawDrift = Math.IEEEremainder(currentYaw - initialYaw, 360.0); + TestDashboard.putResultDouble(this, "RealTime/YawDrift", yawDrift); + + for (int i = 0; i < NUM_MODULES; i++) { + SwerveModuleState state = modules[i].getState(); + TestDashboard.putResultDouble(this, "RealTime/Mod" + i + "Angle", + state.angle.getDegrees()); + TestDashboard.putResultDouble(this, "RealTime/Mod" + i + "Velocity", + state.speedMetersPerSecond); + } + } + + // ============================================================================ + // Results Reporting + // ============================================================================ + + /** + * Prints comprehensive results analysis. + */ + private void printResults() { + System.out.println("\n=== STRAIGHT LINE TEST RESULTS ==="); + System.out.println("Samples collected: " + sampleCount); + System.out.println("Control mode: " + (useClosedLoop ? "Closed-Loop" : "Open-Loop")); + System.out.println("Commanded speed: " + driveSpeedMps + " m/s"); + + if (sampleCount == 0) { + System.out.println("No samples collected."); + System.out.println("==================================\n"); + return; + } + + // === Module Angle Analysis === + System.out.println("\n--- Module Angle Analysis ---"); + System.out.println("(All modules should read ≈ 0° for straight-forward driving)"); + System.out.println(String.format(" %-10s %-12s %-12s %-12s %-12s", + "Module", "Avg Angle", "Std Dev", "Min", "Max")); + + for (int mod = 0; mod < NUM_MODULES; mod++) { + double sum = 0, min = Double.MAX_VALUE, max = -Double.MAX_VALUE; + for (int s = 0; s < sampleCount; s++) { + double angle = sampleAngles[s][mod]; + sum += angle; + if (angle < min) min = angle; + if (angle > max) max = angle; + } + double avg = sum / sampleCount; + + double variance = 0; + for (int s = 0; s < sampleCount; s++) { + double diff = sampleAngles[s][mod] - avg; + variance += diff * diff; + } + double stdDev = Math.sqrt(variance / sampleCount); + + String status; + if (Math.abs(avg) <= 2.0 && stdDev <= 1.0) { + status = "✓"; + } else if (Math.abs(avg) <= 5.0) { + status = "⚠️"; + } else { + status = "✗"; + } + + System.out.println(String.format(" Mod %-5d %-12.2f %-12.2f %-12.2f %-12.2f %s", + mod, avg, stdDev, min, max, status)); + + TestDashboard.putResultDouble(this, "Summary/Mod" + mod + "/AvgAngle", avg); + TestDashboard.putResultDouble(this, "Summary/Mod" + mod + "/AngleStdDev", stdDev); + } + + // === Module Velocity Analysis === + System.out.println("\n--- Module Velocity Analysis ---"); + System.out.println(String.format("(All modules should read ≈ %.2f m/s)", driveSpeedMps)); + System.out.println(String.format(" %-10s %-12s %-12s %-12s %-12s", + "Module", "Avg Vel", "Std Dev", "Min", "Max")); + + double[] avgVelocities = new double[NUM_MODULES]; + for (int mod = 0; mod < NUM_MODULES; mod++) { + double sum = 0, min = Double.MAX_VALUE, max = -Double.MAX_VALUE; + for (int s = 0; s < sampleCount; s++) { + double vel = sampleVelocities[s][mod]; + sum += vel; + if (vel < min) min = vel; + if (vel > max) max = vel; + } + double avg = sum / sampleCount; + avgVelocities[mod] = avg; + + double variance = 0; + for (int s = 0; s < sampleCount; s++) { + double diff = sampleVelocities[s][mod] - avg; + variance += diff * diff; + } + double stdDev = Math.sqrt(variance / sampleCount); + + double pctError = Math.abs(avg - driveSpeedMps) / driveSpeedMps * 100.0; + String status; + if (pctError <= 10.0) { + status = "✓"; + } else if (pctError <= 25.0) { + status = "⚠️"; + } else { + status = "✗"; + } + + System.out.println(String.format(" Mod %-5d %-12.3f %-12.3f %-12.3f %-12.3f %s (%.1f%% error)", + mod, avg, stdDev, min, max, status, pctError)); + + TestDashboard.putResultDouble(this, "Summary/Mod" + mod + "/AvgVelocity", avg); + TestDashboard.putResultDouble(this, "Summary/Mod" + mod + "/VelStdDev", stdDev); + } + + // Velocity spread (difference between fastest and slowest module) + double minAvgVel = avgVelocities[0], maxAvgVel = avgVelocities[0]; + for (int mod = 1; mod < NUM_MODULES; mod++) { + if (avgVelocities[mod] < minAvgVel) minAvgVel = avgVelocities[mod]; + if (avgVelocities[mod] > maxAvgVel) maxAvgVel = avgVelocities[mod]; + } + double velSpread = maxAvgVel - minAvgVel; + System.out.println(String.format("\n Velocity spread (fastest - slowest): %.3f m/s", velSpread)); + if (velSpread > 0.3) { + System.out.println(" ✗ Significant velocity mismatch — modules are not driving at the same speed."); + System.out.println(" → If open-loop: Try closed-loop control (motor variation is normal in open-loop)."); + System.out.println(" → If closed-loop: Check PID/feedforward tuning, or mechanical drag on one module."); + } else if (velSpread > 0.1) { + System.out.println(" ⚠️ Moderate velocity mismatch — may contribute to drift."); + } else { + System.out.println(" ✓ Velocities are well-matched across modules."); + } + TestDashboard.putResultDouble(this, "Summary/VelocitySpread", velSpread); + + // === Gyro Heading Analysis === + System.out.println("\n--- Gyro Heading Analysis ---"); + double finalYaw = sampleYaw[sampleCount - 1]; + double totalYawDrift = Math.IEEEremainder(finalYaw - initialYaw, 360.0); + + System.out.println(String.format(" Initial Yaw: %.2f°", initialYaw)); + System.out.println(String.format(" Final Yaw: %.2f°", finalYaw)); + System.out.println(String.format(" Total Drift: %.2f°", totalYawDrift)); + System.out.println(String.format(" Drift Rate: %.2f °/s", totalYawDrift / driveTimeSeconds)); + + if (Math.abs(totalYawDrift) <= 2.0) { + System.out.println(" ✓ Heading is stable — robot drove straight."); + } else if (Math.abs(totalYawDrift) <= 10.0) { + System.out.println(" ⚠️ Moderate heading drift — robot is turning slightly."); + } else { + System.out.println(" ✗ Significant heading drift — robot is veering off course."); + } + + TestDashboard.putResultDouble(this, "Summary/TotalYawDrift", totalYawDrift); + TestDashboard.putResultDouble(this, "Summary/YawDriftRate", totalYawDrift / driveTimeSeconds); + + // === Diagnostic Summary === + System.out.println("\n--- Diagnostic Summary ---"); + + // Check if angle errors could explain drift + boolean angleIssue = false; + for (int mod = 0; mod < NUM_MODULES; mod++) { + double sum = 0; + for (int s = 0; s < sampleCount; s++) { + sum += sampleAngles[s][mod]; + } + double avg = sum / sampleCount; + if (Math.abs(avg) > 3.0) { + System.out.println(String.format( + " ⚠️ Module %d average angle is %.2f° (should be ≈ 0°). " + + "Check angleOffset calibration.", mod, avg)); + angleIssue = true; + } + } + if (!angleIssue && Math.abs(totalYawDrift) > 5.0) { + System.out.println(" Module angles look correct but robot still drifts."); + System.out.println(" Likely causes: velocity mismatch, mechanical (uneven wheels/friction), or weight distribution."); + } + if (!angleIssue && Math.abs(totalYawDrift) <= 2.0 && velSpread <= 0.1) { + System.out.println(" ✓ All measurements look good. Drive code appears correct."); + System.out.println(" If the robot still drifts with joystick input, the issue may be in"); + System.out.println(" TeleopSwerve (deadband, input scaling) or field-oriented heading."); + } + + System.out.println("\n==================================\n"); + } +} diff --git a/src/test/java/frc/lib/test/DiagnosticTestValidationTest.java b/src/test/java/frc/lib/test/DiagnosticTestValidationTest.java new file mode 100644 index 0000000..9b3e997 --- /dev/null +++ b/src/test/java/frc/lib/test/DiagnosticTestValidationTest.java @@ -0,0 +1,209 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.lib.test; + +import static org.junit.jupiter.api.Assertions.*; + +import org.junit.jupiter.api.Test; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.test.DiagnosticTest; +import frc.lib.test.TestDashboard; +import frc.robot.test.SwerveAngleDriftTestCommand; + +/** + * Basic validation tests for diagnostic test framework functionality. + * These tests check for obvious runtime errors like null pointer exceptions, + * parameter validation, and basic logic correctness. + * + * Note: These tests don't require hardware and can be run on any machine. + */ +class DiagnosticTestValidationTest { + + /** Set to true to enable verbose output for debugging. */ + private static final boolean DEBUG = false; + + /** + * Simple test command that validates parameters. This allows testing parameter + * validation logic without requiring the full command or hardware. + */ + private static class ParameterValidationTestCommand extends Command implements DiagnosticTest { + private enum ValidationState { + VALID, + COMPLETE // Set when validation fails + } + + private ValidationState state = ValidationState.VALID; + + @Override + public String getTestName() { + return "Parameter Validation Test"; + } + + @Override + public void initializeParameters() { + // Set up SmartDashboard parameters with default values (same as SwerveAngleDriftTestCommand) + TestDashboard.putParamInt(this, "ModuleNumber", 0); + TestDashboard.putParamDouble(this, "Angle", 90.0); + TestDashboard.putParamInt(this, "NumberOfCycles", 10); + TestDashboard.putParamDouble(this, "AngleTolerance", 2.0); + TestDashboard.putParamDouble(this, "MaxWaitTime", 1.0); + TestDashboard.putParamDouble(this, "MinHoldTime", 0.5); + } + + @Override + public void initialize() { + // Read parameters from SmartDashboard (same as SwerveAngleDriftTestCommand) + int moduleNumber = TestDashboard.getParamInt(this, "ModuleNumber", 0); + double testAngleDegrees = TestDashboard.getParamDouble(this, "Angle", 90.0); + int numberOfCycles = TestDashboard.getParamInt(this, "NumberOfCycles", 10); + + // Validate parameters (same validation logic as SwerveAngleDriftTestCommand) + if (moduleNumber < 0 || moduleNumber > 3) { + if (DEBUG) System.err.println("ERROR: Invalid module number: " + moduleNumber + ". Must be 0-3."); + state = ValidationState.COMPLETE; + return; + } + if (numberOfCycles < 1) { + if (DEBUG) System.err.println("ERROR: Number of cycles must be at least 1. Got: " + numberOfCycles); + state = ValidationState.COMPLETE; + return; + } + if (testAngleDegrees < 0 || testAngleDegrees >= 360) { + if (DEBUG) System.err.println("WARNING: Test angle should be 0-360 degrees. Using: " + testAngleDegrees); + } + + // Simulate module null check (in real command, this would call swerveSubsystem.getModule()) + // For testing, we'll simulate this by checking if moduleNumber is valid but module is null + // In this simple test, we'll just validate the parameters themselves + state = ValidationState.VALID; + } + + @Override + public boolean isFinished() { + return state == ValidationState.COMPLETE; + } + + @Override + public void execute() { + // No-op for this test command + } + } + + /** + * Tests that TestCycleResult record can be created with valid data. + * This verifies the record structure and drift calculation logic. + */ + @Test + void testCycleResultCreation() { + // Test with typical values + SwerveAngleDriftTestCommand.TestCycleResult result = + SwerveAngleDriftTestCommand.TestCycleResult.create( + 1, // cycleNumber + 90.0, 100.0, false, // relativeAtTarget, absoluteAtTarget, timeoutAtTarget + 0.0, 5.0, false // relativeAtZero, absoluteAtZero, timeoutAtZero + ); + + assertNotNull(result); + assertEquals(1, result.cycleNumber()); + assertEquals(90.0, result.relativeEncoderAtTarget()); + assertEquals(100.0, result.absoluteEncoderAtTarget()); + assertEquals(0.0, result.relativeEncoderAtZero()); + assertEquals(5.0, result.absoluteEncoderAtZero()); + assertFalse(result.wasTimeoutAtTarget()); + assertFalse(result.wasTimeoutAtZero()); + + // Verify drift calculation (should be -10.0 degrees at target, -5.0 at zero) + assertEquals(-10.0, result.driftAtTarget(), 0.001); + assertEquals(-5.0, result.driftAtZero(), 0.001); + } + + /** + * Tests drift calculation with angles that wrap around 360 degrees. + */ + @Test + void testCycleResultDriftCalculationWithWrapAround() { + // Test case: relative encoder at 350°, absolute at 10° + // The drift should be calculated correctly accounting for wrap-around + SwerveAngleDriftTestCommand.TestCycleResult result = + SwerveAngleDriftTestCommand.TestCycleResult.create( + 1, + 350.0, 10.0, false, // 350° - 10° = 340°, but normalized should be -20° + 0.0, 0.0, false + ); + + // IEEEremainder(350 - 10, 360) = IEEEremainder(340, 360) = -20 + assertEquals(-20.0, result.driftAtTarget(), 0.001); + } + + /** + * Tests that parameter validation works correctly. + * Uses a simple local test command. This test verifies that invalid parameters + * cause the command to finish immediately. + */ + @Test + void testParameterValidation() { + // Test 1: Invalid module number (< 0) + ParameterValidationTestCommand testCommand = new ParameterValidationTestCommand(); + testCommand.initializeParameters(); + TestDashboard.putParamInt(testCommand, "ModuleNumber", -1); + TestDashboard.putParamInt(testCommand, "NumberOfCycles", 10); + TestDashboard.putParamDouble(testCommand, "Angle", 90.0); + testCommand.initialize(); + assertTrue(testCommand.isFinished(), "Command should finish immediately with invalid module number < 0"); + + // Test 2: Invalid module number (> 3) + testCommand = new ParameterValidationTestCommand(); + testCommand.initializeParameters(); + TestDashboard.putParamInt(testCommand, "ModuleNumber", 4); + TestDashboard.putParamInt(testCommand, "NumberOfCycles", 10); + TestDashboard.putParamDouble(testCommand, "Angle", 90.0); + testCommand.initialize(); + assertTrue(testCommand.isFinished(), "Command should finish immediately with invalid module number > 3"); + + // Test 3: Invalid number of cycles (< 1) + testCommand = new ParameterValidationTestCommand(); + testCommand.initializeParameters(); + TestDashboard.putParamInt(testCommand, "ModuleNumber", 0); + TestDashboard.putParamInt(testCommand, "NumberOfCycles", 0); + TestDashboard.putParamDouble(testCommand, "Angle", 90.0); + testCommand.initialize(); + assertTrue(testCommand.isFinished(), "Command should finish immediately with numberOfCycles < 1"); + + // Test 4: Valid parameters should pass validation + testCommand = new ParameterValidationTestCommand(); + testCommand.initializeParameters(); + TestDashboard.putParamInt(testCommand, "ModuleNumber", 0); + TestDashboard.putParamInt(testCommand, "NumberOfCycles", 10); + TestDashboard.putParamDouble(testCommand, "Angle", 90.0); + testCommand.initialize(); + assertFalse(testCommand.isFinished(), "Command should continue with valid parameters"); + } + + /** + * Tests that SmartDashboard parameter initialization works correctly. + * Verifies that initializeParameters() sets up all required parameters with correct default values. + */ + @Test + void testDashboardInitialization() { + ParameterValidationTestCommand testCommand = new ParameterValidationTestCommand(); + + // Call initializeParameters() as the DiagnosticTestManager framework would + testCommand.initializeParameters(); + + // Verify all parameters are initialized with correct default values + assertEquals(0, TestDashboard.getParamInt(testCommand, "ModuleNumber", -999), + "ModuleNumber should default to 0"); + assertEquals(90.0, TestDashboard.getParamDouble(testCommand, "Angle", -999.0), 0.001, + "Angle should default to 90.0 degrees"); + assertEquals(10, TestDashboard.getParamInt(testCommand, "NumberOfCycles", -999), + "NumberOfCycles should default to 10"); + assertEquals(2.0, TestDashboard.getParamDouble(testCommand, "AngleTolerance", -999.0), 0.001, + "AngleTolerance should default to 2.0 degrees"); + assertEquals(1.0, TestDashboard.getParamDouble(testCommand, "MaxWaitTime", -999.0), 0.001, + "MaxWaitTime should default to 1.0 seconds"); + assertEquals(0.5, TestDashboard.getParamDouble(testCommand, "MinHoldTime", -999.0), 0.001, + "MinHoldTime should default to 0.5 seconds"); + } +} diff --git a/src/test/java/frc/robot/auto/TrajectoryGenerationTest.java b/src/test/java/frc/robot/auto/TrajectoryGenerationTest.java new file mode 100644 index 0000000..b25238c --- /dev/null +++ b/src/test/java/frc/robot/auto/TrajectoryGenerationTest.java @@ -0,0 +1,411 @@ +package frc.robot.auto; + +import static org.junit.jupiter.api.Assertions.*; + +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.path.Waypoint; +import com.pathplanner.lib.trajectory.PathPlannerTrajectory; +import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.system.plant.DCMotor; + +import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.SwerveConstants; + +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; + +import java.util.List; + +/** + * Tests that PathPlanner trajectory generation produces valid, non-degenerate + * trajectories for our robot configuration. + * + *

All test paths are constructed programmatically so they are immune to + * changes in the team's actual auto path files (8FuelPath, DriveTestPath, etc.). + * + *

Motivation: In simulation, FollowPathCommand finished after one execute() + * cycle because the generated trajectory had totalTime=0.0. The root cause was + * that ModuleConfig.maxDriveVelocityMPS was set to the software speed limit + * (3.0 m/s) instead of the motor's physical free speed (~5.35 m/s), causing + * PathPlanner to compute zero available torque for acceleration. + * + *

See docs/auto-sim-log-analysis.md (Section 6) on the jmm-auto-mode-debug + * branch for the full root cause analysis. + * + *

Set {@link #DEBUG} to {@code true} for verbose output during development. + */ +class TrajectoryGenerationTest { + + /** Set to true to enable verbose output (trajectory states, config values, etc.) */ + private static final boolean DEBUG = false; + + // Standard constraints used by most tests — similar to our real path constraints + private static final PathConstraints STANDARD_CONSTRAINTS = + new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI); + + @BeforeAll + static void initHAL() { + // HAL must be initialized for PathPlannerPath internals to work. + HAL.initialize(500, 0); + } + + // ========================================================================= + // Test path factories — programmatic paths independent of path files + // ========================================================================= + + /** + * A simple 2-meter straight line from (1,1) to (3,1), heading east. + * The simplest possible non-trivial path. + */ + private static PathPlannerPath createStraightLinePath() { + List waypoints = PathPlannerPath.waypointsFromPoses( + new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0)), + new Pose2d(3.0, 1.0, Rotation2d.fromDegrees(0)) + ); + PathPlannerPath path = new PathPlannerPath( + waypoints, STANDARD_CONSTRAINTS, null, + new GoalEndState(0.0, Rotation2d.fromDegrees(0))); + path.preventFlipping = true; + return path; + } + + /** + * An S-curve with 3 waypoints: starts at (1,1) heading up-right, curves + * through (3,3) heading right, then curves back down to (5,1) heading + * down-right. Total distance ~5.7m. Tests curved trajectory generation. + */ + private static PathPlannerPath createSCurvePath() { + List waypoints = PathPlannerPath.waypointsFromPoses( + new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(45)), // start heading up-right + new Pose2d(3.0, 3.0, Rotation2d.fromDegrees(0)), // midpoint heading right + new Pose2d(5.0, 1.0, Rotation2d.fromDegrees(-45)) // end heading down-right + ); + PathPlannerPath path = new PathPlannerPath( + waypoints, STANDARD_CONSTRAINTS, null, + new GoalEndState(0.0, Rotation2d.fromDegrees(-45))); + path.preventFlipping = true; + return path; + } + + // ========================================================================= + // Debug helpers — output is suppressed unless DEBUG = true + // ========================================================================= + + private void debugPrint(String msg) { + if (DEBUG) System.out.println(msg); + } + + private void debugPrintf(String fmt, Object... args) { + if (DEBUG) System.out.printf(fmt, args); + } + + private void dumpTrajectory(String label, PathPlannerTrajectory traj) { + if (!DEBUG) return; + var states = traj.getStates(); + System.out.println("\n=== " + label + " ==="); + System.out.printf(" totalTime = %.6f s%n", traj.getTotalTimeSeconds()); + System.out.printf(" stateCount = %d%n", states.size()); + System.out.printf(" isFinite(totalTime) = %b%n", Double.isFinite(traj.getTotalTimeSeconds())); + for (int i = 0; i < states.size(); i++) { + PathPlannerTrajectoryState s = states.get(i); + System.out.printf(" state[%2d] t=%8.4f pose=(%7.3f, %7.3f, %7.2f°) vel=%7.3f m/s heading=%7.2f° fieldSpeeds=(vx=%.3f, vy=%.3f, omega=%.3f)%n", + i, s.timeSeconds, + s.pose.getX(), s.pose.getY(), s.pose.getRotation().getDegrees(), + s.linearVelocity, + s.heading.getDegrees(), + s.fieldSpeeds.vxMetersPerSecond, s.fieldSpeeds.vyMetersPerSecond, + s.fieldSpeeds.omegaRadiansPerSecond); + } + } + + private void dumpPathPoints(String label, PathPlannerPath path) { + if (!DEBUG) return; + List points = path.getAllPathPoints(); + System.out.println("\n--- " + label + " path points (" + points.size() + ") ---"); + for (int i = 0; i < points.size(); i++) { + PathPoint p = points.get(i); + System.out.printf(" point[%2d] pos=(%7.3f, %7.3f) distAlongPath=%.4f waypointRelPos=%.4f%n", + i, p.position.getX(), p.position.getY(), + p.distanceAlongPath, p.waypointRelativePos); + } + } + + private void dumpConfig(String label, RobotConfig config) { + if (!DEBUG) return; + System.out.println("\n--- " + label + " ---"); + System.out.printf(" massKG = %.2f%n", config.massKG); + System.out.printf(" MOI = %.2f%n", config.MOI); + System.out.printf(" numModules = %d%n", config.numModules); + System.out.printf(" isHolonomic = %b%n", config.isHolonomic); + System.out.printf(" wheelFrictionForce = %.4f N%n", config.wheelFrictionForce); + System.out.printf(" maxTorqueFriction = %.4f Nm%n", config.maxTorqueFriction); + for (int i = 0; i < config.numModules; i++) { + System.out.printf(" moduleLocation[%d] = (%7.4f, %7.4f) pivotDist=%.4f%n", + i, config.moduleLocations[i].getX(), config.moduleLocations[i].getY(), + config.modulePivotDistance[i]); + } + ModuleConfig mc = config.moduleConfig; + System.out.printf(" ModuleConfig:%n"); + System.out.printf(" wheelRadiusMeters = %.4f%n", mc.wheelRadiusMeters); + System.out.printf(" maxDriveVelocityMPS = %.3f%n", mc.maxDriveVelocityMPS); + System.out.printf(" maxDriveVelocityRadPerSec = %.3f%n", mc.maxDriveVelocityRadPerSec); + System.out.printf(" wheelCOF = %.3f%n", mc.wheelCOF); + System.out.printf(" driveCurrentLimit = %.1f A%n", mc.driveCurrentLimit); + System.out.printf(" torqueLoss = %.4f Nm%n", mc.torqueLoss); + System.out.printf(" driveMotor stallTorque = %.3f Nm, freeSpeed = %.1f rad/s%n", + mc.driveMotor.stallTorqueNewtonMeters, mc.driveMotor.freeSpeedRadPerSec); + } + + // ========================================================================= + // Test: Robot configuration values are sane + // ========================================================================= + @Test + void testRobotConfigValues() { + RobotConfig config = AutoConstants.ROBOT_CONFIG; + dumpConfig("AutoConstants.ROBOT_CONFIG", config); + + assertTrue(config.massKG > 0, "Mass must be positive"); + assertTrue(config.MOI > 0, "MOI must be positive"); + assertEquals(4, config.numModules, "Should have 4 swerve modules"); + assertTrue(config.isHolonomic, "Swerve robot should be holonomic"); + assertTrue(config.wheelFrictionForce > 0, "Friction force must be positive"); + assertTrue(config.maxTorqueFriction > 0, "Max torque friction must be positive"); + + ModuleConfig mc = config.moduleConfig; + assertTrue(mc.wheelRadiusMeters > 0, "Wheel radius must be positive"); + assertTrue(mc.maxDriveVelocityMPS > 0, "Max velocity must be positive"); + assertTrue(mc.torqueLoss >= 0, "Torque loss must be non-negative"); + + // Critical check: torqueLoss should be LESS than the stall torque, otherwise + // the robot can never accelerate from rest + double stallTorque = mc.driveMotor.stallTorqueNewtonMeters; + debugPrintf("%n CRITICAL CHECK: torqueLoss (%.4f) vs stallTorque (%.4f) => %s%n", + mc.torqueLoss, stallTorque, + mc.torqueLoss < stallTorque ? "OK — robot can accelerate" : "PROBLEM — torqueLoss >= stallTorque!"); + assertTrue(mc.torqueLoss < stallTorque, + "torqueLoss must be less than stall torque, otherwise robot can never accelerate. " + + "torqueLoss=" + mc.torqueLoss + " stallTorque=" + stallTorque); + } + + // ========================================================================= + // Test: Straight line path generates a valid trajectory + // ========================================================================= + @Test + void testStraightLinePath() { + PathPlannerPath path = createStraightLinePath(); + dumpPathPoints("Straight line (2m)", path); + + PathPlannerTrajectory traj = path.generateTrajectory( + new ChassisSpeeds(), Rotation2d.kZero, AutoConstants.ROBOT_CONFIG); + dumpTrajectory("Straight line (2m)", traj); + + assertTrue(traj.getTotalTimeSeconds() > 0, + "Straight line trajectory should have non-zero totalTime. Got: " + + traj.getTotalTimeSeconds()); + + var first = traj.getInitialState(); + var last = traj.getEndState(); + double dist = first.pose.getTranslation().getDistance(last.pose.getTranslation()); + debugPrintf(" Start-to-end distance = %.4f m%n", dist); + assertTrue(dist > 1.0, "Start and end should be at least 1.0m apart. Got: " + dist); + } + + // ========================================================================= + // Test: S-curve path generates a valid trajectory + // ========================================================================= + @Test + void testSCurvePath() { + PathPlannerPath path = createSCurvePath(); + dumpPathPoints("S-curve (~5.7m)", path); + + PathPlannerTrajectory traj = path.generateTrajectory( + new ChassisSpeeds(), Rotation2d.fromDegrees(45), AutoConstants.ROBOT_CONFIG); + dumpTrajectory("S-curve (~5.7m)", traj); + + assertTrue(traj.getTotalTimeSeconds() > 0, + "S-curve trajectory should have non-zero totalTime. Got: " + + traj.getTotalTimeSeconds()); + + // Verify states span real distance + var states = traj.getStates(); + double maxDist = 0; + for (int i = 1; i < states.size(); i++) { + double d = states.get(i).pose.getTranslation().getDistance( + states.get(0).pose.getTranslation()); + maxDist = Math.max(maxDist, d); + } + debugPrintf(" Max distance from first state = %.4f m%n", maxDist); + assertTrue(maxDist > 1.0, "States should span at least 1.0m. Got: " + maxDist); + } + + // ========================================================================= + // Test: S-curve after flipPath (simulating red alliance) + // ========================================================================= + @Test + void testSCurvePath_flipped() { + PathPlannerPath path = createSCurvePath(); + PathPlannerPath flipped = path.flipPath(); + dumpPathPoints("S-curve flipped", flipped); + + PathPlannerTrajectory traj = flipped.generateTrajectory( + new ChassisSpeeds(), Rotation2d.fromDegrees(-45 + 180), AutoConstants.ROBOT_CONFIG); + dumpTrajectory("S-curve flipped", traj); + + assertTrue(traj.getTotalTimeSeconds() > 0, + "Flipped S-curve trajectory should have non-zero totalTime. Got: " + + traj.getTotalTimeSeconds()); + } + + // ========================================================================= + // Test: S-curve after mirrorPath (simulating right-side auto) + // ========================================================================= + @Test + void testSCurvePath_mirrored() { + PathPlannerPath path = createSCurvePath(); + PathPlannerPath mirrored = path.mirrorPath(); + dumpPathPoints("S-curve mirrored", mirrored); + + PathPlannerTrajectory traj = mirrored.generateTrajectory( + new ChassisSpeeds(), Rotation2d.fromDegrees(-45), AutoConstants.ROBOT_CONFIG); + dumpTrajectory("S-curve mirrored", traj); + + assertTrue(traj.getTotalTimeSeconds() > 0, + "Mirrored S-curve trajectory should have non-zero totalTime. Got: " + + traj.getTotalTimeSeconds()); + } + + // ========================================================================= + // Test: Prove the old config was broken and the current config is fixed + // + // OLD BUG: maxDriveVelocityMPS was set to 3.0 m/s (software speed limit). + // At 3.0 m/s the motor draws 94.6A, but driveCurrentLimit = 40A. + // Both torqueLoss and forward-pass torque clamp to 40A → same torque + // → zero available torque → zero acceleration → totalTime = 0. + // + // FIX: maxDriveVelocityMPS is now the motor's theoretical free speed + // (~5.35 m/s), where the motor draws only 3.6A — well below the 40A limit. + // ========================================================================= + @Test + void testRootCause_currentLimitCausesZeroAcceleration() { + DCMotor motor = DCMotor.getNeoVortex(1).withReduction(SwerveConstants.driveGearRatio); + double wheelRadius = SwerveConstants.wheelDiameter / 2.0; + double currentLimit = SwerveConstants.driveContinuousCurrentLimit; // 40A + + // --- Part 1: Prove the OLD config (maxSpeed = 3.0 m/s) was broken --- + double oldMaxSpeed = 3.0; // the old (buggy) value + ModuleConfig oldMC = new ModuleConfig( + wheelRadius, oldMaxSpeed, 1.2, motor, currentLimit, 1); + + double oldMaxSpeedRad = oldMaxSpeed / wheelRadius; + double oldCurrentAtMaxSpeed = motor.getCurrent(oldMaxSpeedRad, 12.0); + double oldClampedCurrent = Math.min(oldCurrentAtMaxSpeed, currentLimit); + double oldTorqueLoss = Math.max(motor.getTorque(oldClampedCurrent), 0.0); + + double stallCurrent = motor.getCurrent(0, 12.0); + double clampedStallCurrent = Math.min(stallCurrent, currentLimit); + double stallTorque = motor.getTorque(clampedStallCurrent); + double oldAvailableTorque = stallTorque - oldTorqueLoss; + + debugPrint("\n=== OLD CONFIG (BROKEN) — maxDriveVelocityMPS = 3.0 m/s ==="); + debugPrintf(" currentAtMaxSpeed = %.2f A (clamped to %.0f A → %.2f A)%n", + oldCurrentAtMaxSpeed, currentLimit, oldClampedCurrent); + debugPrintf(" torqueLoss = %.6f Nm%n", oldTorqueLoss); + debugPrintf(" stallCurrent = %.2f A (clamped to %.0f A → %.2f A)%n", + stallCurrent, currentLimit, clampedStallCurrent); + debugPrintf(" stallTorque = %.6f Nm%n", stallTorque); + debugPrintf(" availableTorque = %.6f − %.6f = %.6f Nm ← ZERO!%n", + stallTorque, oldTorqueLoss, oldAvailableTorque); + + // Confirm the bug: both currents clamp to 40A → same torque → zero available + assertEquals(oldClampedCurrent, clampedStallCurrent, 0.001, + "BUG CONFIRMED: with maxSpeed=3.0, both currents clamp to " + currentLimit + "A"); + assertEquals(0.0, oldAvailableTorque, 0.001, + "BUG CONFIRMED: available torque should be zero with old config"); + + // Generate trajectory with old config → totalTime should be 0 + PathPlannerPath path = createSCurvePath(); + RobotConfig oldConfig = new RobotConfig(52, 6.8, oldMC, + SwerveConstants.FRONT_LEFT, SwerveConstants.FRONT_RIGHT, + SwerveConstants.BACK_RIGHT, SwerveConstants.BACK_LEFT); + PathPlannerTrajectory oldTraj = path.generateTrajectory( + new ChassisSpeeds(), Rotation2d.fromDegrees(45), oldConfig); + debugPrintf(" OLD trajectory totalTime = %.6f s (expected: 0.0)%n", + oldTraj.getTotalTimeSeconds()); + assertEquals(0.0, oldTraj.getTotalTimeSeconds(), 0.001, + "Old config should produce a zero-duration trajectory"); + + // --- Part 2: Verify the CURRENT config (fix applied) works --- + ModuleConfig fixedMC = AutoConstants.MODULE_CONFIG; + double fixedMaxSpeedRad = fixedMC.maxDriveVelocityMPS / fixedMC.wheelRadiusMeters; + double fixedCurrentAtMaxSpeed = motor.getCurrent(fixedMaxSpeedRad, 12.0); + double fixedClampedCurrent = Math.min(fixedCurrentAtMaxSpeed, fixedMC.driveCurrentLimit); + double fixedTorqueLoss = Math.max(motor.getTorque(fixedClampedCurrent), 0.0); + double fixedAvailableTorque = stallTorque - fixedTorqueLoss; + + debugPrintf("%n=== FIXED CONFIG — maxDriveVelocityMPS = %.3f m/s ===\n", + fixedMC.maxDriveVelocityMPS); + debugPrintf(" currentAtMaxSpeed = %.2f A (below %.0f A limit? %b)%n", + fixedCurrentAtMaxSpeed, fixedMC.driveCurrentLimit, + fixedCurrentAtMaxSpeed < fixedMC.driveCurrentLimit); + debugPrintf(" torqueLoss = %.6f Nm%n", fixedTorqueLoss); + debugPrintf(" availableTorque = %.6f − %.6f = %.6f Nm ← NON-ZERO!%n", + stallTorque, fixedTorqueLoss, fixedAvailableTorque); + + // Current at theoretical free speed should be well below the 40A limit + assertTrue(fixedCurrentAtMaxSpeed < fixedMC.driveCurrentLimit, + "FIXED: current at max speed (" + fixedCurrentAtMaxSpeed + + "A) should be below limit (" + fixedMC.driveCurrentLimit + "A)"); + assertTrue(fixedAvailableTorque > 1.0, + "FIXED: available torque should be substantial. Got: " + fixedAvailableTorque); + + // Generate trajectory with fixed config → should have real duration + PathPlannerTrajectory fixedTraj = path.generateTrajectory( + new ChassisSpeeds(), Rotation2d.fromDegrees(45), AutoConstants.ROBOT_CONFIG); + dumpTrajectory("S-curve — FIXED CONFIG", fixedTraj); + assertTrue(fixedTraj.getTotalTimeSeconds() > 0.5, + "FIXED: trajectory should have meaningful totalTime. Got: " + + fixedTraj.getTotalTimeSeconds()); + } + + // ========================================================================= + // Test: Verify path points have non-zero distanceAlongPath spread + // ========================================================================= + @Test + void testPathPointsAreDistinct() { + debugPrint("\n=== Path point distance check ==="); + checkPathPointSpread("straight line", createStraightLinePath()); + checkPathPointSpread("S-curve", createSCurvePath()); + checkPathPointSpread("S-curve flipped", createSCurvePath().flipPath()); + checkPathPointSpread("S-curve mirrored", createSCurvePath().mirrorPath()); + } + + private void checkPathPointSpread(String label, PathPlannerPath path) { + List points = path.getAllPathPoints(); + debugPrintf(" %s: %d points%n", label, points.size()); + assertTrue(points.size() >= 2, label + ": Path should have at least 2 points"); + + Translation2d first = points.get(0).position; + Translation2d last = points.get(points.size() - 1).position; + double dist = first.getDistance(last); + debugPrintf(" first=(%7.3f, %7.3f) last=(%7.3f, %7.3f) dist=%.4f m%n", + first.getX(), first.getY(), last.getX(), last.getY(), dist); + assertTrue(dist > 0.5, + label + ": First and last path points should be >0.5m apart. Got: " + dist); + + double totalDist = points.get(points.size() - 1).distanceAlongPath; + debugPrintf(" totalDistanceAlongPath = %.4f m%n", totalDist); + assertTrue(totalDist > 0.5, + label + ": Total distance along path should be >0.5m. Got: " + totalDist); + } +} diff --git a/vendordeps/Phoenix5-frc2026-latest.json b/vendordeps/Phoenix5-frc2026-latest.json deleted file mode 100644 index 2350393..0000000 --- a/vendordeps/Phoenix5-frc2026-latest.json +++ /dev/null @@ -1,171 +0,0 @@ -{ - "fileName": "Phoenix5-frc2026-latest.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.36.0", - "frcYear": "2026", - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2026-latest.json", - "requires": [ - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6-frc2026-latest.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json" - } - ], - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", - "offlineFileName": "Phoenix6-replay-frc2026-latest.json" - }, - { - "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", - "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", - "offlineFileName": "Phoenix5-replay-frc2026-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.36.0" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.36.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.36.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.36.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.36.0", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.36.0", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.36.0", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.36.0", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.36.0", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.36.0", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file