diff --git a/.cursor/cloud-install.sh b/.cursor/cloud-install.sh index a7c397b..7a7b9d2 100755 --- a/.cursor/cloud-install.sh +++ b/.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 diff --git a/.cursor/rules/frc-commands-subsystems.mdc b/.cursor/rules/frc-commands-subsystems.mdc index f17024f..b661e08 100644 --- a/.cursor/rules/frc-commands-subsystems.mdc +++ b/.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 ``` diff --git a/.cursor/rules/frc-unit-tests.mdc b/.cursor/rules/frc-unit-tests.mdc index 9c76fb5..61747dd 100644 --- a/.cursor/rules/frc-unit-tests.mdc +++ b/.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` 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`). diff --git a/AGENTS.md b/AGENTS.md index 434820c..539b56b 100644 --- a/AGENTS.md +++ b/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. diff --git a/settings.gradle b/settings.gradle index 88d7e3e..a04a24c 100644 --- a/settings.gradle +++ b/settings.gradle @@ -3,6 +3,7 @@ import org.gradle.internal.os.OperatingSystem pluginManagement { repositories { gradlePluginPortal() + mavenCentral() mavenLocal() String frcYear = '2026' File frcHome diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 65fc02f..a46c371 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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() { } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8bd2cb2..398eb72 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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 diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java new file mode 100644 index 0000000..bcb2f8a --- /dev/null +++ b/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}. + * + *

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"); + } +} diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java new file mode 100644 index 0000000..7c299b0 --- /dev/null +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -0,0 +1,25 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +/** + * Field layout values that change each FRC season (poses, landmarks, practice positions). + * + *

Keep game-specific numbers here instead of in {@link frc.robot.subsystems.drive.SwerveDrive} or + * commands so they are easy to update year over year. + */ +public final class FieldConstants { + private FieldConstants() {} + + /** + * Pose used when the robot boots before vision or a driver reset. Update for the current field and + * default auto start. + */ + public static final Pose2d kDefaultOdometryPose = new Pose2d(8.5, 4.0, Rotation2d.kZero); + + /** Field X (m) for driver “reset heading” practice spot — see {@link frc.robot.commands.drive.DriveCommands#resetHeading}. */ + public static final double kDriverPracticeResetX = 1.016; + /** Field Y (m) for driver practice reset. */ + public static final double kDriverPracticeResetY = 4.0; +} diff --git a/src/main/java/frc/robot/hid/DemonCommandXboxController.java b/src/main/java/frc/robot/hid/DemonCommandXboxController.java index 416fa0a..0968d13 100644 --- a/src/main/java/frc/robot/hid/DemonCommandXboxController.java +++ b/src/main/java/frc/robot/hid/DemonCommandXboxController.java @@ -4,6 +4,10 @@ package frc.robot.hid; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -24,6 +28,70 @@ public DemonCommandXboxController(int port) { super(port); } + /** + * Translational speeds for swerve teleop from the left stick (robot-forward X, robot-left Y). + * + * @param maxTranslationalMetersPerSecond scale at full deflection + */ + public DriveTranslation getDriveTranslation( + double deadband, double sensitivity, double maxTranslationalMetersPerSecond) { + return driveTranslationFromAxes( + getLeftY(), + getLeftX(), + deadband, + sensitivity, + maxTranslationalMetersPerSecond); + } + + /** @see #getDriveTranslation(double, double, double) using {@link #kJoystickDeadband} and {@link #kJoystickSensitivity} */ + public DriveTranslation getDriveTranslation(double maxTranslationalMetersPerSecond) { + return getDriveTranslation(kJoystickDeadband, kJoystickSensitivity, maxTranslationalMetersPerSecond); + } + + /** Angular rate for swerve teleop from the right stick X axis. */ + public double getDriveRotation(double deadband, double sensitivity, double maxAngularRadiansPerSecond) { + return rotationFromAxis(getRightX(), deadband, sensitivity, maxAngularRadiansPerSecond); + } + + /** + * Field-relative translation sign for red alliance ({@code -1}) vs robot-relative or blue ({@code 1}). + */ + public static int fieldRelativeAllianceSign(boolean fieldRelative, Alliance alliance) { + return fieldRelative && alliance == Alliance.Red ? -1 : 1; + } + + /** @see #fieldRelativeAllianceSign(boolean, Alliance) using {@link DriverStation#getAlliance()} */ + public static int fieldRelativeAllianceSign(BooleanSupplier fieldRelative) { + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + return fieldRelativeAllianceSign(fieldRelative.getAsBoolean(), alliance); + } + + /** + * Pure left-stick mapping for unit tests (Xbox: forward is {@code leftY = -1}). + */ + static DriveTranslation driveTranslationFromAxes( + double leftY, + double leftX, + double deadband, + double sensitivity, + double maxTranslationalMetersPerSecond) { + double xSpeed = HIDUtils.scaledStickAxis(-leftY, deadband, sensitivity, maxTranslationalMetersPerSecond); + double ySpeed = HIDUtils.scaledStickAxis(-leftX, deadband, sensitivity, maxTranslationalMetersPerSecond); + return new DriveTranslation(xSpeed, ySpeed); + } + + /** Pure right-stick X mapping for unit tests. */ + static double rotationFromAxis( + double rightX, + double deadband, + double sensitivity, + double maxAngularRadiansPerSecond) { + return HIDUtils.scaledStickAxis(-rightX, deadband, sensitivity, maxAngularRadiansPerSecond); + } + + /** Robot-frame translational speeds after deadband and scaling (m/s). */ + public record DriveTranslation(double xMetersPerSecond, double yMetersPerSecond) {} + public Trigger resetRobotHeading() { return this.start().debounce(0.5); } diff --git a/src/main/java/frc/robot/hid/HIDUtils.java b/src/main/java/frc/robot/hid/HIDUtils.java index 35c7bb3..107ff07 100644 --- a/src/main/java/frc/robot/hid/HIDUtils.java +++ b/src/main/java/frc/robot/hid/HIDUtils.java @@ -26,4 +26,20 @@ private HIDUtils() { public static double smoothInputWithCubic(double input, double sensitivity) { return sensitivity * Math.pow(input, 3) + (1 - sensitivity) * input; } + + /** + * Deadband, cubic smoothing, and scaling for a single joystick axis. + * + * @param rawAxis raw axis value from the HID + * @param deadband magnitudes at or below this are treated as zero + * @param sensitivity cubic blend [0, 1]; 0 = linear, 1 = full cubic + * @param maxOutput scale when axis is saturated (e.g. m/s or rad/s) + * @return scaled output, or 0 inside the deadband + */ + public static double scaledStickAxis(double rawAxis, double deadband, double sensitivity, double maxOutput) { + if (Math.abs(rawAxis) <= deadband) { + return 0.0; + } + return smoothInputWithCubic(rawAxis, sensitivity) * maxOutput; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index 9c28b84..7adfb05 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -7,12 +7,8 @@ package frc.robot.subsystems.drive; -import java.util.Optional; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import com.fasterxml.jackson.databind.ser.std.NumberSerializers.DoubleSerializer; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; @@ -25,8 +21,6 @@ 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.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -34,584 +28,338 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.simulation.SimHooks; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.RobotMap; -import frc.robot.hid.HIDUtils; -import frc.robot.subsystems.cameras.AprilTagTarget; -import frc.robot.RobotContainer; +import frc.robot.constants.FieldConstants; +import frc.robot.util.AngleUtils; /** - * Swerve drive style drivetrain. - * - * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/intro-and-chassis-speeds.html - * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html - * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html + * Holonomic swerve drivetrain subsystem. + * + *

Owns four {@link SwerveModule}s, fuses wheel odometry with vision, and exposes low-level + * {@link #drive(double, double, double, boolean)} for commands and autos. PathPlanner + * {@link com.pathplanner.lib.auto.AutoBuilder} is configured in the constructor. + * + *

Driver-facing {@link edu.wpi.first.wpilibj2.command.Command}s are built in + * {@link frc.robot.commands.drive.DriveCommands}. + * + * @see SwerveDriveConfig + * @see frc.robot.constants.FieldConstants */ public class SwerveDrive extends SubsystemBase { - /** Deadband for X and Y input speeds for joystick driving. */ private static final double kDriveVelocityDeadband = 0.005; - /** Deadband for rotational input speed for joystick driving. */ private static final double kDriveOmegaDeadband = 0.005; - /** Max allowable translational speed of the robot in meters per second. */ + /** Upper bound on translational speed used by teleop and heading hold (m/s). Tunable on Shuffleboard. */ public static double maxTranslationalSpeedMetersPerSecond = 4.6; - /** Max allowable angular speed of the robot in radians per second. */ + /** Upper bound on rotation rate used by teleop and heading hold (rad/s). Tunable on Shuffleboard. */ public static double maxAngularSpeedRadiansPerSecond = Math.toRadians(540); - private Translation2d centerOfRotation = Translation2d.kZero; - private Rotation2d headingToHold = Rotation2d.fromDegrees(0); - - private final SwerveModule kFrontLeft = new SwerveModule( - "FrontLeft", - RobotMap.FRONT_LEFT_DRIVE, - RobotMap.FRONT_LEFT_PIVOT, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_P, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_I, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_D, - SwerveDriveConfig.DRIVE_MODULE_P, - SwerveDriveConfig.DRIVE_MODULE_I, - SwerveDriveConfig.DRIVE_MODULE_D, - SwerveDriveConfig.DRIVE_MODULE_KS, - SwerveDriveConfig.DRIVE_MODULE_KV, - SwerveDriveConfig.DRIVE_MODULE_KA, - RobotMap.FRONT_LEFT_PIVOT_ENCODER, + private final SwerveModule kFrontLeft = createModule( + "FrontLeft", RobotMap.FRONT_LEFT_DRIVE, RobotMap.FRONT_LEFT_PIVOT, RobotMap.FRONT_LEFT_PIVOT_ENCODER, SwerveDriveConfig.kFrontLeftPivotOffsetDegrees); - private final SwerveModule kFrontRight = new SwerveModule( - "FrontRight", - RobotMap.FRONT_RIGHT_DRIVE, - RobotMap.FRONT_RIGHT_PIVOT, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_P, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_I, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_D, - SwerveDriveConfig.DRIVE_MODULE_P, - SwerveDriveConfig.DRIVE_MODULE_I, - SwerveDriveConfig.DRIVE_MODULE_D, - SwerveDriveConfig.DRIVE_MODULE_KS, - SwerveDriveConfig.DRIVE_MODULE_KV, - SwerveDriveConfig.DRIVE_MODULE_KA, - RobotMap.FRONT_RIGHT_PIVOT_ENCODER, + private final SwerveModule kFrontRight = createModule( + "FrontRight", RobotMap.FRONT_RIGHT_DRIVE, RobotMap.FRONT_RIGHT_PIVOT, RobotMap.FRONT_RIGHT_PIVOT_ENCODER, SwerveDriveConfig.kFrontRightPivotOffsetDegrees); - private final SwerveModule kRearLeft = new SwerveModule( - "RearLeft", - RobotMap.REAR_LEFT_DRIVE, - RobotMap.REAR_LEFT_PIVOT, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_P, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_I, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_D, - SwerveDriveConfig.DRIVE_MODULE_P, - SwerveDriveConfig.DRIVE_MODULE_I, - SwerveDriveConfig.DRIVE_MODULE_D, - SwerveDriveConfig.DRIVE_MODULE_KS, - SwerveDriveConfig.DRIVE_MODULE_KV, - SwerveDriveConfig.DRIVE_MODULE_KA, - RobotMap.REAR_LEFT_PIVOT_ENCODER, + private final SwerveModule kRearLeft = createModule( + "RearLeft", RobotMap.REAR_LEFT_DRIVE, RobotMap.REAR_LEFT_PIVOT, RobotMap.REAR_LEFT_PIVOT_ENCODER, SwerveDriveConfig.kRearLeftPivotOffsetDegrees); - private final SwerveModule kRearRight = new SwerveModule( - "RearRight", - RobotMap.REAR_RIGHT_DRIVE, - RobotMap.REAR_RIGHT_PIVOT, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_P, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_I, - SwerveDriveConfig.DRIVE_MODULE_PIVOT_D, - SwerveDriveConfig.DRIVE_MODULE_P, - SwerveDriveConfig.DRIVE_MODULE_I, - SwerveDriveConfig.DRIVE_MODULE_D, - SwerveDriveConfig.DRIVE_MODULE_KS, - SwerveDriveConfig.DRIVE_MODULE_KV, - SwerveDriveConfig.DRIVE_MODULE_KA, - RobotMap.REAR_RIGHT_PIVOT_ENCODER, + private final SwerveModule kRearRight = createModule( + "RearRight", RobotMap.REAR_RIGHT_DRIVE, RobotMap.REAR_RIGHT_PIVOT, RobotMap.REAR_RIGHT_PIVOT_ENCODER, SwerveDriveConfig.kRearRightPivotOffsetDegrees); - /** - * https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html - */ private final SwerveDriveKinematics kSwerveKinematics = new SwerveDriveKinematics( - SwerveDriveConfig.kFrontLeftModuleOffset, SwerveDriveConfig.kFrontRightModuleOffset, - SwerveDriveConfig.kRearLeftModuleOffset, SwerveDriveConfig.kRearRightModuleOffset); + SwerveDriveConfig.kFrontLeftModuleOffset, + SwerveDriveConfig.kFrontRightModuleOffset, + SwerveDriveConfig.kRearLeftModuleOffset, + SwerveDriveConfig.kRearRightModuleOffset); - /** - * Standard deviations of the pose estimate (x position in meters, y position in - * meters, and heading in radians). - *

- * Increase these numbers to trust the state estimate less. - */ + /** Default process noise for pose estimation (x, y, theta). Increase to trust odometry less. */ public static final Matrix kDefaultStateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); - /** - * Standard deviations of the vision pose measurement (x position in meters, y - * position in meters, and heading in radians). - *

- * Increase these numbers to trust the vision pose measurement less. + * Default vision measurement noise (x, y, theta). Theta uses {@link Double#MAX_VALUE} so vision + * does not correct heading when using wheel + gyro odometry. */ public static final Matrix kDefaultVisionMeasurementStdDevs = VecBuilder.fill(0.7, 0.7, Double.MAX_VALUE); - /** - * https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html - */ - private final SwerveDrivePoseEstimator kPoseEstimator = new SwerveDrivePoseEstimator( - kSwerveKinematics, - getAngleForOdometry(), - getSwerveModulePositions(), - new Pose2d(), - kDefaultStateStdDevs, - kDefaultVisionMeasurementStdDevs); - - private final PIDController xPid = new PIDController(SwerveDriveConfig.DRIVE_X_P, SwerveDriveConfig.DRIVE_X_I, - SwerveDriveConfig.DRIVE_X_D); - private final PIDController yPid = new PIDController(SwerveDriveConfig.DRIVE_Y_P, SwerveDriveConfig.DRIVE_Y_I, - SwerveDriveConfig.DRIVE_Y_D); - private final PIDController thetaPid = new PIDController(SwerveDriveConfig.DRIVE_ANGLE_P, - SwerveDriveConfig.DRIVE_ANGLE_I, SwerveDriveConfig.DRIVE_ANGLE_D); - - // for april tag alignment - private double errorX, errorY; - private Optional targetOpt = Optional.empty(); + private final SwerveDrivePoseEstimator kPoseEstimator; + + private final Supplier headingSupplier; + + private final PIDController thetaPid = new PIDController( + SwerveDriveConfig.DRIVE_ANGLE_P, + SwerveDriveConfig.DRIVE_ANGLE_I, + SwerveDriveConfig.DRIVE_ANGLE_D); /** - * Constructs the Swerve Drive. + * Constructs modules, pose estimator, optional PathPlanner AutoBuilder, and initial field pose. + * + * @param headingSupplier robot heading for wheel odometry fusion (e.g. NavX from {@link frc.robot.RobotContainer}) + * @param configurePathPlanner when false, skips {@link AutoBuilder} setup (use in unit tests to avoid global side effects) */ - public SwerveDrive() { - // meters - xPid.setTolerance(0.1); - yPid.setTolerance(0.1); - - // degrees + public SwerveDrive(Supplier headingSupplier, boolean configurePathPlanner) { + this.headingSupplier = headingSupplier; thetaPid.enableContinuousInput(-180.0, 180.0); thetaPid.setTolerance(3); - configurePathPlannerAutoBuilder(); + kPoseEstimator = new SwerveDrivePoseEstimator( + kSwerveKinematics, + getAngleForOdometry(), + getSwerveModulePositions(), + new Pose2d(), + kDefaultStateStdDevs, + kDefaultVisionMeasurementStdDevs); - resetPose(new Pose2d(8.5, 4.0, Rotation2d.fromDegrees(0))); + if (configurePathPlanner) { + configurePathPlannerAutoBuilder(); + } + resetPose(FieldConstants.kDefaultOdometryPose); SmartDashboard.putData(this); } - /** {@inheritDoc} */ + /** @see #SwerveDrive(Supplier, boolean) with PathPlanner enabled */ + public SwerveDrive(Supplier headingSupplier) { + this(headingSupplier, true); + } + + private static SwerveModule createModule( + String name, int driveMotorId, int pivotMotorId, int pivotEncoderId, double pivotOffsetDegrees) { + return new SwerveModule( + name, + driveMotorId, + pivotMotorId, + SwerveDriveConfig.DRIVE_MODULE_PIVOT_P, + SwerveDriveConfig.DRIVE_MODULE_PIVOT_I, + SwerveDriveConfig.DRIVE_MODULE_PIVOT_D, + SwerveDriveConfig.DRIVE_MODULE_P, + SwerveDriveConfig.DRIVE_MODULE_I, + SwerveDriveConfig.DRIVE_MODULE_D, + SwerveDriveConfig.DRIVE_MODULE_KS, + SwerveDriveConfig.DRIVE_MODULE_KV, + SwerveDriveConfig.DRIVE_MODULE_KA, + pivotEncoderId, + pivotOffsetDegrees); + } + @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); builder.setSmartDashboardType(getName()); builder.setSafeState(this::stop); builder.setActuator(true); - builder.addDoubleProperty("Pose-X", () -> getPose().getX(), (xPoseMeters) -> { - var currentPose = getPose(); - var updatedPose = new Pose2d(xPoseMeters, currentPose.getY(), currentPose.getRotation()); - resetPose(updatedPose); + builder.addDoubleProperty("Pose-X", () -> getPose().getX(), xPoseMeters -> { + Pose2d currentPose = getPose(); + resetPose(new Pose2d(xPoseMeters, currentPose.getY(), currentPose.getRotation())); }); - builder.addDoubleProperty("Pose-Y", () -> getPose().getY(), (yPoseMeters) -> { - var currentPose = getPose(); - var updatedPose = new Pose2d(currentPose.getX(), yPoseMeters, currentPose.getRotation()); - resetPose(updatedPose); + builder.addDoubleProperty("Pose-Y", () -> getPose().getY(), yPoseMeters -> { + Pose2d currentPose = getPose(); + resetPose(new Pose2d(currentPose.getX(), yPoseMeters, currentPose.getRotation())); }); - builder.addDoubleProperty("Pose-Theta", () -> getPose().getRotation().getDegrees(), (thetaPoseDegrees) -> { - var currentPose = getPose(); - var updatedPose = new Pose2d(currentPose.getX(), currentPose.getY(), Rotation2d.fromDegrees(thetaPoseDegrees)); - resetPose(updatedPose); + builder.addDoubleProperty("Pose-Theta", () -> getPose().getRotation().getDegrees(), thetaPoseDegrees -> { + Pose2d currentPose = getPose(); + resetPose(new Pose2d(currentPose.getX(), currentPose.getY(), Rotation2d.fromDegrees(thetaPoseDegrees))); }); builder.addDoubleProperty("MaxTranslationalSpeed", () -> maxTranslationalSpeedMetersPerSecond, - (maxSpeedMetersPerSecond) -> maxTranslationalSpeedMetersPerSecond = Math.abs(maxSpeedMetersPerSecond)); + maxSpeedMetersPerSecond -> maxTranslationalSpeedMetersPerSecond = Math.abs(maxSpeedMetersPerSecond)); builder.addDoubleProperty("MaxAngularSpeed", () -> Math.toDegrees(maxAngularSpeedRadiansPerSecond), - (maxSpeedDegreesPerSecond) -> maxAngularSpeedRadiansPerSecond = Math.toRadians(maxSpeedDegreesPerSecond)); + maxSpeedDegreesPerSecond -> maxAngularSpeedRadiansPerSecond = Math.toRadians(maxSpeedDegreesPerSecond)); } - /** {@inheritDoc} */ + /** Updates {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator} from module encoders and gyro. */ @Override public void periodic() { - updateOdometryWithModuleStates(); + kPoseEstimator.updateWithTime(Timer.getFPGATimestamp(), getAngleForOdometry(), getSwerveModulePositions()); } - /** - * https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html#updating-the-robot-pose - */ - private void updateOdometryWithModuleStates() { - kPoseEstimator.updateWithTime(Timer.getFPGATimestamp(), getAngleForOdometry(), getSwerveModulePositions()); + /** Steps REV {@link com.revrobotics.sim.SparkMaxSim} on each module when running desktop simulation. */ + @Override + public void simulationPeriodic() { + kFrontLeft.simulationPeriodic(); + kFrontRight.simulationPeriodic(); + kRearLeft.simulationPeriodic(); + kRearRight.simulationPeriodic(); } /** - * Adjusts the robot's current position on the field using the given - * measurements from vision. - * - * @param pose the estimated pose of the robot from the vision - * @param timestamp the timestamp from when the robot's position was captured - * @param stdDevs the standard deviations to use for the vision adjustments + * Fuses a vision pose into odometry (e.g. Limelight/Photon callbacks in {@link frc.robot.RobotContainer}). + * + * @param pose field pose from vision + * @param timestamp measurement time (seconds) + * @param stdDevs per-axis standard deviations; use {@link #kDefaultVisionMeasurementStdDevs} for defaults */ public void addVisionMeasurementForOdometry(Pose2d pose, double timestamp, Matrix stdDevs) { kPoseEstimator.addVisionMeasurement(pose, timestamp, stdDevs); } - /** https://pathplanner.dev/pplib-getting-started.html#holonomic-swerve */ private void configurePathPlannerAutoBuilder() { - // Load the RobotConfig from the GUI settings. - RobotConfig config; try { - config = RobotConfig.fromGUISettings(); - - // Configure AutoBuilder last + RobotConfig config = RobotConfig.fromGUISettings(); AutoBuilder.configure( - this::getPose, // Robot pose supplier - this::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) - this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> setModuleStates(speeds), // Method that will drive the robot given ROBOT - // RELATIVE - // ChassisSpeeds. Also optionally outputs individual module - // feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for - // holonomic drive trains - new PIDConstants(SwerveDriveConfig.DRIVE_PATHING_TRANSLATION_P, + this::getPose, + this::resetPose, + this::getChassisSpeeds, + (speeds, feedforwards) -> setModuleStates(speeds), + new PPHolonomicDriveController( + new PIDConstants( + SwerveDriveConfig.DRIVE_PATHING_TRANSLATION_P, SwerveDriveConfig.DRIVE_PATHING_TRANSLATION_I, - SwerveDriveConfig.DRIVE_PATHING_TRANSLATION_D), // Translation PID constants - new PIDConstants(SwerveDriveConfig.DRIVE_PATHING_ROTATION_P, + SwerveDriveConfig.DRIVE_PATHING_TRANSLATION_D), + new PIDConstants( + SwerveDriveConfig.DRIVE_PATHING_ROTATION_P, SwerveDriveConfig.DRIVE_PATHING_ROTATION_I, - SwerveDriveConfig.DRIVE_PATHING_ROTATION_D) // Rotation PID constants - ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red - // alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); + SwerveDriveConfig.DRIVE_PATHING_ROTATION_D)), + config, + () -> DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red, + this); } catch (Exception ex) { ex.printStackTrace(); DriverStation.reportError("Failed to configure PathPlanner AutoBuilder: " + ex.getMessage(), false); } } - /** - * Gets the robot's current position on the field. - * - * @return the robot's position on the field from the odometry - */ + /** @return fused field pose (meters, radians) from odometry and vision */ public Pose2d getPose() { return kPoseEstimator.getEstimatedPosition(); } /** - * Resets the robot's pose for odometry. - * - *

- * - * https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html#resetting-the-robot-pose - * - * @param pose the robot's new pose + * Resets the estimated field pose while keeping gyro and module positions. + * + * @param pose new field pose */ public void resetPose(Pose2d pose) { kPoseEstimator.resetPosition(getAngleForOdometry(), getSwerveModulePositions(), pose); } - /** - * Resets the robot heading to be 0 when blue alliance and 180 when red - * alliance. - * - * @return the command to reset the robot heading - */ - public Command resetHeading(DoubleSupplier x, DoubleSupplier y) { - var cmd = runOnce(() -> { - // always reset facing RED alliance - var awayAngle = 0; - resetPose(new Pose2d(x.getAsDouble(), y.getAsDouble(), Rotation2d.fromDegrees(awayAngle))); - }); - return cmd.withName("ResetRobotHeading"); - } - - public Command setCenterOfRotation(Translation2d rotationPoint) { - Runnable setNewRotation = () -> { - centerOfRotation = rotationPoint; - }; - Runnable setCenterRotation = () -> { - centerOfRotation = Translation2d.kZero; - }; - var cmd = Commands.startEnd(setNewRotation, setCenterRotation); - return cmd.withName("SwerveSetCenterRotation"); - } - - /** - * Gets the robot's current speed. - * - * @return the robot's current speed - */ + /** @return current chassis speeds from measured module states (robot-relative) */ public ChassisSpeeds getChassisSpeeds() { return kSwerveKinematics.toChassisSpeeds(getSwerveModuleStates()); } - /** - * Sets the module states based on the desired robot speed. - * - * @param chassisSpeeds the desired robot speed - */ - private void setModuleStates(final ChassisSpeeds chassisSpeeds) { - var desiredStates = getOptimizedModuleStatesFromChassisSpeeds(chassisSpeeds); - kFrontLeft.setDesiredState(desiredStates[0], true); - kFrontRight.setDesiredState(desiredStates[1], true); - kRearLeft.setDesiredState(desiredStates[2], true); - kRearRight.setDesiredState(desiredStates[3], true); + private void setModuleStates(ChassisSpeeds chassisSpeeds) { + SwerveModuleState[] desiredStates = getOptimizedModuleStatesFromChassisSpeeds(chassisSpeeds); + kFrontLeft.setDesiredState(desiredStates[0]); + kFrontRight.setDesiredState(desiredStates[1]); + kRearLeft.setDesiredState(desiredStates[2]); + kRearRight.setDesiredState(desiredStates[3]); } - /** - * Optimizes the swerve module states given the desired robot speeds. - * - * @param originalSpeeds the speeds to optimze - * @return the optimized swerve module states - */ - private SwerveModuleState[] getOptimizedModuleStatesFromChassisSpeeds(final ChassisSpeeds originalSpeeds) { - // optimize chassis speeds - var optimizedChassisSpeeds = correctForDynamics(originalSpeeds); - - // https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html#using-custom-centers-of-rotation - var optimizedModuleStates = kSwerveKinematics.toSwerveModuleStates(optimizedChassisSpeeds, centerOfRotation); - - // normalize wheel speeds + private SwerveModuleState[] getOptimizedModuleStatesFromChassisSpeeds(ChassisSpeeds originalSpeeds) { + ChassisSpeeds optimizedChassisSpeeds = SwerveDynamics.correctForDynamics(originalSpeeds); + SwerveModuleState[] optimizedModuleStates = kSwerveKinematics.toSwerveModuleStates(optimizedChassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(optimizedModuleStates, maxTranslationalSpeedMetersPerSecond); - return optimizedModuleStates; } - /** - * Correction for swerve second order dynamics issue. Borrowed from 254: - * https://github.com/Team254/FRC-2022-Public/blob/main/src/main/java/com/team254/frc2022/subsystems/Drive.java#L325 - * Discussion: - * https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964 - */ - private static ChassisSpeeds correctForDynamics(final ChassisSpeeds originalSpeeds) { - final double LOOP_TIME_S = 0.02; - Pose2d futureRobotPose = new Pose2d( - originalSpeeds.vxMetersPerSecond * LOOP_TIME_S, - originalSpeeds.vyMetersPerSecond * LOOP_TIME_S, - Rotation2d.fromRadians(originalSpeeds.omegaRadiansPerSecond * LOOP_TIME_S)); - Twist2d twistForPose = log(futureRobotPose); - ChassisSpeeds updatedSpeeds = new ChassisSpeeds( - twistForPose.dx / LOOP_TIME_S, - twistForPose.dy / LOOP_TIME_S, - twistForPose.dtheta / LOOP_TIME_S); - return updatedSpeeds; + private Rotation2d getAngleForOdometry() { + return headingSupplier.get(); } - /** - * Logical inverse of the above. Borrowed from 254: - * https://github.com/Team254/FRC-2022-Public/blob/b5da3c760b78d598b492e1cc51d8331c2ad50f6a/src/main/java/com/team254/lib/geometry/Pose2d.java - */ - private static Twist2d log(final Pose2d transform) { - final double kEps = 1E-9; - final double dtheta = transform.getRotation().getRadians(); - final double half_dtheta = 0.5 * dtheta; - final double cos_minus_one = Math.cos(transform.getRotation().getRadians()) - 1.0; - double halftheta_by_tan_of_halfdtheta; - if (Math.abs(cos_minus_one) < kEps) { - halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; - } else { - halftheta_by_tan_of_halfdtheta = -(half_dtheta * Math.sin(transform.getRotation().getRadians())) / cos_minus_one; - } - final Translation2d translation_part = transform - .getTranslation() - .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta)); - return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta); + /** Releases module CAN devices; for simulation unit tests only. */ + void closeModulesForTest() { + kFrontLeft.close(); + kFrontRight.close(); + kRearLeft.close(); + kRearRight.close(); } - /** - * Returns the angle of the robot as a Rotation2d as read by the navx. - *

- * Note that {@link #getPose()} should be used when trying to access the robot's - * actual heading (rotation) - * relative to the field. This method is only used internally to update the - * odometry. - * - * @return The angle of the robot. - * @see #getPose() - */ - private Rotation2d getAngleForOdometry() { - return RobotContainer.kNavx.getRotation2d(); + /** Seeds pivot CANcoder sim at 0° on all modules (sim unit tests). */ + void seedSimulationPivotEncodersZero() { + seedSimulationPivotEncoder(kFrontLeft); + seedSimulationPivotEncoder(kFrontRight); + seedSimulationPivotEncoder(kRearLeft); + seedSimulationPivotEncoder(kRearRight); } - /** - * Command to set the swerve drive to go slow until interrupted. - * - * @return the go slow comamnd - */ - public Command goSlow() { - Runnable setSlowSpeed = () -> { - maxTranslationalSpeedMetersPerSecond = 0.5; - maxAngularSpeedRadiansPerSecond = Math.toRadians(90); - }; - Runnable setNormalSpeed = () -> { - maxTranslationalSpeedMetersPerSecond = 4.6; - maxAngularSpeedRadiansPerSecond = Math.toRadians(540); - }; - var cmd = Commands.startEnd(setSlowSpeed, setNormalSpeed); - return cmd.withName("SwerveGoSlow"); + private static void seedSimulationPivotEncoder(SwerveModule module) { + module.pivotEncoderSimState().setSupplyVoltage(12.0); + module.pivotEncoderSimState().setRawPosition(Units.Rotations.of(0.0)); + module.refreshPivotAbsolutePosition(); + SimHooks.stepTiming(0.02); } - /** - * Command to hold the current heading of the robot while driving. - * - * @return command to hold the current heading of the robot while driving - */ - public Command holdCurrentHeadingWhileDriving(CommandXboxController hid, BooleanSupplier fieldRelative, double joystickDeadband, double joystickSensitivity) { - var setHeading = runOnce(() -> headingToHold = getPose().getRotation()); - var holdHeading = runEnd(() -> { - // Get the x speed. We are inverting this because Xbox controllers return - // negative values when we push forward. - var xSpeed = 0.0; - var leftYInput = -hid.getLeftY(); - if (Math.abs(leftYInput) > joystickDeadband) { - xSpeed = HIDUtils.smoothInputWithCubic(leftYInput, joystickSensitivity) * maxTranslationalSpeedMetersPerSecond; - } - - // Get the y speed or sideways/strafe speed. We are inverting this because - // we want a positive value when we pull to the left. Xbox controllers - // return positive values when you pull to the right by default. - var ySpeed = 0.0; - var leftXInput = -hid.getLeftX(); - if (Math.abs(leftXInput) > joystickDeadband) { - ySpeed = HIDUtils.smoothInputWithCubic(leftXInput, joystickSensitivity) * maxTranslationalSpeedMetersPerSecond; - } - - // flip drive input based on alliance since robot's movement is always - // relative to the blue alliance (AKA facing towards red alliance) - var alliance = DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() : Alliance.Blue; - var sign = fieldRelative.getAsBoolean() && alliance == Alliance.Red ? -1 : 1; - holdAngleWhileDriving(sign * xSpeed, sign * ySpeed, headingToHold, 1, fieldRelative.getAsBoolean()); - }, this::stop); - var cmd = Commands.sequence(setHeading, holdHeading); - return cmd.withName("SwerveHoldCurrentHeadingWhileDriving"); + /** Largest absolute pivot motor output across modules (sim unit tests). */ + double maxAbsPivotMotorOutput() { + return Math.max( + Math.max(absAppliedOutput(kFrontLeft.pivotSparkSim()), absAppliedOutput(kFrontRight.pivotSparkSim())), + Math.max(absAppliedOutput(kRearLeft.pivotSparkSim()), absAppliedOutput(kRearRight.pivotSparkSim()))); } - /** - * Command to drive the robot with an Xbox Controller. - * - * @param hid the xbox controller - * @param fieldRelative whether or not to use field relative drive - * @param joystickDeadband the deadband for the joysticks to consider as - * non-zero - * @param joystickSensitivity ranges from [0, 1] where 0 is full linear and 1 is - * full cubic for smoother inputs - * @return the command to drive the robot with an Xbox Controller - */ - public Command driveWithXboxController(CommandXboxController hid, BooleanSupplier fieldRelative, - double joystickDeadband, double joystickSensitivity) { - var cmd = runEnd(() -> { - // Get the x speed. We are inverting this because Xbox controllers return - // negative values when we push forward. - var xSpeed = 0.0; - var leftYInput = -hid.getLeftY(); - if (Math.abs(leftYInput) > joystickDeadband) { - xSpeed = HIDUtils.smoothInputWithCubic(leftYInput, joystickSensitivity) * maxTranslationalSpeedMetersPerSecond; - } - - // Get the y speed or sideways/strafe speed. We are inverting this because - // we want a positive value when we pull to the left. Xbox controllers - // return positive values when you pull to the right by default. - var ySpeed = 0.0; - var leftXInput = -hid.getLeftX(); - if (Math.abs(leftXInput) > joystickDeadband) { - ySpeed = HIDUtils.smoothInputWithCubic(leftXInput, joystickSensitivity) * maxTranslationalSpeedMetersPerSecond; - } - - // Get the rate of angular rotation. We are inverting this because we want a - // positive value when we pull to the left (remember, CCW is positive in - // mathematics). Xbox controllers return positive values when you pull to - // the right by default. - var rotationSpeed = 0.0; - var rightXInput = -hid.getRightX(); - if (Math.abs(rightXInput) > joystickDeadband) { - rotationSpeed = HIDUtils.smoothInputWithCubic(rightXInput, joystickSensitivity) - * maxAngularSpeedRadiansPerSecond; - } - // flip drive input based on alliance since robot's movement is always - // relative to the blue alliance (AKA facing towards red alliance) - var alliance = DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() : Alliance.Blue; - var sign = fieldRelative.getAsBoolean() && alliance == Alliance.Red ? -1 : 1; - drive(sign * xSpeed, sign * ySpeed, rotationSpeed, fieldRelative.getAsBoolean()); - }, this::stop); - return cmd.withName("DriveWithXboxController"); - } - - private Command alignToTarget(Supplier> targetSupp, double XDist, double YDist, boolean wantsFinalAdjustment) { - var alignCmd = runEnd(() -> { - targetOpt = targetSupp.get(); - if (targetOpt.isEmpty()) { - this.stop(); - return; - } - - var target = targetOpt.get(); - - errorX = XDist - target.getOffetX(); - var outputX = -(SwerveDriveConfig.VISION_X_P * errorX); - - errorY = YDist - target.getOffsetY(); - var outputY = -(SwerveDriveConfig.VISION_Y_P * errorY); - - var targetAngle = target.getTargetAngle(); - - this.holdAngleWhileDriving(outputX, outputY, Rotation2d.fromDegrees(targetAngle), false); - }, this::stop) - .until(() -> targetOpt.isEmpty() || (Math.abs(errorX) <= 0.01 && Math.abs(errorY) <= 0.01 && atAngleSetpoint())); - var finalAdjustment = Commands.none(); - if (wantsFinalAdjustment){ - finalAdjustment = runEnd(() -> this.drive(0.2, 0, 0, false), this::stop) - .withTimeout(0.5); + /** Largest absolute drive motor output across modules (sim unit tests). */ + double maxAbsDriveMotorOutput() { + return Math.max( + Math.max(absAppliedOutput(kFrontLeft.driveSparkSim()), absAppliedOutput(kFrontRight.driveSparkSim())), + Math.max(absAppliedOutput(kRearLeft.driveSparkSim()), absAppliedOutput(kRearRight.driveSparkSim()))); + } + + private static double absAppliedOutput(com.revrobotics.sim.SparkMaxSim sim) { + return sim == null ? 0.0 : Math.abs(sim.getAppliedOutput()); + } + + /** Steps module physics sim {@code iterations} times (sim unit tests). */ + void runSimulationSteps(int iterations) { + for (int i = 0; i < iterations; i++) { + simulationPeriodic(); + SimHooks.stepTiming(0.02); } + } - var cmd = Commands.sequence(alignCmd, finalAdjustment); + /** Sum of absolute drive motor outputs across modules (sim unit tests). */ + double sumAbsDriveMotorOutputs() { + return absAppliedOutput(kFrontLeft.driveSparkSim()) + + absAppliedOutput(kFrontRight.driveSparkSim()) + + absAppliedOutput(kRearLeft.driveSparkSim()) + + absAppliedOutput(kRearRight.driveSparkSim()); + } - return cmd.withName("SwerveDriveAlignToTarget"); + /** Whether any module is on the inverted-drive optimization path after the last setpoint. */ + boolean isAnyModuleOptimizingAngle() { + return kFrontLeft.isOptimizingAngle() + || kFrontRight.isOptimizingAngle() + || kRearLeft.isOptimizingAngle() + || kRearRight.isOptimizingAngle(); } /** - * Method to drive the robot using joystick info. (used for teleop) + * Sets chassis motion from teleop, heading hold, or internal callers. * - * @param vxMetersPerSecond Speed of the robot in the x direction (forward) - * in m/s. - * @param vyMetersPerSecond Speed of the robot in the y direction (sideways) - * in m/s. - * @param omegaRadiansPerSecond Angular rate of the robot in rad/sec. - * @param fieldRelative Whether the provided x and y speeds are relative - * to the - * field. - */ - public void drive(double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond, + * @param vxMetersPerSecond forward speed (m/s); positive is robot forward + * @param vyMetersPerSecond strafe speed (m/s); positive is robot left + * @param omegaRadiansPerSecond counter-clockwise rotation rate (rad/s) + * @param fieldRelative if true, vx/vy are relative to the field; if false, robot-relative + */ + public void drive( + double vxMetersPerSecond, + double vyMetersPerSecond, + double omegaRadiansPerSecond, boolean fieldRelative) { - if (Math.abs(omegaRadiansPerSecond) < kDriveOmegaDeadband && Math.abs(vxMetersPerSecond) < kDriveVelocityDeadband - && Math.abs(vyMetersPerSecond) < kDriveVelocityDeadband) { - kFrontLeft.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(kFrontLeft.getAngle()))); - kFrontRight.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(kFrontRight.getAngle()))); - kRearLeft.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(kRearLeft.getAngle()))); - kRearRight.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(kRearRight.getAngle()))); - } else { - // get desired chassis speeds - ChassisSpeeds desiredChassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond, - getPose().getRotation()) - : new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); - - // set desired chassis speeds - setModuleStates(desiredChassisSpeeds); + if (shouldCoastSwerveModules(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond)) { + stopModulesButHoldPivotAngle(); + return; } + + ChassisSpeeds desiredChassisSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds( + vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond, getPose().getRotation()) + : new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); + setModuleStates(desiredChassisSpeeds); } - /** - * Gets the current drive speeds and pivot angles of all swerve modules in the - * following order: FL, FR, RL, RR. - * - * @return the states of the module (speeds and angles) in the following order: - * FL, FR, RL, RR. - */ + private void stopModulesButHoldPivotAngle() { + kFrontLeft.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(kFrontLeft.getAngle()))); + kFrontRight.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(kFrontRight.getAngle()))); + kRearLeft.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(kRearLeft.getAngle()))); + kRearRight.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromDegrees(kRearRight.getAngle()))); + } + + /** @return measured module states in order FL, FR, RL, RR */ public SwerveModuleState[] getSwerveModuleStates() { return new SwerveModuleState[] { kFrontLeft.getState(), @@ -621,13 +369,7 @@ public SwerveModuleState[] getSwerveModuleStates() { }; } - /** - * Gets the current drive distance traveled and pivot angles of all swerve - * modules in the following order: FL, FR, RL, RR. - * - * @return the states of the module (speeds and angles) in the following order: - * FL, FR, RL, RR. - */ + /** @return integrated module positions in order FL, FR, RL, RR (for odometry) */ public SwerveModulePosition[] getSwerveModulePositions() { return new SwerveModulePosition[] { kFrontLeft.getPosition(), @@ -637,147 +379,66 @@ public SwerveModulePosition[] getSwerveModulePositions() { }; } - /** - * Command to stop all modules. - * - * @return a command to stop all modules - */ - public Command stopCommand() { - var cmd = runOnce(this::stop); - return cmd.withName("StopSwerve"); - } - - /** - * Stops all modules and resets the internal PID controllers. - */ + /** Stops all modules and resets the heading-hold PID controller. */ public void stop() { kFrontRight.stop(); kFrontLeft.stop(); kRearRight.stop(); kRearLeft.stop(); - resetPID(); + thetaPid.reset(); } - /** - * Sets swerve modules to be all angled in an "X" pattern - useful when trying to stay still on an - * incline or to help prevent getting pushed by defense. - */ + /** Angles modules in an X pattern to resist being pushed on defense or on a slope. */ public void lock() { kFrontRight.setDesiredState(0, -45); kFrontLeft.setDesiredState(0, 45); kRearRight.setDesiredState(0, 45); kRearLeft.setDesiredState(0, -45); } - - /** - * Command for lock. - * @see #lock() - * @return command to lock the drive train for defense - */ - public Command lockCmd() { - var cmd = runEnd(this::lock, this::stop); - return cmd.withName("SwerveDriveLock"); - } - - /** - * Holds the desired angle while translating. - * - * @param xSpeed x speed in m/s - * @param ySpeed y speed in m/s - * @param setpointAngle the desired robot angle to hold about the robot's center - * @param maxSpeed desired percent of max speed (0.0 to 1.0) - * @param fieldOriented field relative speeds (true) or robot relative speeds - * (false) - */ - public void holdAngleWhileDriving(double xSpeed, double ySpeed, Rotation2d setpointAngle, double maxSpeed, boolean fieldOriented) { - var rotateOutput = MathUtil - .clamp(thetaPid.calculate(getPose().getRotation().getDegrees(), normalizeAngle(setpointAngle.getDegrees())), -maxSpeed, - maxSpeed) - * maxAngularSpeedRadiansPerSecond; - drive(xSpeed, ySpeed, rotateOutput, fieldOriented); - } - - public void holdAngleWhileDriving(double xSpeed, double ySpeed, Rotation2d setpointAngle, boolean fieldOriented) { - holdAngleWhileDriving(xSpeed, ySpeed, setpointAngle, 0.5, fieldOriented); - } /** - * Rotate to a desired angle about the robot's center. - */ - public void rotateToAngleInPlace(Rotation2d setpointAngle, double maxSpeed) { - holdAngleWhileDriving(0, 0, setpointAngle, maxSpeed, false); - } - - public void rotateToAngleInPlace(Rotation2d setpointAngle) { - holdAngleWhileDriving(0, 0, setpointAngle, false); - } - - /** - * Rotate to a desired angle about the robot's center. - * - * @param setpointAngle - */ - public Command rotateToAngleInPlaceCmd(Rotation2d setpointAngle) { - var cmd = runEnd(() -> rotateToAngleInPlace(setpointAngle), this::stop) - .until(() -> this.atAngleSetpoint()); - return cmd.withName("SwerveRotateToAngleInPlace"); - } - - /** - * Sets the drive to go to the desired pose given the robot's current pose vs - * target - * - * @param target the desired pose the robot should go to - * @param fieldOriented field relative speeds (true) or robot relative speeds - * (false) + * Translates while a PID loop drives robot heading to {@code setpointAngle}. + * + * @param xSpeed forward component (m/s) + * @param ySpeed strafe component (m/s) + * @param setpointAngle desired field- or robot-relative heading per {@code fieldOriented} + * @param maxSpeed fraction of {@link #maxAngularSpeedRadiansPerSecond} applied to rotation PID output + * @param fieldOriented passed through to {@link #drive(double, double, double, boolean)} */ - public void goToPose(Pose2d target, boolean fieldOriented) { - Pose2d pose = getPose(); - double xSpeed = MathUtil.clamp(xPid.calculate(pose.getX(), target.getX()), -1, 1) - * maxTranslationalSpeedMetersPerSecond; - double ySpeed = MathUtil.clamp(yPid.calculate(pose.getY(), target.getY()), -1, 1) - * maxTranslationalSpeedMetersPerSecond; - double vTheta = MathUtil.clamp(thetaPid.calculate(normalizeAngle(pose.getRotation().getDegrees()), - normalizeAngle(target.getRotation().getDegrees())), -1, 1) * maxAngularSpeedRadiansPerSecond; - drive(xSpeed, ySpeed, vTheta, fieldOriented); + public void holdAngleWhileDriving( + double xSpeed, + double ySpeed, + Rotation2d setpointAngle, + double maxSpeed, + boolean fieldOriented) { + drive(xSpeed, ySpeed, computeHeadingHoldOmegaRadians(thetaPid, getPose().getRotation(), setpointAngle, maxSpeed, + maxAngularSpeedRadiansPerSecond), fieldOriented); } /** - * Gets if the robot's heading is at the desired angle. - * - * @return true if the robot's heading is at the desired angle, false otherwise + * True when teleop inputs are below {@link #kDriveVelocityDeadband} / {@link #kDriveOmegaDeadband} so modules + * coast with pivots held (package-visible for unit tests). */ - public boolean atAngleSetpoint() { - return thetaPid.atSetpoint(); + static boolean shouldCoastSwerveModules(double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) { + return Math.abs(omegaRadiansPerSecond) < kDriveOmegaDeadband + && Math.abs(vxMetersPerSecond) < kDriveVelocityDeadband + && Math.abs(vyMetersPerSecond) < kDriveVelocityDeadband; } /** - * Resets the internal X, Y and Theta PID controllers. + * Rotation rate from heading-hold PID (package-visible for unit tests). */ - public void resetPID() { - xPid.reset(); - yPid.reset(); - thetaPid.reset(); + static double computeHeadingHoldOmegaRadians( + PIDController thetaPid, + Rotation2d currentHeading, + Rotation2d setpointAngle, + double maxSpeedFraction, + double maxAngularSpeedRadiansPerSecond) { + return MathUtil.clamp( + thetaPid.calculate( + currentHeading.getDegrees(), AngleUtils.normalizeDegrees(setpointAngle.getDegrees())), + -maxSpeedFraction, + maxSpeedFraction) * maxAngularSpeedRadiansPerSecond; } - /** - * Clamps an angle to be from [-180, 180] - * - * @param angle the angle to clamp - * @return the newly clamped angle from [-180, 180] - */ - private static double normalizeAngle(double angle) { - if (angle > 0) { - angle %= 360; - if (angle > 180) { - angle -= 360; - } - } else if (angle < 0) { - angle %= -360; - if (angle < -180) { - angle += 360; - } - } - return angle; - } } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDriveConfig.java b/src/main/java/frc/robot/subsystems/drive/SwerveDriveConfig.java index 0e1a43d..2c6c9b7 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDriveConfig.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDriveConfig.java @@ -7,7 +7,13 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +/** + * Mechanical and control constants for the swerve drivetrain (gains, module positions, encoder scaling). + * + *

Tune PID and feedforward here; CAN IDs live in {@link frc.robot.RobotMap}. + */ public final class SwerveDriveConfig { + private SwerveDriveConfig() {} public static final double kDriveWheelRadiusMeters = Units.inchesToMeters(2); public static final double SDS_L2_DRIVE_GEARING = 6.75; public static final double kDriveEncoderPositionConversionFactor = (2 * Math.PI * kDriveWheelRadiusMeters) @@ -23,15 +29,7 @@ public final class SwerveDriveConfig { public static final Translation2d kRearRightModuleOffset = new Translation2d(Units.inchesToMeters(-11.75), Units.inchesToMeters(-9.25)); - // April Tags - public static final double VISION_X_P = 1.25; - public static final double VISION_Y_P = 2; - public static final double VISION_REEF_TARGET_X_DISTANCE_METERS = 0.15; - public static final double VISION_REEF_TARGET_Y_DISTANCE_METERS = 0.02; - public static final double VISION_CORAL_STATION_TARGET_X_DISTANCE_METERS = 0.7; - public static final double VISION_CORAL_STATION_TARGET_Y_DISTANCE_METERS = -0.18; - - // individual offsets after calibrating each module + // Individual pivot encoder offsets after calibrating each module public static final double kFrontLeftPivotOffsetDegrees = 225; public static final double kFrontRightPivotOffsetDegrees = 135; public static final double kRearLeftPivotOffsetDegrees = 315; @@ -52,14 +50,6 @@ public final class SwerveDriveConfig { public static final double DRIVE_ANGLE_I = 0; public static final double DRIVE_ANGLE_D = 0; - public static final double DRIVE_X_P = 0.3; - public static final double DRIVE_X_I = 0; - public static final double DRIVE_X_D = 0; - - public static final double DRIVE_Y_P = 0.3; - public static final double DRIVE_Y_I = 0; - public static final double DRIVE_Y_D = 0; - public static final double DRIVE_PATHING_TRANSLATION_P = 4.0; public static final double DRIVE_PATHING_TRANSLATION_I = 0.0; public static final double DRIVE_PATHING_TRANSLATION_D = 0.3; diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDynamics.java b/src/main/java/frc/robot/subsystems/drive/SwerveDynamics.java new file mode 100644 index 0000000..7165406 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDynamics.java @@ -0,0 +1,63 @@ +package frc.robot.subsystems.drive; + +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.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * Second-order kinematics correction for swerve (curvature-induced translation skew). + * + *

Based on Team 254 and the Chief Delphi + * whitepaper. + * Applied in {@link SwerveDrive} before {@link edu.wpi.first.math.kinematics.SwerveDriveKinematics} + * converts chassis speeds to module states. + */ +public final class SwerveDynamics { + private static final double kDefaultLoopTimeSeconds = 0.02; + + private SwerveDynamics() {} + + /** + * Corrects chassis speeds for one 20 ms loop using the default control period. + * + * @param originalSpeeds desired speeds before skew correction + * @return adjusted speeds to send to module kinematics + */ + public static ChassisSpeeds correctForDynamics(ChassisSpeeds originalSpeeds) { + return correctForDynamics(originalSpeeds, kDefaultLoopTimeSeconds); + } + + /** + * @param loopTimeSeconds integration period (s), typically {@code 0.02} + */ + static ChassisSpeeds correctForDynamics(ChassisSpeeds originalSpeeds, double loopTimeSeconds) { + Pose2d futureRobotPose = new Pose2d( + originalSpeeds.vxMetersPerSecond * loopTimeSeconds, + originalSpeeds.vyMetersPerSecond * loopTimeSeconds, + Rotation2d.fromRadians(originalSpeeds.omegaRadiansPerSecond * loopTimeSeconds)); + Twist2d twistForPose = poseLog(futureRobotPose); + return new ChassisSpeeds( + twistForPose.dx / loopTimeSeconds, + twistForPose.dy / loopTimeSeconds, + twistForPose.dtheta / loopTimeSeconds); + } + + /** Lie algebra log map for a small pose delta (internal; covered by unit tests). */ + static Twist2d poseLog(Pose2d transform) { + final double epsilon = 1E-9; + final double dTheta = transform.getRotation().getRadians(); + final double halfDTheta = 0.5 * dTheta; + final double cosMinusOne = Math.cos(transform.getRotation().getRadians()) - 1.0; + double halfThetaByTanOfHalfDTheta; + if (Math.abs(cosMinusOne) < epsilon) { + halfThetaByTanOfHalfDTheta = 1.0 - 1.0 / 12.0 * dTheta * dTheta; + } else { + halfThetaByTanOfHalfDTheta = -(halfDTheta * Math.sin(transform.getRotation().getRadians())) / cosMinusOne; + } + Translation2d translationPart = transform.getTranslation() + .rotateBy(new Rotation2d(halfThetaByTanOfHalfDTheta, -halfDTheta)); + return new Twist2d(translationPart.getX(), translationPart.getY(), dTheta); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 187adfe..4cca1e6 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -6,87 +6,141 @@ /*----------------------------------------------------------------------------*/ package frc.robot.subsystems.drive; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.hardware.CANcoder; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.ctre.phoenix6.sim.CANcoderSimState; +import com.revrobotics.sim.SparkMaxSim; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.spark.config.ClosedLoopConfig; -import com.revrobotics.spark.config.EncoderConfig; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.ResetMode; - +import frc.robot.util.AngleUtils; /** - * Swerve module for a swerve drive style drivetrain. - * - * @see {@link frc.robot.subsystems.drive.SwerveDrive} + * One swerve corner: NEO drive on a Spark MAX (velocity + feedforward) and pivot on a Spark MAX with + * {@link CANcoder} absolute angle. + * + *

Applies WPILib-style module optimization via {@link SwerveModuleOptimization}. On desktop, + * attaches {@link SparkMaxSim} for physics simulation in {@link #simulationPeriodic()}. + * + * @see SwerveDrive */ public class SwerveModule extends SubsystemBase { + /** Module label (e.g. {@code FrontLeft}) for logging and Shuffleboard. */ public final String name; + /** Calibrated CANcoder offset added to absolute reading (degrees). */ public final double offsetAngle; - private SparkMax driveMotor; - private RelativeEncoder driveEncoder; - private SparkClosedLoopController drivePID; + private final SparkMax driveMotor; + private final RelativeEncoder driveEncoder; + private final SparkClosedLoopController drivePid; private final SimpleMotorFeedforward driveFeedForward; private final CANcoder pivotEncoder; - private SparkMax pivotMotor; - private final PIDController pivotPID; + private final SparkMax pivotMotor; + private final PIDController pivotPid; private boolean isCurrentlyFlippedForShorterPath; + private final SparkMaxSim driveSim; + private final SparkMaxSim pivotSim; + + /** + * @param name module corner name + * @param driveMotorChannel CAN ID of the drive Spark MAX + * @param pivotMotorChannel CAN ID of the pivot Spark MAX + * @param pivotP pivot PID proportional gain + * @param pivotI pivot PID integral gain + * @param pivotD pivot PID derivative gain + * @param driveP drive PID proportional gain (Spark closed-loop) + * @param driveI drive PID integral gain + * @param driveD drive PID derivative gain + * @param drivekS drive feedforward kS (V) + * @param drivekV drive feedforward kV (V·s/m) + * @param drivekA drive feedforward kA (V·s²/m) + * @param pivotEncoderId CAN ID of the pivot CANcoder + * @param offsetAngle pivot encoder offset (degrees) + */ + public SwerveModule( + String name, + int driveMotorChannel, + int pivotMotorChannel, + double pivotP, + double pivotI, + double pivotD, + double driveP, + double driveI, + double driveD, + double drivekS, + double drivekV, + double drivekA, + int pivotEncoderId, + double offsetAngle) { + this(name, driveMotorChannel, pivotMotorChannel, pivotP, pivotI, pivotD, driveP, driveI, driveD, drivekS, drivekV, + drivekA, pivotEncoderId, false, offsetAngle); + } + /** - * Constructs a Swerve Module. + * Full constructor including drive motor inversion. * - * @param name the name/position of the module - * @param driveMotorChannel CAN ID for the drive motor - * @param pivotMotorChannel CAN ID for the pivot motor - * @param pivotP P value of Pivot PID - * @param pivotI I value of Pivot PID - * @param pivotD D value of Pivot PID - * @param driveP P value of Drive PID - * @param driveI I value of Drive PID - * @param driveD D value of Drive PID - * @param pivotEncoderId port number for the pivot CANCoder - * @param reversed true if drive motor is reversed + * @param driveInverted true if the drive motor direction is reversed on the CAN bus */ - public SwerveModule(String name, int driveMotorChannel, int pivotMotorChannel, double pivotP, double pivotI, - double pivotD, double driveP, double driveI, double driveD, - double drivekS, double drivekV, double drivekA, int pivotEncoderId, boolean reversed, double offsetAngle) { + public SwerveModule( + String name, + int driveMotorChannel, + int pivotMotorChannel, + double pivotP, + double pivotI, + double pivotD, + double driveP, + double driveI, + double driveD, + double drivekS, + double drivekV, + double drivekA, + int pivotEncoderId, + boolean driveInverted, + double offsetAngle) { super(SwerveDrive.class.getSimpleName() + "/Module-" + name); this.name = name; + this.offsetAngle = offsetAngle; - var driveEncoderConfig = new EncoderConfig() + EncoderConfig driveEncoderConfig = new EncoderConfig() .positionConversionFactor(SwerveDriveConfig.kDriveEncoderPositionConversionFactor) .velocityConversionFactor(SwerveDriveConfig.kDriveEncoderVelocityConversionFactor); - var driveClosedLoopConfig = new ClosedLoopConfig() + ClosedLoopConfig driveClosedLoopConfig = new ClosedLoopConfig() .pid(driveP, driveI, driveD, ClosedLoopSlot.kSlot0); var driveMotorConfig = new SparkMaxConfig() .idleMode(IdleMode.kBrake) .smartCurrentLimit(40) - .inverted(reversed) + .inverted(driveInverted) .apply(driveEncoderConfig) .apply(driveClosedLoopConfig); + driveMotor = new SparkMax(driveMotorChannel, MotorType.kBrushless); - driveMotor.configure(driveMotorConfig, ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); - drivePID = driveMotor.getClosedLoopController(); + driveMotor.configure(driveMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + drivePid = driveMotor.getClosedLoopController(); driveFeedForward = new SimpleMotorFeedforward(drivekS, drivekV, drivekA); driveEncoder = driveMotor.getEncoder(); @@ -94,37 +148,79 @@ public SwerveModule(String name, int driveMotorChannel, int pivotMotorChannel, d .idleMode(IdleMode.kBrake) .smartCurrentLimit(30); pivotMotor = new SparkMax(pivotMotorChannel, MotorType.kBrushless); - pivotMotor.configure(pivotMotorConfig, ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); + pivotMotor.configure(pivotMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); pivotEncoder = new CANcoder(pivotEncoderId); - pivotPID = new PIDController(pivotP, pivotI, pivotD); - pivotPID.enableContinuousInput(-180, 180); - pivotPID.setTolerance(1); - - this.offsetAngle = offsetAngle; + pivotPid = new PIDController(pivotP, pivotI, pivotD); + pivotPid.enableContinuousInput(-180, 180); + pivotPid.setTolerance(1); + + if (!RobotBase.isReal()) { + DCMotor neo = DCMotor.getNEO(1); + driveSim = new SparkMaxSim(driveMotor, neo); + pivotSim = new SparkMaxSim(pivotMotor, neo); + driveSim.enable(); + pivotSim.enable(); + } else { + driveSim = null; + pivotSim = null; + } SmartDashboard.putData(this); } /** - * Constructs a Swerve Module. - * - * @param name the name/position of the module - * @param driveMotorChannel CAN ID for the drive motor - * @param pivotMotorChannel CAN ID for the pivot motor - * @param pivotP P value of Pivot PID - * @param pivotI I value of Pivot PID - * @param pivotD D value of Pivot PID + * Advances REV {@link SparkMaxSim} physics for drive and pivot (desktop / unit tests only). + */ + @Override + public void simulationPeriodic() { + if (driveSim == null) { + return; + } + double busVoltage = RoboRioSim.getVInVoltage(); + driveSim.iterate(driveSim.getVelocity(), busVoltage, 0.02); + pivotSim.iterate(pivotSim.getVelocity(), busVoltage, 0.02); + } + + /** Steps REV physics sim; used by unit tests and robot simulation. */ + void simulationStep() { + simulationPeriodic(); + } + + /** Whether the last {@link #setDesiredState} chose the inverted-drive optimization path. */ + boolean isOptimizingAngle() { + return isCurrentlyFlippedForShorterPath; + } + + /** @return CTRE sim state for injecting pivot angle in simulation */ + CANcoderSimState pivotEncoderSimState() { + return pivotEncoder.getSimState(); + } + + /** Forces a fresh CANcoder absolute position sample (sim / tests). */ + void refreshPivotAbsolutePosition() { + BaseStatusSignal.refreshAll(pivotEncoder.getAbsolutePosition()); + } + + /** @return pivot {@link SparkMaxSim}, or null on the roboRIO */ + SparkMaxSim pivotSparkSim() { + return pivotSim; + } + + /** @return drive {@link SparkMaxSim}, or null on the roboRIO */ + SparkMaxSim driveSparkSim() { + return driveSim; + } + + /** + * Closes Spark and CANcoder handles. Call from sim tests so CAN IDs can be reused in the same JVM. */ - public SwerveModule(String name, int driveMotorChannel, int pivotMotorChannel, double pivotP, double pivotI, - double pivotD, double driveP, double driveI, double driveD, - double drivekS, double drivekV, double drivekA, int pivotEncoderId, double offsetAngle) { - this(name, driveMotorChannel, pivotMotorChannel, pivotP, pivotI, pivotD, driveP, driveI, - driveD, drivekS, drivekV, drivekA, pivotEncoderId, false, offsetAngle); + public void close() { + driveMotor.close(); + pivotMotor.close(); + pivotEncoder.close(); } - /** {@inheritDoc} */ @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); @@ -135,170 +231,80 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Angle", () -> getState().angle.getDegrees(), null); builder.addBooleanProperty("IsOptimizingAngle", () -> isCurrentlyFlippedForShorterPath, null); builder.addDoubleProperty("OffsetAngle", () -> offsetAngle, null); - } - - /** {@inheritDoc} */ - @Override - public void periodic() { } - /** - * Gets the current drive speed and pivot angle of the module. - * - * @return the current drive speed and pivot angle of the module - */ + /** @return measured drive velocity and pivot angle as a {@link SwerveModuleState} */ public SwerveModuleState getState() { return new SwerveModuleState(driveEncoder.getVelocity(), Rotation2d.fromDegrees(getAngle())); - } + } - /** - * Gets the current drive distance traveled and pivot angle of the module. - * - * @return the current drive distance traveled and pivot angle of the module - */ + /** @return integrated drive distance and pivot angle for odometry */ public SwerveModulePosition getPosition() { return new SwerveModulePosition(driveEncoder.getPosition(), Rotation2d.fromDegrees(getAngle())); - } + } /** - * Sets the PID values for the pivot module. + * Commands drive speed and pivot angle with shortest-path optimization enabled. * - * @param p the P value for the pivot module - * @param i the I value for the pivot module - * @param d the D value for the pivot module + * @param state desired wheel speed (m/s) and module heading */ - public void setPivotPID(double p, double i, double d) { - pivotPID.setPID(p, i, d); + public void setDesiredState(SwerveModuleState state) { + setDesiredState(state, true); } /** - * Sets the desired state for the module. - * - * @param state desired state with the speed and angle - * @param useShortestPath whether or not to use the shortest path + * @param state desired wheel speed and heading + * @param useShortestPath if true, may flip drive direction per {@link SwerveModuleOptimization} */ public void setDesiredState(SwerveModuleState state, boolean useShortestPath) { - // pivot - var currentPivotAngle = getAngle(); - var targetPivotAngle = useShortestPath ? calculateShortestPath(state.angle.getDegrees()) : state.angle.getDegrees(); - var pivotOutput = MathUtil.clamp(pivotPID.calculate(currentPivotAngle, targetPivotAngle), -1, 1); + double currentPivotAngle = getAngle(); + SwerveModuleOptimization.PivotTarget pivotTarget; + if (useShortestPath) { + pivotTarget = SwerveModuleOptimization.shortestPivotTarget(currentPivotAngle, state.angle.getDegrees()); + isCurrentlyFlippedForShorterPath = pivotTarget.driveInverted(); + } else { + pivotTarget = new SwerveModuleOptimization.PivotTarget(state.angle.getDegrees(), false); + isCurrentlyFlippedForShorterPath = false; + } + double targetPivotAngle = pivotTarget.angleDegrees(); + double pivotOutput = MathUtil.clamp(pivotPid.calculate(currentPivotAngle, targetPivotAngle), -1, 1); pivotMotor.set(pivotOutput); - // drive - var sign = isCurrentlyFlippedForShorterPath && useShortestPath ? -1 : 1; - var cosineComp = calculateCosineCompensation(currentPivotAngle, targetPivotAngle); - var driveOutput = sign * cosineComp * state.speedMetersPerSecond; - drivePID.setSetpoint(driveOutput, SparkMax.ControlType.kVoltage, ClosedLoopSlot.kSlot0, + int sign = pivotTarget.driveInverted() && useShortestPath ? -1 : 1; + double cosineComp = SwerveModuleOptimization.cosineScale(currentPivotAngle, targetPivotAngle); + double driveOutput = sign * cosineComp * state.speedMetersPerSecond; + drivePid.setSetpoint( + driveOutput, + SparkMax.ControlType.kVoltage, + ClosedLoopSlot.kSlot0, driveFeedForward.calculate(driveOutput)); } /** - * Sets the desired state for the module. - * - * @param state desired state with the speed and angle + * @param speedMetersPerSecond wheel linear speed (m/s) + * @param angleDegrees module azimuth in degrees [-180, 180] */ - public void setDesiredState(SwerveModuleState state) { - setDesiredState(state, true); + public void setDesiredState(double speedMetersPerSecond, double angleDegrees) { + setDesiredState(speedMetersPerSecond, angleDegrees, true); } - /** - * Sets the desired state for the module. - * - * @param speed the desired speed in meters/second - * @param angle the desired angle in degrees from [-180, 180] - * @param useShortestPath whether or not to use the shortest path - */ - public void setDesiredState(double speed, double angle, boolean useShortestPath) { - setDesiredState(new SwerveModuleState(speed, Rotation2d.fromDegrees(angle)), useShortestPath); + public void setDesiredState(double speedMetersPerSecond, double angleDegrees, boolean useShortestPath) { + setDesiredState(new SwerveModuleState(speedMetersPerSecond, Rotation2d.fromDegrees(angleDegrees)), useShortestPath); } /** - * Sets the desired state for the module. - * - * @param speed the desired speed in meters/second - * @param angle the desired angle in degrees from [-180, 180] - */ - public void setDesiredState(double speed, double angle) { - setDesiredState(speed, angle, true); - } - - /** - * Gets the angle of the pivot module. - * - * @return the angle of the pivot module ranging from [-180,180] + * @return pivot angle in degrees [-180, 180], from CANcoder rotations plus {@link #offsetAngle} */ public double getAngle() { - double voltage = pivotEncoder.getAbsolutePosition().getValueAsDouble(); - double angle = voltage * 360 + offsetAngle; - - if (angle > 0) { - angle %= 360; - if (angle > 180) { - angle -= 360; - } - } else if (angle < 0) { - angle %= -360; - if (angle < -180) { - angle += 360; - } - } - - return angle; + double angle = pivotEncoder.getAbsolutePosition().getValueAsDouble() * 360 + offsetAngle; + return AngleUtils.normalizeDegrees(angle); } - /** - * Stops the module and resets the pivot PID controller. - */ + /** Sets drive and pivot outputs to zero and resets pivot PID. */ public void stop() { driveMotor.set(0); pivotMotor.set(0); - pivotPID.reset(); + pivotPid.reset(); } - /** - * Calculates the shortest path the pivot module should take, it - * might be the given targetAngle. Flips the drive motor - * if there is a shorter path. - * - *

- * - * https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html#module-angle-optimization - * - * @param targetAngle the desired angle of the module - * @return the shortest path to the target angle, flips the - * drive motor if there is a shorter path - */ - private double calculateShortestPath(double targetAngle) { - var currentAngle = getAngle(); - var dAngle = Math.abs(targetAngle - currentAngle); - - isCurrentlyFlippedForShorterPath = dAngle > 90 && dAngle < 270; - - if (isCurrentlyFlippedForShorterPath) { - if (targetAngle > 0 || targetAngle == 0 && currentAngle < 0) { - targetAngle -= 180; - } else if (targetAngle < 0 || targetAngle == 0 && currentAngle > 0) { - targetAngle += 180; - } - } - - return targetAngle; - } - - /** - * Control optimization technique to reduce the amount the module translates - * before reaching the target pivot angle. - * - *

- * - * https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html#cosine-compensation - * - * @param currentAngle the current angle of the module - * @param targetAngle the target angle of the module - * @return the cosine of the angle between the given angles - */ - private double calculateCosineCompensation(double currentAngle, double targetAngle) { - var angleError = Math.abs(currentAngle - targetAngle); - return Math.cos(Math.toRadians(angleError)); - } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModuleOptimization.java b/src/main/java/frc/robot/subsystems/drive/SwerveModuleOptimization.java new file mode 100644 index 0000000..c89e5a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModuleOptimization.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.drive; + +/** + * WPILib swerve module optimizations: shortest pivot path and cosine drive scaling. + * + * @see Swerve drive kinematics + */ +public final class SwerveModuleOptimization { + private SwerveModuleOptimization() {} + + /** + * Result of pivot angle optimization. + * + * @param angleDegrees pivot setpoint to track (may differ from requested angle by 180°) + * @param driveInverted if true, wheel speed should be negated to follow the shorter pivot path + */ + public record PivotTarget(double angleDegrees, boolean driveInverted) {} + + /** + * Chooses pivot target and whether to run the drive motor inverted per WPILib module optimization. + * + * @param currentAngleDegrees measured pivot angle + * @param targetAngleDegrees desired module heading from kinematics + */ + public static PivotTarget shortestPivotTarget(double currentAngleDegrees, double targetAngleDegrees) { + double targetAngle = targetAngleDegrees; + double deltaAngle = Math.abs(targetAngle - currentAngleDegrees); + boolean driveInverted = deltaAngle > 90 && deltaAngle < 270; + + if (driveInverted) { + if (targetAngle > 0 || targetAngle == 0 && currentAngleDegrees < 0) { + targetAngle -= 180; + } else if (targetAngle < 0 || targetAngle == 0 && currentAngleDegrees > 0) { + targetAngle += 180; + } + } + + return new PivotTarget(targetAngle, driveInverted); + } + + /** + * Cosine compensation factor in [0, 1] based on pivot error (reduces wheel speed while turning). + */ + public static double cosineScale(double currentAngleDegrees, double targetAngleDegrees) { + return Math.cos(Math.toRadians(Math.abs(currentAngleDegrees - targetAngleDegrees))); + } +} diff --git a/src/main/java/frc/robot/util/AngleUtils.java b/src/main/java/frc/robot/util/AngleUtils.java new file mode 100644 index 0000000..df6ba62 --- /dev/null +++ b/src/main/java/frc/robot/util/AngleUtils.java @@ -0,0 +1,30 @@ +package frc.robot.util; + +/** + * Shared angle normalization for mechanisms that report degrees outside ±180°. + */ +public final class AngleUtils { + private AngleUtils() {} + + /** + * Wraps an angle to the principal range {@code [-180, 180]} degrees. + * + * @param angleDegrees input angle in degrees + * @return equivalent angle in [-180, 180] + */ + public static double normalizeDegrees(double angleDegrees) { + double angle = angleDegrees; + if (angle > 0) { + angle %= 360; + if (angle > 180) { + angle -= 360; + } + } else if (angle < 0) { + angle %= -360; + if (angle < -180) { + angle += 360; + } + } + return angle; + } +} diff --git a/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java b/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java new file mode 100644 index 0000000..2a287a0 --- /dev/null +++ b/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java @@ -0,0 +1,211 @@ +package frc.robot.commands.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.TestInstance; +import org.junit.jupiter.api.extension.ExtendWith; + +import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj.simulation.XboxControllerSim; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +import frc.robot.constants.FieldConstants; +import frc.robot.hid.DemonCommandXboxController; +import frc.robot.subsystems.drive.SwerveDrive; +import frc.robot.subsystems.drive.SwerveDriveTestFixture; +import frc.robot.testsupport.WpilibSimulationExtension; + +/** End-to-end {@link DriveCommands} against a sim {@link SwerveDrive} (one drivetrain per class). */ +@ExtendWith(WpilibSimulationExtension.class) +@TestInstance(TestInstance.Lifecycle.PER_CLASS) +class DriveCommandsSwerveIntegrationTest { + private static final int kControllerPort = 3; + private static final double DELTA = 1e-3; + + private SwerveDriveTestFixture fixture; + private SwerveDrive swerve; + private DemonCommandXboxController controller; + private XboxControllerSim controllerSim; + + @BeforeEach + void setUp() { + fixture = SwerveDriveTestFixture.open(); + swerve = fixture.swerve(); + // New instances each test: WpilibSimulationExtension resets DriverStation data before each method. + controller = new DemonCommandXboxController(kControllerPort); + controllerSim = new XboxControllerSim(kControllerPort); + fixture.resetBetweenTests(); + resetControllerSticks(); + setAllianceBlue(); + } + + @AfterEach + void tearDown() { + fixture.resetBetweenTests(); + resetControllerSticks(); + setAllianceBlue(); + } + + private void resetControllerSticks() { + controllerSim.setLeftY(0.0); + controllerSim.setLeftX(0.0); + controllerSim.setRightX(0.0); + controllerSim.notifyNewData(); + } + + private void setAllianceBlue() { + DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); + DriverStationSim.notifyNewData(); + } + + @Test + void driveWithXboxController_centeredThenFullStick_togglesMotorOutput() { + // Arrange + Command teleop = DriveCommands.driveWithXboxController( + swerve, controller, () -> false, 0.05, 0.0); + CommandScheduler.getInstance().schedule(teleop); + + // Act — centered sticks + resetControllerSticks(); + SimHooks.stepTiming(0.02); + for (int i = 0; i < 3; i++) { + CommandScheduler.getInstance().run(); + fixture.runSimulationSteps(1); + } + + // Act — full forward + controllerSim.setLeftY(-1.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + for (int i = 0; i < 8; i++) { + CommandScheduler.getInstance().run(); + fixture.runSimulationSteps(1); + } + + // Assert + assertTrue(fixture.maxAbsDriveMotorOutput() > 0.1); + } + + @Test + void resetHeading_setsPracticeFieldPose() { + // Arrange + Command reset = DriveCommands.resetHeading( + swerve, () -> FieldConstants.kDriverPracticeResetX, () -> FieldConstants.kDriverPracticeResetY); + + // Act + CommandScheduler.getInstance().schedule(reset); + CommandScheduler.getInstance().run(); + + // Assert + assertEquals(FieldConstants.kDriverPracticeResetX, swerve.getPose().getX(), DELTA); + assertEquals(FieldConstants.kDriverPracticeResetY, swerve.getPose().getY(), DELTA); + assertEquals(0.0, swerve.getPose().getRotation().getDegrees(), DELTA); + } + + @Test + void stopCommand_invokesSubsystemStop() { + // Arrange + swerve.drive(1.0, 0.0, 0.0, false); + fixture.runSimulationSteps(3); + assertTrue(fixture.maxAbsDriveMotorOutput() > 0.01); + Command stop = DriveCommands.stop(swerve); + + // Act + CommandScheduler.getInstance().schedule(stop); + CommandScheduler.getInstance().run(); + + // Assert + assertEquals(0.0, fixture.maxAbsDriveMotorOutput(), DELTA); + } + + @Test + void lockCommand_whileScheduled_runsUntilCancelled() { + // Arrange + Command lock = DriveCommands.lock(swerve); + + // Act + CommandScheduler.getInstance().schedule(lock); + CommandScheduler.getInstance().run(); + boolean scheduled = CommandScheduler.getInstance().isScheduled(lock); + CommandScheduler.getInstance().cancel(lock); + CommandScheduler.getInstance().run(); + + // Assert + assertTrue(scheduled); + assertEquals(0.0, fixture.maxAbsDriveMotorOutput(), DELTA); + } + + @Test + void goSlow_whileScheduled_limitsTeleopMaxSpeed() { + // Arrange + Command goSlow = DriveCommands.goSlow(swerve); + Command teleop = DriveCommands.driveWithXboxController( + swerve, controller, () -> false, 0.05, 0.0); + controllerSim.setLeftY(-1.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + + // Act + CommandScheduler.getInstance().schedule(goSlow); + CommandScheduler.getInstance().schedule(teleop); + CommandScheduler.getInstance().run(); + + // Assert + assertEquals(0.5, SwerveDrive.maxTranslationalSpeedMetersPerSecond, DELTA); + assertFalse(Double.isNaN(controller.getDriveTranslation(0.05, 0.0, SwerveDrive.maxTranslationalSpeedMetersPerSecond) + .xMetersPerSecond())); + } + + @Test + void holdCurrentHeadingWhileDriving_schedulesAndRunsWithoutInterrupting() { + // Arrange + swerve.resetPose(new Pose2d(0, 0, Rotation2d.kZero)); + Rotation2d headingBefore = swerve.getPose().getRotation(); + Command defense = DriveCommands.holdCurrentHeadingWhileDriving( + swerve, controller, () -> false, 0.05, 0.0); + controllerSim.setLeftY(-1.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + + // Act + CommandScheduler.getInstance().schedule(defense); + for (int i = 0; i < 8; i++) { + CommandScheduler.getInstance().run(); + fixture.runSimulationSteps(1); + } + + // Assert + assertTrue(CommandScheduler.getInstance().isScheduled(defense) || defense.isFinished()); + assertEquals(headingBefore.getDegrees(), swerve.getPose().getRotation().getDegrees(), DELTA); + } + + @Test + void fieldRelativeAllianceSign_readsDriverStationSimAlliance() { + // Arrange & Act — red + DriverStationSim.setAllianceStationId(AllianceStationID.Red1); + DriverStationSim.notifyNewData(); + + // Assert + assertEquals(Alliance.Red, DriverStation.getAlliance().orElse(Alliance.Blue)); + assertEquals(-1, DemonCommandXboxController.fieldRelativeAllianceSign(() -> true)); + + // Arrange & Act — blue + setAllianceBlue(); + + // Assert + assertEquals(Alliance.Blue, DriverStation.getAlliance().orElse(Alliance.Blue)); + assertEquals(1, DemonCommandXboxController.fieldRelativeAllianceSign(() -> true)); + } +} diff --git a/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java b/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java new file mode 100644 index 0000000..3e7ee5e --- /dev/null +++ b/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java @@ -0,0 +1,93 @@ +package frc.robot.commands.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.extension.ExtendWith; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +import frc.robot.hid.HIDUtils; +import frc.robot.subsystems.drive.SwerveDrive; +import frc.robot.testsupport.WpilibSimulationExtension; + +@ExtendWith(WpilibSimulationExtension.class) +class DriveCommandsTest { + private double savedTranslationalMax; + private double savedAngularMax; + + @BeforeEach + void setUp() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().run(); + SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.6; + SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); + savedTranslationalMax = 4.6; + savedAngularMax = Math.toRadians(540); + } + + @AfterEach + void tearDown() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().run(); + SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.6; + SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); + } + + @Test + void goSlow_whileScheduled_lowersMaxSpeeds_restoresOnEnd() { + // Arrange + Command goSlow = DriveCommands.goSlow(null); + + // Act — start + CommandScheduler.getInstance().schedule(goSlow); + CommandScheduler.getInstance().run(); + + // Assert — slow limits + assertEquals(0.5, SwerveDrive.maxTranslationalSpeedMetersPerSecond, 1e-9); + assertEquals(Math.toRadians(90), SwerveDrive.maxAngularSpeedRadiansPerSecond, 1e-9); + + // Act — end + CommandScheduler.getInstance().cancel(goSlow); + CommandScheduler.getInstance().run(); + + // Assert — normal limits restored + assertEquals(4.6, SwerveDrive.maxTranslationalSpeedMetersPerSecond, 1e-9); + assertEquals(Math.toRadians(540), SwerveDrive.maxAngularSpeedRadiansPerSecond, 1e-9); + } + + @Test + void goSlow_lowersStaticMaxUsedForStickScaling() { + // Arrange + Command goSlow = DriveCommands.goSlow(null); + + // Act — start + CommandScheduler.getInstance().schedule(goSlow); + CommandScheduler.getInstance().run(); + // Assert — teleop scales sticks with this static max (linear sticks, full forward) + assertEquals(0.5, SwerveDrive.maxTranslationalSpeedMetersPerSecond, 1e-9); + assertEquals( + 0.5, + HIDUtils.smoothInputWithCubic(1.0, 0.0) * SwerveDrive.maxTranslationalSpeedMetersPerSecond, + 1e-9); + + // Act — end + CommandScheduler.getInstance().cancel(goSlow); + CommandScheduler.getInstance().run(); + + // Assert — restored + assertEquals(4.6, SwerveDrive.maxTranslationalSpeedMetersPerSecond, 1e-9); + } + + @Test + void goSlow_hasExpectedCommandName() { + // Arrange & Act + Command goSlow = DriveCommands.goSlow(null); + + // Assert + assertEquals("SwerveGoSlow", goSlow.getName()); + } +} diff --git a/src/test/java/frc/robot/constants/FieldConstantsTest.java b/src/test/java/frc/robot/constants/FieldConstantsTest.java new file mode 100644 index 0000000..1dc901e --- /dev/null +++ b/src/test/java/frc/robot/constants/FieldConstantsTest.java @@ -0,0 +1,32 @@ +package frc.robot.constants; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.geometry.Rotation2d; + +class FieldConstantsTest { + @Test + void defaultOdometryPose_isExpectedStartPose() { + // Arrange & Act + var pose = FieldConstants.kDefaultOdometryPose; + + // Assert + assertEquals(8.5, pose.getX(), 1e-9); + assertEquals(4.0, pose.getY(), 1e-9); + assertEquals(0.0, pose.getRotation().getDegrees(), 1e-9); + assertEquals(Rotation2d.kZero, pose.getRotation()); + } + + @Test + void driverPracticeReset_hasExpectedFieldCoordinates() { + // Arrange & Act — season constants (no mutation) + double resetX = FieldConstants.kDriverPracticeResetX; + double resetY = FieldConstants.kDriverPracticeResetY; + + // Assert + assertEquals(1.016, resetX, 1e-9); + assertEquals(4.0, resetY, 1e-9); + } +} diff --git a/src/test/java/frc/robot/hid/DemonCommandXboxControllerSimulationTest.java b/src/test/java/frc/robot/hid/DemonCommandXboxControllerSimulationTest.java new file mode 100644 index 0000000..7ad1899 --- /dev/null +++ b/src/test/java/frc/robot/hid/DemonCommandXboxControllerSimulationTest.java @@ -0,0 +1,56 @@ +package frc.robot.hid; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.extension.ExtendWith; + +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj.simulation.XboxControllerSim; + +import frc.robot.testsupport.WpilibSimulationExtension; + +@ExtendWith(WpilibSimulationExtension.class) +class DemonCommandXboxControllerSimulationTest { + private static final double DELTA = 1e-2; + private static final int kControllerPort = 0; + + private DemonCommandXboxController controller; + private XboxControllerSim controllerSim; + + @BeforeEach + void setUpController() { + controller = new DemonCommandXboxController(kControllerPort); + controllerSim = new XboxControllerSim(kControllerPort); + } + + @Test + void getDriveTranslation_usesSimulatedJoystickAxes() { + // Arrange + controllerSim.setLeftY(-1.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + + // Act + DemonCommandXboxController.DriveTranslation speeds = controller.getDriveTranslation(0.05, 0.0, 4.0); + + // Assert + assertEquals(4.0, speeds.xMetersPerSecond(), DELTA); + assertEquals(0.0, speeds.yMetersPerSecond(), DELTA); + } + + @Test + void getDriveRotation_usesSimulatedRightStick() { + // Arrange + controllerSim.setRightX(-1.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + + // Act + double omega = controller.getDriveRotation(0.05, 0.0, Math.toRadians(360)); + + // Assert + assertEquals(Math.toRadians(360), omega, DELTA); + } +} diff --git a/src/test/java/frc/robot/hid/DemonCommandXboxControllerTest.java b/src/test/java/frc/robot/hid/DemonCommandXboxControllerTest.java new file mode 100644 index 0000000..6a96e9c --- /dev/null +++ b/src/test/java/frc/robot/hid/DemonCommandXboxControllerTest.java @@ -0,0 +1,108 @@ +package frc.robot.hid; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +class DemonCommandXboxControllerTest { + @Test + void driveTranslationFromAxes_belowDeadband_isZero() { + // Arrange + double leftY = 0.01; + double leftX = 0.01; + double deadband = 0.05; + double maxTranslation = 4.0; + + // Act + DemonCommandXboxController.DriveTranslation speeds = DemonCommandXboxController.driveTranslationFromAxes( + leftY, leftX, deadband, 0.0, maxTranslation); + + // Assert + assertEquals(0.0, speeds.xMetersPerSecond(), 1e-9); + assertEquals(0.0, speeds.yMetersPerSecond(), 1e-9); + } + + @Test + void driveTranslationFromAxes_fullForward_usesInvertedLeftY() { + // Arrange + double leftY = -1.0; + double leftX = 0.0; + double deadband = 0.05; + double maxTranslation = 4.0; + + // Act + DemonCommandXboxController.DriveTranslation speeds = DemonCommandXboxController.driveTranslationFromAxes( + leftY, leftX, deadband, 0.0, maxTranslation); + + // Assert + assertEquals(4.0, speeds.xMetersPerSecond(), 1e-9); + assertEquals(0.0, speeds.yMetersPerSecond(), 1e-9); + } + + @Test + void fieldRelativeAllianceSign_redAllianceAndFieldRelative_flipsSign() { + // Arrange & Act + int fieldRelativeRed = DemonCommandXboxController.fieldRelativeAllianceSign(true, Alliance.Red); + int notFieldRelativeRed = DemonCommandXboxController.fieldRelativeAllianceSign(false, Alliance.Red); + + // Assert + assertEquals(-1, fieldRelativeRed); + assertEquals(1, notFieldRelativeRed); + } + + @Test + void driveTranslationFromAxes_fullStick_appliesCubicSmoothingToBothAxes() { + // Arrange + double maxTranslation = 4.6; + double sensitivity = 0.5; + double expectedX = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxTranslation; + double expectedY = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxTranslation; + + // Act + DemonCommandXboxController.DriveTranslation speeds = DemonCommandXboxController.driveTranslationFromAxes( + -1.0, -1.0, 0.05, sensitivity, maxTranslation); + + // Assert + assertEquals(expectedX, speeds.xMetersPerSecond(), 1e-9); + assertEquals(expectedY, speeds.yMetersPerSecond(), 1e-9); + } + + @Test + void rotationFromAxis_fullStick_appliesCubicSmoothing() { + // Arrange + double maxAngular = Math.toRadians(540); + double sensitivity = 0.5; + double expectedOmega = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxAngular; + + // Act + double omega = DemonCommandXboxController.rotationFromAxis(-1.0, 0.05, sensitivity, maxAngular); + + // Assert + assertEquals(expectedOmega, omega, 1e-9); + } + + @Test + void rotationFromAxis_belowDeadband_isZero() { + // Arrange + double rawAxis = 0.02; + double deadband = 0.05; + double maxAngular = Math.toRadians(540); + + // Act + double omega = DemonCommandXboxController.rotationFromAxis(rawAxis, deadband, 0.0, maxAngular); + + // Assert + assertEquals(0.0, omega, 1e-9); + } + + @Test + void fieldRelativeAllianceSign_blueAlliance_doesNotFlip() { + // Arrange & Act + int sign = DemonCommandXboxController.fieldRelativeAllianceSign(true, Alliance.Blue); + + // Assert + assertEquals(1, sign); + } +} diff --git a/src/test/java/frc/robot/hid/HIDUtilsTest.java b/src/test/java/frc/robot/hid/HIDUtilsTest.java new file mode 100644 index 0000000..d5177d2 --- /dev/null +++ b/src/test/java/frc/robot/hid/HIDUtilsTest.java @@ -0,0 +1,75 @@ +package frc.robot.hid; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +class HIDUtilsTest { + @Test + void smoothInputWithCubic_linearWhenSensitivityZero() { + // Arrange + double sensitivity = 0.0; + + // Act + double positiveOutput = HIDUtils.smoothInputWithCubic(0.5, sensitivity); + double negativeOutput = HIDUtils.smoothInputWithCubic(-0.25, sensitivity); + + // Assert + assertEquals(0.5, positiveOutput, 1e-9); + assertEquals(-0.25, negativeOutput, 1e-9); + } + + @Test + void smoothInputWithCubic_cubicWhenSensitivityOne() { + // Arrange + double input = 0.5; + double sensitivity = 1.0; + + // Act + double output = HIDUtils.smoothInputWithCubic(input, sensitivity); + + // Assert + assertEquals(0.125, output, 1e-9); + } + + @Test + void scaledStickAxis_belowDeadband_isZero() { + // Arrange + double rawAxis = 0.02; + double deadband = 0.05; + double maxOutput = 4.0; + + // Act + double output = HIDUtils.scaledStickAxis(rawAxis, deadband, 0.0, maxOutput); + + // Assert + assertEquals(0.0, output, 1e-9); + } + + @Test + void scaledStickAxis_fullDeflection_scalesToMax() { + // Arrange + double rawAxis = 1.0; + double deadband = 0.05; + double maxOutput = 4.0; + + // Act + double output = HIDUtils.scaledStickAxis(rawAxis, deadband, 0.0, maxOutput); + + // Assert + assertEquals(4.0, output, 1e-9); + } + + @Test + void smoothInputWithCubic_zeroInput_isZero() { + // Arrange + double input = 0.0; + double sensitivity = 0.75; + + // Act + double output = HIDUtils.smoothInputWithCubic(input, sensitivity); + + // Assert + assertEquals(0.0, output, 1e-9); + } +} diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveKinematicsTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveKinematicsTest.java new file mode 100644 index 0000000..fd985f6 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveKinematicsTest.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +class SwerveDriveKinematicsTest { + private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics( + SwerveDriveConfig.kFrontLeftModuleOffset, + SwerveDriveConfig.kFrontRightModuleOffset, + SwerveDriveConfig.kRearLeftModuleOffset, + SwerveDriveConfig.kRearRightModuleOffset); + + @Test + void desaturateWheelSpeeds_limitsFastestWheelToMaxTranslationalSpeed() { + // Arrange + ChassisSpeeds corrected = SwerveDynamics.correctForDynamics(new ChassisSpeeds(10.0, 10.0, 0.0)); + double maxTranslational = SwerveDrive.maxTranslationalSpeedMetersPerSecond; + + // Act + SwerveModuleState[] states = kinematics.toSwerveModuleStates(corrected); + SwerveDriveKinematics.desaturateWheelSpeeds(states, maxTranslational); + + // Assert + double fastestWheel = 0.0; + for (SwerveModuleState state : states) { + fastestWheel = Math.max(fastestWheel, state.speedMetersPerSecond); + } + assertEquals(maxTranslational, fastestWheel, 1e-9); + } + + @Test + void toChassisSpeeds_roundTripsRobotRelativeTranslation() { + // Arrange + ChassisSpeeds requested = new ChassisSpeeds(2.5, -0.5, 0.0); + ChassisSpeeds corrected = SwerveDynamics.correctForDynamics(requested); + + // Act + SwerveModuleState[] states = kinematics.toSwerveModuleStates(corrected); + ChassisSpeeds reconstructed = kinematics.toChassisSpeeds(states); + + // Assert + assertEquals(corrected.vxMetersPerSecond, reconstructed.vxMetersPerSecond, 1e-6); + assertEquals(corrected.vyMetersPerSecond, reconstructed.vyMetersPerSecond, 1e-6); + assertEquals(corrected.omegaRadiansPerSecond, reconstructed.omegaRadiansPerSecond, 1e-6); + } + + @Test + void fromFieldRelativeSpeeds_rotatesTranslationWithRobotHeading() { + // Arrange — same transform {@link SwerveDrive#drive} uses when field-relative is enabled + Rotation2d heading = Rotation2d.fromDegrees(90); + + // Act + ChassisSpeeds robotRelative = ChassisSpeeds.fromFieldRelativeSpeeds(3.0, 0.0, 0.0, heading); + + // Assert + assertEquals(0.0, robotRelative.vxMetersPerSecond, 1e-6); + assertEquals(-3.0, robotRelative.vyMetersPerSecond, 1e-6); + assertEquals(0.0, robotRelative.omegaRadiansPerSecond, 1e-6); + } +} diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveMotionTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveMotionTest.java new file mode 100644 index 0000000..eb1f119 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveMotionTest.java @@ -0,0 +1,89 @@ +package frc.robot.subsystems.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; + +class SwerveDriveMotionTest { + private PIDController thetaPid; + + @BeforeEach + void setUpPid() { + thetaPid = new PIDController( + SwerveDriveConfig.DRIVE_ANGLE_P, + SwerveDriveConfig.DRIVE_ANGLE_I, + SwerveDriveConfig.DRIVE_ANGLE_D); + thetaPid.enableContinuousInput(-180.0, 180.0); + } + + @Test + void shouldCoastSwerveModules_allBelowDeadband_isTrue() { + // Arrange & Act + boolean atZero = SwerveDrive.shouldCoastSwerveModules(0.0, 0.0, 0.0); + boolean nearZero = SwerveDrive.shouldCoastSwerveModules(0.004, -0.004, 0.004); + + // Assert + assertTrue(atZero); + assertTrue(nearZero); + } + + @Test + void shouldCoastSwerveModules_anyAxisAboveDeadband_isFalse() { + // Arrange & Act + boolean xAbove = SwerveDrive.shouldCoastSwerveModules(0.006, 0.0, 0.0); + boolean yAbove = SwerveDrive.shouldCoastSwerveModules(0.0, -0.006, 0.0); + boolean omegaAbove = SwerveDrive.shouldCoastSwerveModules(0.0, 0.0, 0.006); + + // Assert + assertFalse(xAbove); + assertFalse(yAbove); + assertFalse(omegaAbove); + } + + @Test + void computeHeadingHoldOmegaRadians_headingError_producesCorrectSign() { + // Arrange — robot at 0°, want 90° + double maxOmega = Math.toRadians(540); + + // Act + double omega = SwerveDrive.computeHeadingHoldOmegaRadians( + thetaPid, Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(90), 1.0, maxOmega); + + // Assert — positive CCW correction, scaled by P gain and max-speed fraction + assertTrue(omega > 0.0); + assertTrue(omega <= maxOmega); + } + + @Test + void computeHeadingHoldOmegaRadians_wrapsSetpointAcross180Boundary() { + // Arrange — current 170°, setpoint -170° (20° error, not 340°) + double maxOmega = Math.toRadians(100); + + // Act + double omega = SwerveDrive.computeHeadingHoldOmegaRadians( + thetaPid, Rotation2d.fromDegrees(170), Rotation2d.fromDegrees(-170), 1.0, maxOmega); + + // Assert — small positive correction + assertTrue(omega > 0.0); + assertTrue(Math.abs(omega) < maxOmega); + } + + @Test + void computeHeadingHoldOmegaRadians_maxSpeedFraction_clampsOutput() { + // Arrange + double maxOmega = Math.toRadians(540); + + // Act + double omega = SwerveDrive.computeHeadingHoldOmegaRadians( + thetaPid, Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(90), 0.25, maxOmega); + + // Assert + assertEquals(0.25 * maxOmega, omega, 1e-6); + } +} diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java new file mode 100644 index 0000000..e2aa977 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java @@ -0,0 +1,266 @@ +package frc.robot.subsystems.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.TestInstance; +import org.junit.jupiter.api.extension.ExtendWith; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +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.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +import frc.robot.constants.FieldConstants; +import frc.robot.testsupport.WpilibSimulationExtension; + +/** One sim {@link SwerveDrive} per class — avoids re-opening the same CAN devices every test method. */ +@ExtendWith(WpilibSimulationExtension.class) +@TestInstance(TestInstance.Lifecycle.PER_CLASS) +class SwerveDriveTest { + private static final double DELTA = 1e-3; + + private SwerveDriveTestFixture fixture; + + @BeforeEach + void setUp() { + fixture = SwerveDriveTestFixture.open(); + fixture.resetBetweenTests(); + } + + @AfterEach + void tearDown() { + fixture.resetBetweenTests(); + } + + @Test + void constructor_seedsPoseFromFieldConstants() { + // Arrange & Act + Pose2d pose = fixture.swerve().getPose(); + + // Assert + assertEquals(FieldConstants.kDefaultOdometryPose.getX(), pose.getX(), DELTA); + assertEquals(FieldConstants.kDefaultOdometryPose.getY(), pose.getY(), DELTA); + } + + @Test + void resetPose_updatesEstimatedPose() { + // Arrange + Pose2d expected = new Pose2d(2.0, 1.5, Rotation2d.fromDegrees(30)); + SwerveDrive swerve = fixture.swerve(); + + // Act + swerve.resetPose(expected); + + // Assert + assertEquals(expected.getX(), swerve.getPose().getX(), DELTA); + assertEquals(expected.getY(), swerve.getPose().getY(), DELTA); + assertEquals(expected.getRotation().getDegrees(), swerve.getPose().getRotation().getDegrees(), DELTA); + } + + @Test + void periodic_usesInjectedHeadingSupplier() { + SwerveDrive swerve = fixture.swerve(); + + // Arrange + swerve.resetPose(Pose2d.kZero); + fixture.setHeading(Rotation2d.fromDegrees(45)); + + // Act + SimHooks.stepTiming(0.02); + swerve.periodic(); + + // Assert + assertEquals(45.0, swerve.getPose().getRotation().getDegrees(), DELTA); + } + + @Test + void getChassisSpeeds_atRest_isNearZero() { + // Arrange & Act + ChassisSpeeds speeds = fixture.swerve().getChassisSpeeds(); + + // Assert + assertEquals(0.0, speeds.vxMetersPerSecond, DELTA); + assertEquals(0.0, speeds.vyMetersPerSecond, DELTA); + assertEquals(0.0, speeds.omegaRadiansPerSecond, DELTA); + } + + @Test + void drive_belowDeadband_zerosModuleWheelSpeeds() { + SwerveDrive swerve = fixture.swerve(); + double belowDeadband = 0.001; + + // Act + swerve.drive(belowDeadband, belowDeadband, belowDeadband, false); + swerve.runSimulationSteps(1); + + // Assert + for (SwerveModuleState state : swerve.getSwerveModuleStates()) { + assertEquals(0.0, state.speedMetersPerSecond, DELTA); + } + } + + @Test + void drive_forwardRobotRelative_commandsDriveMotors() { + SwerveDrive swerve = fixture.swerve(); + + // Act + swerve.drive(1.0, 0.0, 0.0, false); + swerve.runSimulationSteps(5); + + // Assert + assertTrue(swerve.maxAbsDriveMotorOutput() > 0.01); + } + + @Test + void drive_strafeRobotRelative_commandsDriveMotors() { + SwerveDrive swerve = fixture.swerve(); + + // Act + swerve.drive(0.0, 1.0, 0.0, false); + swerve.runSimulationSteps(5); + + // Assert + assertTrue(swerve.maxAbsDriveMotorOutput() > 0.01); + } + + @Test + void drive_pureRotation_commandsMotors() { + SwerveDrive swerve = fixture.swerve(); + + // Act + swerve.drive(0.0, 0.0, 1.0, false); + swerve.runSimulationSteps(5); + + // Assert + assertTrue(swerve.maxAbsDriveMotorOutput() > 0.01); + } + + @Test + void drive_fieldRelative_commandsDriveMotorsWhenPoseRotated() { + SwerveDrive swerve = fixture.swerve(); + + // Arrange — robot facing 90° + swerve.resetPose(new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(90))); + + // Act — field-relative forward + swerve.drive(2.0, 0.0, 0.0, true); + swerve.runSimulationSteps(5); + + // Assert + assertTrue(swerve.maxAbsDriveMotorOutput() > 0.01); + } + + @Test + void holdAngleWhileDriving_headingError_commandsPivotMotors() { + SwerveDrive swerve = fixture.swerve(); + swerve.resetPose(Pose2d.kZero); + + // Act + swerve.holdAngleWhileDriving(0.0, 0.0, Rotation2d.fromDegrees(90), 1.0, false); + swerve.runSimulationSteps(5); + + // Assert + assertTrue(swerve.maxAbsPivotMotorOutput() > 0.01); + } + + @Test + void holdAngleWhileDriving_withTranslation_commandsDriveMotors() { + SwerveDrive swerve = fixture.swerve(); + swerve.resetPose(Pose2d.kZero); + + // Act + swerve.holdAngleWhileDriving(1.5, 0.0, Rotation2d.fromDegrees(0), 1.0, false); + swerve.runSimulationSteps(5); + + // Assert + assertTrue(swerve.maxAbsDriveMotorOutput() > 0.01); + } + + @Test + void lock_doesNotThrowAndStopsModulesOnStop() { + SwerveDrive swerve = fixture.swerve(); + + // Act + swerve.lock(); + swerve.runSimulationSteps(2); + swerve.stop(); + + // Assert + assertEquals(0.0, swerve.maxAbsDriveMotorOutput(), DELTA); + assertEquals(0.0, swerve.maxAbsPivotMotorOutput(), DELTA); + } + + @Test + void getSwerveModulePositions_returnsFourZeroedModulesAtRest() { + SwerveDrive swerve = fixture.swerve(); + + // Arrange & Act + SwerveModulePosition[] positions = swerve.getSwerveModulePositions(); + + // Assert + assertEquals(4, positions.length); + for (SwerveModulePosition position : positions) { + assertEquals(0.0, position.distanceMeters, DELTA); + } + } + + @Test + void addVisionMeasurement_ignoresVisionHeadingWithDefaultStdDevs() { + SwerveDrive swerve = fixture.swerve(); + + // Arrange + swerve.resetPose(Pose2d.kZero); + fixture.setHeading(Rotation2d.kZero); + for (int i = 0; i < 5; i++) { + SimHooks.stepTiming(0.02); + swerve.periodic(); + } + double timestamp = Timer.getFPGATimestamp() - 0.01; + Pose2d visionPose = new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(90)); + + // Act + swerve.addVisionMeasurementForOdometry(visionPose, timestamp, SwerveDrive.kDefaultVisionMeasurementStdDevs); + SimHooks.stepTiming(0.02); + swerve.periodic(); + + // Assert — theta std dev is Double.MAX_VALUE, so heading stays on gyro/odometry + assertEquals(0.0, swerve.getPose().getRotation().getDegrees(), 1.0); + assertTrue(swerve.getPose().getX() > 0.1); + } + + @Test + void addVisionMeasurement_fusesPoseTowardMeasurement() { + SwerveDrive swerve = fixture.swerve(); + Matrix tightVisionStdDevs = VecBuilder.fill(0.02, 0.02, Double.MAX_VALUE); + + // Arrange + swerve.resetPose(Pose2d.kZero); + for (int i = 0; i < 5; i++) { + SimHooks.stepTiming(0.02); + swerve.periodic(); + } + double timestamp = Timer.getFPGATimestamp() - 0.01; + Pose2d visionPose = new Pose2d(2.0, 0.5, Rotation2d.kZero); + + // Act + swerve.addVisionMeasurementForOdometry(visionPose, timestamp, tightVisionStdDevs); + SimHooks.stepTiming(0.02); + swerve.periodic(); + + // Assert + assertTrue(swerve.getPose().getX() > 0.5); + assertTrue(swerve.getPose().getY() > 0.1); + } +} diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java new file mode 100644 index 0000000..d917bb0 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java @@ -0,0 +1,101 @@ +package frc.robot.subsystems.drive; + +import java.util.concurrent.atomic.AtomicReference; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +import frc.robot.constants.FieldConstants; + +/** + * Constructs a sim-backed {@link SwerveDrive} without {@link frc.robot.RobotContainer}. + * + *

Call {@link #close()} (or use try-with-resources) to release module CAN handles. + */ +public final class SwerveDriveTestFixture implements AutoCloseable { + private static SwerveDriveTestFixture active; + + private final AtomicReference heading = new AtomicReference<>(Rotation2d.kZero); + private final SwerveDrive swerve; + + /** Opens the shared sim drivetrain for this JVM (one instance across swerve test classes). */ + public static SwerveDriveTestFixture open() { + if (active == null) { + active = new SwerveDriveTestFixture(); + } + return active; + } + + /** + * Closes the shared instance if it was opened. + * + *

Do not call from per-class {@code @AfterAll} when multiple test classes share this fixture — + * class execution order is undefined and closing early breaks other classes. Prefer leaving the + * sim drivetrain open for the JVM; use this only from try-with-resources in isolated tests. + */ + public static void closeActive() { + if (active != null) { + active.close(); + active = null; + } + } + + private SwerveDriveTestFixture() { + swerve = new SwerveDrive(headingSupplier(), false); + swerve.seedSimulationPivotEncodersZero(); + SimHooks.stepTiming(0.02); + } + + public SwerveDrive swerve() { + return swerve; + } + + public Supplier headingSupplier() { + return heading::get; + } + + public void setHeading(Rotation2d rotation) { + heading.set(rotation); + } + + public void reseedPivotEncoders() { + swerve.seedSimulationPivotEncodersZero(); + } + + /** + * Clears scheduler state, restores teleop speed limits, and resets drivetrain pose/heading for the next test. + * Call from {@code @BeforeEach} / {@code @AfterEach} so tests do not depend on run order. + */ + public void resetBetweenTests() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().run(); + SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.6; + SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); + heading.set(Rotation2d.kZero); + swerve.stop(); + swerve.resetPose(FieldConstants.kDefaultOdometryPose); + swerve.seedSimulationPivotEncodersZero(); + SimHooks.stepTiming(0.02); + } + + public void runSimulationSteps(int iterations) { + swerve.runSimulationSteps(iterations); + } + + public double maxAbsDriveMotorOutput() { + return swerve.maxAbsDriveMotorOutput(); + } + + public double maxAbsPivotMotorOutput() { + return swerve.maxAbsPivotMotorOutput(); + } + + @Override + public void close() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().unregisterSubsystem(swerve); + swerve.closeModulesForTest(); + } +} diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java new file mode 100644 index 0000000..62bd894 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java @@ -0,0 +1,96 @@ +package frc.robot.subsystems.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import org.junit.jupiter.api.Test; + +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; + +class SwerveDynamicsTest { + @Test + void correctForDynamics_pureTranslation_isUnchanged() { + // Arrange + ChassisSpeeds input = new ChassisSpeeds(2.0, -1.0, 0.0); + + // Act + ChassisSpeeds output = SwerveDynamics.correctForDynamics(input); + + // Assert + assertEquals(input.vxMetersPerSecond, output.vxMetersPerSecond, 1e-9); + assertEquals(input.vyMetersPerSecond, output.vyMetersPerSecond, 1e-9); + assertEquals(input.omegaRadiansPerSecond, output.omegaRadiansPerSecond, 1e-9); + } + + @Test + void correctForDynamics_rotationOnly_preservesOmega() { + // Arrange + ChassisSpeeds input = new ChassisSpeeds(0.0, 0.0, Math.PI); + + // Act + ChassisSpeeds output = SwerveDynamics.correctForDynamics(input); + + // Assert + assertEquals(0.0, output.vxMetersPerSecond, 1e-9); + assertEquals(0.0, output.vyMetersPerSecond, 1e-9); + assertEquals(input.omegaRadiansPerSecond, output.omegaRadiansPerSecond, 1e-6); + } + + @Test + void correctForDynamics_curvedMotion_adjustsTranslation() { + // Arrange + ChassisSpeeds input = new ChassisSpeeds(3.0, 0.0, Math.PI / 2.0); + + // Act + ChassisSpeeds output = SwerveDynamics.correctForDynamics(input); + + // Assert + assertEquals(Math.PI / 2.0, output.omegaRadiansPerSecond, 1e-6); + assertNotEquals(input.vyMetersPerSecond, output.vyMetersPerSecond, 1e-6); + assertEquals(input.vxMetersPerSecond, output.vxMetersPerSecond, 1e-2); + } + + @Test + void poseLog_zeroTransform_isZeroTwist() { + // Arrange + Pose2d delta = Pose2d.kZero; + + // Act + Twist2d twist = SwerveDynamics.poseLog(delta); + + // Assert + assertEquals(0.0, twist.dx, 1e-9); + assertEquals(0.0, twist.dy, 1e-9); + assertEquals(0.0, twist.dtheta, 1e-9); + } + + @Test + void poseLog_pureTranslation_matchesTranslation() { + // Arrange + Pose2d delta = new Pose2d(0.5, -0.25, Rotation2d.kZero); + + // Act + Twist2d twist = SwerveDynamics.poseLog(delta); + + // Assert + assertEquals(0.5, twist.dx, 1e-6); + assertEquals(-0.25, twist.dy, 1e-6); + assertEquals(0.0, twist.dtheta, 1e-9); + } + + @Test + void poseLog_smallRotation_usesStableApproximation() { + // Arrange — triggers epsilon branch in poseLog + Pose2d delta = new Pose2d(0.1, 0.0, Rotation2d.fromRadians(1e-10)); + + // Act + Twist2d twist = SwerveDynamics.poseLog(delta); + + // Assert + assertEquals(0.1, twist.dx, 1e-6); + assertEquals(0.0, twist.dy, 1e-6); + } +} diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java new file mode 100644 index 0000000..bd28892 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java @@ -0,0 +1,108 @@ +package frc.robot.subsystems.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +class SwerveModuleOptimizationTest { + @Test + void shortestPivotTarget_smallError_doesNotFlipDrive() { + // Arrange + double currentAngleDegrees = 10; + double targetAngleDegrees = 40; + + // Act + SwerveModuleOptimization.PivotTarget result = + SwerveModuleOptimization.shortestPivotTarget(currentAngleDegrees, targetAngleDegrees); + + // Assert + assertFalse(result.driveInverted()); + assertEquals(40, result.angleDegrees(), 1e-9); + } + + @Test + void shortestPivotTarget_largeError_flipsDriveAndOffsetsTarget() { + // Arrange + double currentAngleDegrees = 10; + double targetAngleDegrees = 170; + + // Act + SwerveModuleOptimization.PivotTarget result = + SwerveModuleOptimization.shortestPivotTarget(currentAngleDegrees, targetAngleDegrees); + + // Assert + assertTrue(result.driveInverted()); + assertEquals(-10, result.angleDegrees(), 1e-9); + } + + @Test + void cosineScale_alignedModule_isOne() { + // Arrange + double currentAngleDegrees = 45; + double targetAngleDegrees = 45; + + // Act + double scale = SwerveModuleOptimization.cosineScale(currentAngleDegrees, targetAngleDegrees); + + // Assert + assertEquals(1.0, scale, 1e-9); + } + + @Test + void shortestPivotTarget_deltaExactly90_doesNotFlipDrive() { + // Arrange — boundary: flip when delta > 90, not when delta == 90 + + // Act + SwerveModuleOptimization.PivotTarget result = + SwerveModuleOptimization.shortestPivotTarget(0, 90); + + // Assert + assertFalse(result.driveInverted()); + assertEquals(90, result.angleDegrees(), 1e-9); + } + + @Test + void shortestPivotTarget_deltaJustAbove90_flipsDrive() { + // Arrange + double currentAngleDegrees = 0; + double targetAngleDegrees = 91; + + // Act + SwerveModuleOptimization.PivotTarget result = + SwerveModuleOptimization.shortestPivotTarget(currentAngleDegrees, targetAngleDegrees); + + // Assert + assertTrue(result.driveInverted()); + assertEquals(-89, result.angleDegrees(), 1e-9); + } + + @Test + void shortestPivotTarget_negativeAngles_usesShortestPath() { + // Arrange + double currentAngleDegrees = -10; + double targetAngleDegrees = -170; + + // Act + SwerveModuleOptimization.PivotTarget result = + SwerveModuleOptimization.shortestPivotTarget(currentAngleDegrees, targetAngleDegrees); + + // Assert + assertTrue(result.driveInverted()); + assertEquals(10, result.angleDegrees(), 1e-9); + } + + @Test + void cosineScale_perpendicularModule_isZero() { + // Arrange + double currentAngleDegrees = 0; + double targetAngleDegrees = 90; + + // Act + double scale = SwerveModuleOptimization.cosineScale(currentAngleDegrees, targetAngleDegrees); + + // Assert + assertEquals(0.0, scale, 1e-9); + } +} diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveModuleSimulationTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveModuleSimulationTest.java new file mode 100644 index 0000000..2b1cc73 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveModuleSimulationTest.java @@ -0,0 +1,111 @@ +package frc.robot.subsystems.drive; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.extension.ExtendWith; + +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.simulation.SimHooks; + +import frc.robot.testsupport.WpilibSimulationExtension; + +/** + * Integration-style tests that exercise REV {@link com.revrobotics.sim.SparkMaxSim} on a real + * {@link SwerveModule} instance (isolated CAN IDs). Requires + * {@link frc.robot.testsupport.WpilibSimulationExtension}. + */ +@ExtendWith(WpilibSimulationExtension.class) +class SwerveModuleSimulationTest { + private static final int kTestDriveCanId = 60; + private static final int kTestPivotCanId = 61; + private static final int kTestPivotEncoderCanId = 62; + + private SwerveModule module; + + @BeforeEach + void setUpModule() { + // Arrange — module with sim-backed hardware (HAL from WpilibSimulationExtension) + module = createTestModule(0.0); + module.pivotEncoderSimState().setSupplyVoltage(12.0); + module.pivotEncoderSimState().setRawPosition(Units.Rotations.of(0.0)); + refreshPivotEncoder(); + } + + @AfterEach + void tearDownModule() { + if (module != null) { + module.close(); + module = null; + } + } + + @Test + void setDesiredState_commandsPivotMotorWhenAngleErrorExists() { + // Arrange + module.pivotEncoderSimState().setRawPosition(Units.Rotations.of(0.0)); + refreshPivotEncoder(); + + // Act + module.setDesiredState(0.0, 90.0); + for (int i = 0; i < 5; i++) { + module.simulationStep(); + SimHooks.stepTiming(0.02); + } + + // Assert + assertTrue(Math.abs(module.pivotSparkSim().getAppliedOutput()) > 0.01); + } + + @Test + void setDesiredState_smallPivotError_doesNotInvertDrive() { + // Arrange + module.pivotEncoderSimState().setRawPosition(Units.Rotations.of(0.0)); + refreshPivotEncoder(); + + // Act + module.setDesiredState(2.0, 20.0); + + // Assert + assertFalse(module.isOptimizingAngle()); + } + + @Test + void setDesiredState_largePivotError_invertsDrive() { + // Arrange + module.pivotEncoderSimState().setRawPosition(Units.Rotations.of(0.0)); + refreshPivotEncoder(); + + // Act + module.setDesiredState(2.0, 170.0); + + // Assert + assertTrue(module.isOptimizingAngle()); + } + + private static SwerveModule createTestModule(double pivotOffsetDegrees) { + return new SwerveModule( + "Test", + kTestDriveCanId, + kTestPivotCanId, + SwerveDriveConfig.DRIVE_MODULE_PIVOT_P, + SwerveDriveConfig.DRIVE_MODULE_PIVOT_I, + SwerveDriveConfig.DRIVE_MODULE_PIVOT_D, + SwerveDriveConfig.DRIVE_MODULE_P, + SwerveDriveConfig.DRIVE_MODULE_I, + SwerveDriveConfig.DRIVE_MODULE_D, + SwerveDriveConfig.DRIVE_MODULE_KS, + SwerveDriveConfig.DRIVE_MODULE_KV, + SwerveDriveConfig.DRIVE_MODULE_KA, + kTestPivotEncoderCanId, + pivotOffsetDegrees); + } + + private void refreshPivotEncoder() { + SimHooks.stepTiming(0.02); + module.refreshPivotAbsolutePosition(); + } +} diff --git a/src/test/java/frc/robot/testsupport/WpilibSimulationExtension.java b/src/test/java/frc/robot/testsupport/WpilibSimulationExtension.java new file mode 100644 index 0000000..a06c138 --- /dev/null +++ b/src/test/java/frc/robot/testsupport/WpilibSimulationExtension.java @@ -0,0 +1,40 @@ +package frc.robot.testsupport; + +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.extension.BeforeEachCallback; +import org.junit.jupiter.api.extension.ExtensionContext; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.SimHooks; + +/** + * JUnit 5 extension that initializes WPILib HAL and a enabled, attached driver station before each test. + * + *

Apply with {@code @ExtendWith(WpilibSimulationExtension.class)} on tests that construct + * {@link edu.wpi.first.wpilibj2.command.button.CommandXboxController}, REV Spark, or CTRE devices. + * + * @see WPILib unit testing + */ +public final class WpilibSimulationExtension implements BeforeEachCallback { + private static boolean halInitialized; + + /** {@inheritDoc} */ + @Override + public void beforeEach(ExtensionContext context) { + if (!halInitialized) { + assertTrue(HAL.initialize(500, 0), "HAL.initialize failed"); + halInitialized = true; + } + DriverStationSim.resetData(); + DriverStationSim.setDsAttached(true); + DriverStationSim.setEnabled(true); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(true); + DriverStationSim.notifyNewData(); + RoboRioSim.setVInVoltage(12.0); + SimHooks.stepTiming(0.02); + } +} diff --git a/src/test/java/frc/robot/util/AngleUtilsTest.java b/src/test/java/frc/robot/util/AngleUtilsTest.java new file mode 100644 index 0000000..41e3794 --- /dev/null +++ b/src/test/java/frc/robot/util/AngleUtilsTest.java @@ -0,0 +1,32 @@ +package frc.robot.util; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.CsvSource; + +class AngleUtilsTest { + @ParameterizedTest + @CsvSource({ + "0, 0", + "180, 180", + "-180, -180", + "360, 0", + "-360, 0", + "270, -90", + "-270, 90", + "450, 90", + "-450, -90", + "181, -179", + "-181, 179" + }) + void normalizeDegrees_wrapsToSignedRange(double inputDegrees, double expectedDegrees) { + // Arrange — inputDegrees and expectedDegrees from @CsvSource + + // Act + double actualDegrees = AngleUtils.normalizeDegrees(inputDegrees); + + // Assert + assertEquals(expectedDegrees, actualDegrees, 1e-9); + } +} diff --git a/src/test/resources/junit-platform.properties b/src/test/resources/junit-platform.properties new file mode 100644 index 0000000..871037e --- /dev/null +++ b/src/test/resources/junit-platform.properties @@ -0,0 +1,2 @@ +# Catch order-dependent tests during local/CI runs (FQCN required — not the shorthand "random") +junit.jupiter.testmethod.order.default=org.junit.jupiter.api.MethodOrderer$Random