Refactor swerve drive, add unit tests, and merge latest main

.cursor/cloud-install.sh

@@ -15,3 +15,33 @@ export PATH="${JAVA_HOME}/bin:${PATH}"
 
 chmod +x gradlew
 ./gradlew classes --no-daemon
+
+# FRC desktop tools (Flutter/GTK) for cloud Desktop tab: PathPlanner + Elastic (WPILib NT).
+install_frc_desktop_tools() {
+  sudo DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends \
+    curl unzip libegl1 libgl1 ca-certificates
+
+  local pp_version="2026.1.2"
+  local elastic_version="2026.1.2"
+
+  if [[ ! -x /opt/pathplanner/pathplanner ]]; then
+    curl -fsSL -o /tmp/PathPlanner-Linux.zip \
+      "https://github.com/mjansen4857/pathplanner/releases/download/v${pp_version}/PathPlanner-Linux-v${pp_version}.zip"
+    sudo rm -rf /opt/pathplanner
+    sudo unzip -o /tmp/PathPlanner-Linux.zip -d /opt/pathplanner
+    sudo chmod +x /opt/pathplanner/pathplanner
+  fi
+
+  if [[ ! -x /opt/elastic/elastic_dashboard ]]; then
+    curl -fsSL -o /tmp/Elastic-WPILib-Linux.zip \
+      "https://github.com/Gold872/elastic_dashboard/releases/download/v${elastic_version}/Elastic-WPILib-Linux.zip"
+    sudo rm -rf /opt/elastic
+    sudo unzip -o /tmp/Elastic-WPILib-Linux.zip -d /opt/elastic
+    sudo chmod +x /opt/elastic/elastic_dashboard
+  fi
+
+  sudo ln -sf /opt/pathplanner/pathplanner /usr/local/bin/pathplanner
+  sudo ln -sf /opt/elastic/elastic_dashboard /usr/local/bin/elastic
+}
+
+install_frc_desktop_tools

.cursor/rules/frc-commands-subsystems.mdc

@@ -6,15 +6,18 @@ alwaysApply: false
 
 # Subsystems vs commands
 
-- **Subsystems** (`subsystems/**`): hardware, `periodic()`, pose/closed-loop APIs (`drive()`, `stop()`, setpoints). No Xbox bindings or `Command` factories on subsystems when avoidable — use `commands/**` factories (e.g. `DriveCommands`).
+- **Subsystems** (`subsystems/**`): hardware, `periodic()`, pose/closed-loop APIs (`drive()`, `stop()`, setpoints). No Xbox bindings or `Command` factories on subsystems when avoidable.
+- **HID** (`hid/**`): custom controllers, button maps, and shared axis helpers (`HIDUtils`). Driver stick scaling and Xbox drive axes live on `DemonCommandXboxController`, not in `commands/`.
+- **Commands** (`commands/**`): wire subsystems to operator intent using HID APIs (e.g. `DriveCommands` + `DemonCommandXboxController`).
 - **RobotContainer:** wire default commands, triggers, and `NamedCommands.registerCommand` only; keep binding logic thin.
-- **Commands:** use `run` / `runOnce` / `runEnd` / `Commands.sequence` with subsystem requirements. Name commands with `.withName(...)` for Shuffleboard and debugging.
+- Command bodies: use `run` / `runOnce` / `runEnd` / `Commands.sequence` with subsystem requirements. Name commands with `.withName(...)` for Shuffleboard and debugging.
 - **Constants:** season field poses and practice coordinates in `constants/FieldConstants`, not buried in subsystems.
-- **Autos:** PathPlanner uses `SwerveDrive` via `AutoBuilder` (pose, reset, chassis speeds). Do not break those method signatures without updating PathPlanner integration.
+- **Autos:** PathPlanner integrates through the drive subsystem (`AutoBuilder`: field pose, reset, chassis speeds, module outputs). Keep that integration contract stable when refactoring drive code.
 
 ```java
 // Prefer: factory in commands/, subsystem API stays imperative
-DriveCommands.driveWithXboxController(swerve, controller, () -> true, deadband, sensitivity);
+DriveCommands.driveWithXboxController(swerve, driverController, () -> true, deadband, sensitivity);
+// driverController: DemonCommandXboxController — use getDriveTranslation() / getDriveRotation()
 
 // Avoid: long teleop runEnd blocks inside SwerveDrive
 ```

.cursor/rules/frc-unit-tests.mdc

@@ -6,8 +6,26 @@ alwaysApply: false
 
 # Unit tests
 
-- **Runner:** JUnit 5 (`org.junit.jupiter`). Use **Arrange / Act / Assert** comments in non-trivial tests.
-- **Pure logic:** test helpers and math without HAL (`SwerveDynamics`, `SwerveModuleOptimization`, `SwerveTeleopInputs` package helpers, `AngleUtils`, `HIDUtils`).
-- **HAL / vendor sim:** `@ExtendWith(WpilibSimulationExtension.class)` for Xbox sim, SparkMaxSim, etc. Use isolated CAN IDs in tests; call `SwerveModule.close()` in `@AfterEach` when constructing real modules.
-- **Do not** instantiate full `Robot` or `SwerveDrive` in unit tests unless building a dedicated test fixture — prefer testing extracted pure functions.
+- **Runner:** JUnit 5 (`org.junit.jupiter`).
+
+## Arrange–Act–Assert (AAA)
+
+Structure every `@Test` (and each logical block in parameterized tests) with section comments:
+
+- `// Arrange` — inputs, fakes, sim state, and anything set in the test body (shared `@BeforeEach` setup counts as arrange; repeat here only when test-specific).
+- `// Act` — the single call or short sequence under test (command schedule/run, kinematics step, sim tick loop).
+- `// Assert` — `assert*` / `assertTrue` / `assertEquals` only; no new mutations.
+
+Rules of thumb:
+
+- Prefer **one** primary Act per test. Stateful lifecycle tests may use multiple **Act → Assert** pairs (e.g. schedule then cancel).
+- One-liner tests: use `// Arrange & Act` then `// Assert`, or split if it stays readable.
+- `@ParameterizedTest`: `// Arrange` may note `@CsvSource` / provider; Act and Assert stay explicit.
+- Do not omit AAA on “small” tests — consistency beats brevity.
+- **Pure logic:** prefer tests that do not need HAL — package-visible helpers, small util/math types, command input mapping, drive kinematics/optimization helpers. No CAN, no full robot graph.
+- **HAL / vendor sim:** `@ExtendWith(WpilibSimulationExtension.class)` when tests touch WPILib sim or vendor sim (joysticks, Spark, CTRE). Use isolated CAN IDs; close module/hardware handles in `@AfterEach` when tests construct real devices.
+- **Do not** instantiate `Robot` or `RobotContainer` in unit tests. `SwerveDrive` takes a `Supplier<Rotation2d>` heading (NavX in production) so tests can use `new SwerveDrive(() -> Rotation2d.kZero, false)` without the static init cycle; prefer `configurePathPlanner = false` in tests.
+- **Sim `SwerveDrive`:** use `SwerveDriveTestFixture.open()` + `resetBetweenTests()` in `@BeforeEach` / `@AfterEach` (one drivetrain per JVM, not per method). Do **not** call `SwerveDriveTestFixture.closeActive()` from per-class `@AfterAll` (class order is undefined). Do **not** rely on `@Order` — tests must pass with random method order (`junit-platform.properties`).
+- **Joysticks + `WpilibSimulationExtension`:** construct a new `CommandXboxController` / `XboxControllerSim` in `@BeforeEach`; the extension resets Driver Station sim data each test.
+- Extract pure logic to package-visible static helpers when a full subsystem instance is still too heavy (see `testsupport/` for HAL sim setup).
 - After changes, run `./gradlew test` (Java 17 toolchain from `build.gradle`).

AGENTS.md

@@ -8,18 +8,33 @@ chmod +x gradlew
 ./gradlew build   # full compile + tests (matches GitHub Actions CI)
 ```
 
-Cloud install (`.cursor/environment.json` → `cloud-install.sh`) installs OpenJDK 17 at `/usr/lib/jvm/java-17-openjdk-amd64` and compiles with `./gradlew classes`. There is no boot service; this is a Gradle robot project. Verify with `./gradlew test` and `./gradlew build`.
+CI uses the same `build` task (WPILib cross container, Java 17 per `build.gradle`).
+
+## Cloud agent environment
+
+Environment is **config as code**: `.cursor/environment.json` runs `.cursor/cloud-install.sh` (JDK, `./gradlew classes`, and desktop **PathPlanner** / **Elastic** on the agent VM for the Desktop tab). There is no boot service. After changing the install script, merge to `main` and run a new team environment **build**.
+
+For manual checks on Desktop: start WPILib sim (`./gradlew simulateJava`), enable the robot in Driver Station sim, then use `elastic` (NT dashboard) and `pathplanner` (open the **repo root** as the robot project). Deploy assets live under `src/main/deploy/`. Vendor libraries for sim/build come from Gradle `vendordeps`, not separate vendor desktop installers.
 
 ## Architecture
 
-- WPILib **command-based** robot. Subsystems own hardware; `frc.robot.commands` holds command factories; `RobotContainer` binds controls and PathPlanner **named commands**.
-- Swerve: `subsystems/drive/`, teleop commands in `commands/drive/` when present on branch.
+WPILib **command-based** robot:
+
+- **`Robot` / `RobotContainer`** — subsystem instances, default commands, button bindings, PathPlanner named commands.
+- **`subsystems/`** — hardware, `periodic()`, and imperative control APIs (no Xbox wiring when avoidable).
+- **`commands/`** — command factories grouped by mechanism (e.g. drive utilities under `commands/drive/`).
+- **`hid/`** — driver HID wrappers (`DemonCommandXboxController`, `OperatorStick`), shared axis math in `HIDUtils`; put stick/button semantics here, not in commands.
+- **`constants/`**, **`*Config`**, **`RobotMap`** — field/season poses, tuning, CAN IDs.
+- **`src/main/deploy/`** — PathPlanner paths/autos, Elastic layout, other deploy files.
+
+**Swerve:** drivetrain code under `subsystems/drive/`; operator-facing drive commands under `commands/drive/`. PathPlanner autos integrate through `SwerveDrive`’s pose and chassis-speed APIs—keep that contract stable when refactoring.
 
-## Cursor rules
+## Tests and conventions
 
-See `.cursor/rules/*.mdc` for FRC-specific conventions.
+Run `./gradlew test` (or `build`) when changing behavior. Use **Arrange / Act / Assert** section comments in tests (`// Arrange`, `// Act`, `// Assert`); details in `.cursor/rules/frc-unit-tests.mdc`. Other FRC conventions: `.cursor/rules/*.mdc`.
 
 ## PR expectations
 
-- Keep robot behavior changes scoped and describe driver/auto impact in the PR.
-- Ensure CI (`build`) passes before merge.
+- Scope changes narrowly; note **driver** and **auto** impact.
+- PathPlanner named commands in Java must match the PathPlanner GUI strings exactly.
+- CI `build` should pass before merge.

settings.gradle

@@ -3,6 +3,7 @@ import org.gradle.internal.os.OperatingSystem
 pluginManagement {
     repositories {
         gradlePluginPortal()
+        mavenCentral()
         mavenLocal()
         String frcYear = '2026'
         File frcHome

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

@@ -117,6 +117,12 @@ public void testInit() {
     CommandScheduler.getInstance().cancelAll();
   }
 
+  /** Runs vendor physics sim (REV Spark) for swerve modules during desktop simulation. */
+  @Override
+  public void simulationPeriodic() {
+    RobotContainer.kSwerveDrive.simulationPeriodic();
+  }
+
   @Override
   public void testPeriodic() {
   }

src/main/java/frc/robot/RobotContainer.java

@@ -20,6 +20,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.commands.drive.DriveCommands;
+import frc.robot.constants.FieldConstants;
 import frc.robot.hid.DemonCommandXboxController;
 import frc.robot.hid.OperatorStick;
 import frc.robot.subsystems.cameras.CameraConfig.PhotonVisionConfig;
@@ -87,7 +89,7 @@ public Command getAutonomousCommand() {
   // https://docs.wpilib.org/en/stable/docs/software/commandbased/subsystems.html
 
   // Subsystems - Mechanisms
-  public static final SwerveDrive kSwerveDrive = new SwerveDrive();
+  public static final SwerveDrive kSwerveDrive = new SwerveDrive(() -> kNavx.getRotation2d());
   public static final Turret kTurret = new Turret();
   public static final Intake kIntake = new Intake();
   public static final Indexer kIndexer = new Indexer();
@@ -132,8 +134,12 @@ public RobotContainer() {
    */
   private void configureSubsystemDefaultCommands() {
     // control swerve drive with the xbox controller by default
-    kSwerveDrive.setDefaultCommand(kSwerveDrive.driveWithXboxController(kDriverController, () -> true,
-       DemonCommandXboxController.kJoystickDeadband, DemonCommandXboxController.kJoystickSensitivity));
+    kSwerveDrive.setDefaultCommand(DriveCommands.driveWithXboxController(
+        kSwerveDrive,
+        kDriverController,
+        () -> true,
+        DemonCommandXboxController.kJoystickDeadband,
+        DemonCommandXboxController.kJoystickSensitivity));
 
     // AprilTag updates for odometry
     // kPhotonCamera1.setDefaultCommand(
@@ -173,8 +179,10 @@ private void configureXboxControllerBindings() {
     // reset robot heading - ALWAYS FACE RED ALLIANCE WHEN DOING THIS - this is
     // // useful during driver practice to reset for field oriented driving direction
     // // or a rare odd scenario on the field during a match
-    kDriverController.resetRobotHeading().onTrue(kSwerveDrive.resetHeading(() -> 1.016, () -> 4.0));
-    // kDriverController.resetRobotHeading().onTrue(kSwerveDrive.resetHeading(() -> kSwerveDrive.getPose().getX(), () -> kSwerveDrive.getPose().getY()));
+    kDriverController.resetRobotHeading().onTrue(DriveCommands.resetHeading(
+        kSwerveDrive,
+        () -> FieldConstants.kDriverPracticeResetX,
+        () -> FieldConstants.kDriverPracticeResetY));
 
     /*
     STOW INTAKE = DOWN BUTTON
@@ -200,12 +208,15 @@ private void configureXboxControllerBindings() {
 
     // // give driver ability to limit speeds for when elevator is high up to
     // // help prevent tipping over - useful for slight alignment adjustments too
-    kDriverController.goSlow().whileTrue(kSwerveDrive.goSlow());
+    kDriverController.goSlow().whileTrue(DriveCommands.goSlow(kSwerveDrive));
 
-    // // For defense / to make the robot harder to move
     kDriverController.lockHeadingForDefense()
-      .whileTrue(kSwerveDrive.holdCurrentHeadingWhileDriving(kDriverController, () -> true,
-        DemonCommandXboxController.kJoystickDeadband, DemonCommandXboxController.kJoystickSensitivity));
+        .whileTrue(DriveCommands.holdCurrentHeadingWhileDriving(
+            kSwerveDrive,
+            kDriverController,
+            () -> true,
+            DemonCommandXboxController.kJoystickDeadband,
+            DemonCommandXboxController.kJoystickSensitivity));
   }
 
 
@@ -275,7 +286,7 @@ private void configureNamedCommandsForAuto() {
 
 
     // Swerve Drive
-    NamedCommands.registerCommand("STOPSwerve", kSwerveDrive.stopCommand());
+    NamedCommands.registerCommand("STOPSwerve", DriveCommands.stop(kSwerveDrive));
 
 
     // Intake

src/main/java/frc/robot/commands/drive/DriveCommands.java

@@ -0,0 +1,130 @@
+package frc.robot.commands.drive;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.hid.DemonCommandXboxController;
+import frc.robot.subsystems.drive.SwerveDrive;
+
+/**
+ * Factory methods for swerve {@link Command}s bound in {@link frc.robot.RobotContainer}.
+ *
+ * <p>Keeps {@link SwerveDrive} focused on hardware and control loops; commands encode operator
+ * intent (teleop, defense, pose reset, etc.).
+ */
+public final class DriveCommands {
+  private static final double kSlowTranslationalSpeedMetersPerSecond = 0.5;
+  private static final double kSlowAngularSpeedRadiansPerSecond = Math.toRadians(90);
+  private static final double kNormalTranslationalSpeedMetersPerSecond = 4.6;
+  private static final double kNormalAngularSpeedRadiansPerSecond = Math.toRadians(540);
+
+  private DriveCommands() {}
+
+  /**
+   * Default teleop drive: left stick translation, right stick rotation, optional field-relative.
+   *
+   * @param swerve drivetrain subsystem
+   * @param controller driver Xbox controller
+   * @param fieldRelative when true, translation is field-oriented
+   * @param joystickDeadband axis magnitude treated as zero below this value
+   * @param joystickSensitivity cubic blend [0, 1]; 0 = linear, 1 = full cubic
+   * @return command that runs until interrupted
+   */
+  public static Command driveWithXboxController(
+      SwerveDrive swerve,
+      DemonCommandXboxController controller,
+      BooleanSupplier fieldRelative,
+      double joystickDeadband,
+      double joystickSensitivity) {
+    return swerve.runEnd(
+        () -> {
+          var translation = controller.getDriveTranslation(
+              joystickDeadband,
+              joystickSensitivity,
+              SwerveDrive.maxTranslationalSpeedMetersPerSecond);
+          double rotationSpeed = controller.getDriveRotation(
+              joystickDeadband,
+              joystickSensitivity,
+              SwerveDrive.maxAngularSpeedRadiansPerSecond);
+          int sign = DemonCommandXboxController.fieldRelativeAllianceSign(fieldRelative);
+          swerve.drive(sign * translation.xMetersPerSecond(), sign * translation.yMetersPerSecond(), rotationSpeed,
+              fieldRelative.getAsBoolean());
+        },
+        swerve::stop)
+        .withName("DriveWithXboxController");
+  }
+
+  /**
+   * Captures heading on start, then translates while PID holds that heading (defense / bump alignment).
+   */
+  public static Command holdCurrentHeadingWhileDriving(
+      SwerveDrive swerve,
+      DemonCommandXboxController controller,
+      BooleanSupplier fieldRelative,
+      double joystickDeadband,
+      double joystickSensitivity) {
+    Rotation2d[] headingToHold = new Rotation2d[1];
+    Command setHeading = swerve.runOnce(() -> headingToHold[0] = swerve.getPose().getRotation());
+    Command holdHeading = swerve.runEnd(
+        () -> {
+          var translation = controller.getDriveTranslation(
+              joystickDeadband,
+              joystickSensitivity,
+              SwerveDrive.maxTranslationalSpeedMetersPerSecond);
+          int sign = DemonCommandXboxController.fieldRelativeAllianceSign(fieldRelative);
+          swerve.holdAngleWhileDriving(
+              sign * translation.xMetersPerSecond(),
+              sign * translation.yMetersPerSecond(),
+              headingToHold[0],
+              1.0,
+              fieldRelative.getAsBoolean());
+        },
+        swerve::stop);
+    return Commands.sequence(setHeading, holdHeading).withName("SwerveHoldCurrentHeadingWhileDriving");
+  }
+
+  /**
+   * Temporarily lowers {@link SwerveDrive#maxTranslationalSpeedMetersPerSecond} and max angular speed
+   * while held (e.g. fine alignment when elevated).
+   */
+  public static Command goSlow(SwerveDrive swerve) {
+    return Commands.startEnd(
+        () -> {
+          SwerveDrive.maxTranslationalSpeedMetersPerSecond = kSlowTranslationalSpeedMetersPerSecond;
+          SwerveDrive.maxAngularSpeedRadiansPerSecond = kSlowAngularSpeedRadiansPerSecond;
+        },
+        () -> {
+          SwerveDrive.maxTranslationalSpeedMetersPerSecond = kNormalTranslationalSpeedMetersPerSecond;
+          SwerveDrive.maxAngularSpeedRadiansPerSecond = kNormalAngularSpeedRadiansPerSecond;
+        })
+        .withName("SwerveGoSlow");
+  }
+
+  /**
+   * Resets odometry pose to ({@code x}, {@code y}) with heading 0° (blue alliance forward).
+   *
+   * @param xMeters field X in meters
+   * @param yMeters field Y in meters
+   */
+  public static Command resetHeading(SwerveDrive swerve, DoubleSupplier xMeters, DoubleSupplier yMeters) {
+    return swerve.runOnce(() -> swerve.resetPose(
+        new Pose2d(xMeters.getAsDouble(), yMeters.getAsDouble(), Rotation2d.kZero)))
+        .withName("ResetRobotHeading");
+  }
+
+  /** One-shot {@link SwerveDrive#stop()}; registered as PathPlanner named command {@code STOPSwerve}. */
+  public static Command stop(SwerveDrive swerve) {
+    return swerve.runOnce(swerve::stop).withName("StopSwerve");
+  }
+
+  /**
+   * Holds modules in an X configuration ({@link SwerveDrive#lock()}) while active, then stops on end.
+   */
+  public static Command lock(SwerveDrive swerve) {
+    return swerve.runEnd(swerve::lock, swerve::stop).withName("SwerveDriveLock");
+  }
+}