From e7d650c5643dc4b59b52df5eb791b89ec4864e55 Mon Sep 17 00:00:00 2001 From: Ernie3 Date: Sun, 18 Oct 2026 22:18:30 -0400 Subject: [PATCH 01/17] Refactor swerve drive and add unit tests Split commands from SwerveDrive, extract dynamics and module optimization, and add pure and simulation unit tests. Co-authored-by: Cursor --- src/main/java/frc/robot/Robot.java | 6 + src/main/java/frc/robot/RobotContainer.java | 29 +- .../robot/commands/drive/DriveCommands.java | 122 +++ .../commands/drive/SwerveTeleopInputs.java | 112 +++ .../frc/robot/constants/FieldConstants.java | 25 + .../robot/subsystems/drive/SwerveDrive.java | 765 ++++-------------- .../subsystems/drive/SwerveDriveConfig.java | 24 +- .../subsystems/drive/SwerveDynamics.java | 63 ++ .../robot/subsystems/drive/SwerveModule.java | 379 ++++----- .../drive/SwerveModuleOptimization.java | 47 ++ src/main/java/frc/robot/util/AngleUtils.java | 30 + .../commands/drive/DriveCommandsTest.java | 61 ++ .../SwerveTeleopInputsSimulationTest.java | 66 ++ .../drive/SwerveTeleopInputsTest.java | 113 +++ .../robot/constants/FieldConstantsTest.java | 28 + src/test/java/frc/robot/hid/HIDUtilsTest.java | 47 ++ .../drive/SwerveDriveKinematicsTest.java | 66 ++ .../subsystems/drive/SwerveDynamicsTest.java | 52 ++ .../drive/SwerveModuleOptimizationTest.java | 65 ++ .../drive/SwerveModuleSimulationTest.java | 111 +++ .../WpilibSimulationExtension.java | 35 + .../java/frc/robot/util/AngleUtilsTest.java | 32 + 22 files changed, 1458 insertions(+), 820 deletions(-) create mode 100644 src/main/java/frc/robot/commands/drive/DriveCommands.java create mode 100644 src/main/java/frc/robot/commands/drive/SwerveTeleopInputs.java create mode 100644 src/main/java/frc/robot/constants/FieldConstants.java create mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveDynamics.java create mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveModuleOptimization.java create mode 100644 src/main/java/frc/robot/util/AngleUtils.java create mode 100644 src/test/java/frc/robot/commands/drive/DriveCommandsTest.java create mode 100644 src/test/java/frc/robot/commands/drive/SwerveTeleopInputsSimulationTest.java create mode 100644 src/test/java/frc/robot/commands/drive/SwerveTeleopInputsTest.java create mode 100644 src/test/java/frc/robot/constants/FieldConstantsTest.java create mode 100644 src/test/java/frc/robot/hid/HIDUtilsTest.java create mode 100644 src/test/java/frc/robot/subsystems/drive/SwerveDriveKinematicsTest.java create mode 100644 src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java create mode 100644 src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java create mode 100644 src/test/java/frc/robot/subsystems/drive/SwerveModuleSimulationTest.java create mode 100644 src/test/java/frc/robot/testsupport/WpilibSimulationExtension.java create mode 100644 src/test/java/frc/robot/util/AngleUtilsTest.java 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..839453a 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; @@ -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..e0ddff0 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -0,0 +1,122 @@ +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 edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +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, + CommandXboxController controller, + BooleanSupplier fieldRelative, + double joystickDeadband, + double joystickSensitivity) { + return swerve.runEnd( + () -> { + var translation = SwerveTeleopInputs.readTranslation(controller, joystickDeadband, joystickSensitivity); + double rotationSpeed = SwerveTeleopInputs.readRotation(controller, joystickDeadband, joystickSensitivity); + int sign = SwerveTeleopInputs.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, + CommandXboxController 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 = SwerveTeleopInputs.readTranslation(controller, joystickDeadband, joystickSensitivity); + int sign = SwerveTeleopInputs.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/commands/drive/SwerveTeleopInputs.java b/src/main/java/frc/robot/commands/drive/SwerveTeleopInputs.java new file mode 100644 index 0000000..f6190cd --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/SwerveTeleopInputs.java @@ -0,0 +1,112 @@ +package frc.robot.commands.drive; + +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 frc.robot.hid.HIDUtils; +import frc.robot.subsystems.drive.SwerveDrive; + +/** + * Converts Xbox controller axes into chassis speeds for {@link DriveCommands}. + * + *

Axis helpers ({@link #translationFromAxes}, {@link #rotationFromAxis}) are package-visible for + * unit tests without HAL. Alliance sign flips field-relative translation on red alliance. + */ +final class SwerveTeleopInputs { + private SwerveTeleopInputs() {} + + /** + * Reads left stick with deadband, cubic smoothing, and current max translational speed. + * + * @see SwerveDrive#maxTranslationalSpeedMetersPerSecond + */ + static TranslationalInput readTranslation( + CommandXboxController controller, + double joystickDeadband, + double joystickSensitivity) { + return translationFromAxes( + controller.getLeftY(), + controller.getLeftX(), + joystickDeadband, + joystickSensitivity, + SwerveDrive.maxTranslationalSpeedMetersPerSecond); + } + + /** Reads right stick X as angular rate using {@link SwerveDrive#maxAngularSpeedRadiansPerSecond}. */ + static double readRotation( + CommandXboxController controller, + double joystickDeadband, + double joystickSensitivity) { + return rotationFromAxis( + controller.getRightX(), + joystickDeadband, + joystickSensitivity, + SwerveDrive.maxAngularSpeedRadiansPerSecond); + } + + /** + * Pure function version of {@link #readTranslation} for tests and simulation. + * + * @param leftY raw left Y axis (−1 forward on Xbox) + * @param leftX raw left X axis + */ + static TranslationalInput translationFromAxes( + double leftY, + double leftX, + double joystickDeadband, + double joystickSensitivity, + double maxTranslationalSpeedMetersPerSecond) { + double xSpeed = 0.0; + double leftYInput = -leftY; + if (Math.abs(leftYInput) > joystickDeadband) { + xSpeed = HIDUtils.smoothInputWithCubic(leftYInput, joystickSensitivity) * maxTranslationalSpeedMetersPerSecond; + } + + double ySpeed = 0.0; + double leftXInput = -leftX; + if (Math.abs(leftXInput) > joystickDeadband) { + ySpeed = HIDUtils.smoothInputWithCubic(leftXInput, joystickSensitivity) * maxTranslationalSpeedMetersPerSecond; + } + + return new TranslationalInput(xSpeed, ySpeed); + } + + /** Pure function version of {@link #readRotation} for tests. */ + static double rotationFromAxis( + double rightX, + double joystickDeadband, + double joystickSensitivity, + double maxAngularSpeedRadiansPerSecond) { + double rotationSpeed = 0.0; + double rightXInput = -rightX; + if (Math.abs(rightXInput) > joystickDeadband) { + rotationSpeed = HIDUtils.smoothInputWithCubic(rightXInput, joystickSensitivity) * maxAngularSpeedRadiansPerSecond; + } + return rotationSpeed; + } + + /** Uses {@link DriverStation#getAlliance()} to flip translation when field-relative on red. */ + static int fieldRelativeAllianceSign(BooleanSupplier fieldRelative) { + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + return fieldRelativeAllianceSign(fieldRelative.getAsBoolean(), alliance); + } + + /** + * @return {@code -1} when field-relative and red alliance (so blue-oriented field coords feel correct), + * otherwise {@code 1} + */ + static int fieldRelativeAllianceSign(boolean fieldRelative, Alliance alliance) { + return fieldRelative && alliance == Alliance.Red ? -1 : 1; + } + + /** + * Robot-frame translational speeds after deadband and scaling. + * + * @param xMetersPerSecond forward (m/s) + * @param yMetersPerSecond left strafe (m/s) + */ + record TranslationalInput(double xMetersPerSecond, double yMetersPerSecond) {} +} 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/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index 9c28b84..ab2102b 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -7,12 +7,6 @@ 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 +19,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; @@ -35,127 +27,64 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.util.sendable.SendableBuilder; 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.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.RobotMap; +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(), @@ -164,454 +93,190 @@ public class SwerveDrive extends SubsystemBase { 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 PIDController thetaPid = new PIDController( + SwerveDriveConfig.DRIVE_ANGLE_P, + SwerveDriveConfig.DRIVE_ANGLE_I, + SwerveDriveConfig.DRIVE_ANGLE_D); - /** - * Constructs the Swerve Drive. - */ + /** Constructs modules, pose estimator, PathPlanner AutoBuilder, and initial field pose. */ public SwerveDrive() { - // meters - xPid.setTolerance(0.1); - yPid.setTolerance(0.1); - - // degrees thetaPid.enableContinuousInput(-180.0, 180.0); thetaPid.setTolerance(3); configurePathPlannerAutoBuilder(); - - resetPose(new Pose2d(8.5, 4.0, Rotation2d.fromDegrees(0))); + resetPose(FieldConstants.kDefaultOdometryPose); SmartDashboard.putData(this); } - /** {@inheritDoc} */ + 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; - } - - /** - * 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); - } - - /** - * 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(); } /** - * 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"); - } - - /** - * 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"); - } - - /** - * 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); - } - - var cmd = Commands.sequence(alignCmd, finalAdjustment); - - return cmd.withName("SwerveDriveAlignToTarget"); - } - - /** - * 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 + 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); + 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 +286,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 +296,43 @@ 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; + * 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 holdAngleWhileDriving( + double xSpeed, + double ySpeed, + Rotation2d setpointAngle, + double maxSpeed, + boolean fieldOriented) { + double rotateOutput = MathUtil.clamp( + thetaPid.calculate(getPose().getRotation().getDegrees(), AngleUtils.normalizeDegrees(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) - */ - 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); - } - - /** - * 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 - */ - public boolean atAngleSetpoint() { - return thetaPid.atSetpoint(); - } - - /** - * Resets the internal X, Y and Theta PID controllers. - */ - public void resetPID() { - xPid.reset(); - yPid.reset(); - thetaPid.reset(); - } - /** - * 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..237df42 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,74 @@ 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; + } + + /** + * 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 +226,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 - */ - public void setDesiredState(SwerveModuleState state) { - setDesiredState(state, 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 + * @param speedMetersPerSecond wheel linear speed (m/s) + * @param angleDegrees module azimuth in degrees [-180, 180] */ - public void setDesiredState(double speed, double angle, boolean useShortestPath) { - setDesiredState(new SwerveModuleState(speed, Rotation2d.fromDegrees(angle)), useShortestPath); + 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] - */ - public void setDesiredState(double speed, double angle) { - setDesiredState(speed, angle, true); + public void setDesiredState(double speedMetersPerSecond, double angleDegrees, boolean useShortestPath) { + setDesiredState(new SwerveModuleState(speedMetersPerSecond, Rotation2d.fromDegrees(angleDegrees)), useShortestPath); } /** - * 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/DriveCommandsTest.java b/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java new file mode 100644 index 0000000..a3b78e9 --- /dev/null +++ b/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java @@ -0,0 +1,61 @@ +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.subsystems.drive.SwerveDrive; +import frc.robot.testsupport.WpilibSimulationExtension; + +@ExtendWith(WpilibSimulationExtension.class) +class DriveCommandsTest { + private double savedTranslationalMax; + private double savedAngularMax; + + @BeforeEach + void setUp() { + savedTranslationalMax = SwerveDrive.maxTranslationalSpeedMetersPerSecond; + savedAngularMax = SwerveDrive.maxAngularSpeedRadiansPerSecond; + CommandScheduler.getInstance().cancelAll(); + } + + @AfterEach + void tearDown() { + CommandScheduler.getInstance().cancelAll(); + SwerveDrive.maxTranslationalSpeedMetersPerSecond = savedTranslationalMax; + SwerveDrive.maxAngularSpeedRadiansPerSecond = savedAngularMax; + } + + @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_hasExpectedCommandName() { + assertEquals("SwerveGoSlow", DriveCommands.goSlow(null).getName()); + } +} diff --git a/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsSimulationTest.java b/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsSimulationTest.java new file mode 100644 index 0000000..455b249 --- /dev/null +++ b/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsSimulationTest.java @@ -0,0 +1,66 @@ +package frc.robot.commands.drive; + +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 edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +import frc.robot.subsystems.drive.SwerveDrive; +import frc.robot.testsupport.WpilibSimulationExtension; + +/** + * Verifies teleop input path through WPILib {@link edu.wpi.first.wpilibj.simulation.XboxControllerSim} + * and a live {@link edu.wpi.first.wpilibj2.command.button.CommandXboxController}. + */ +@ExtendWith(WpilibSimulationExtension.class) +class SwerveTeleopInputsSimulationTest { + private static final double DELTA = 1e-2; + private static final int kControllerPort = 0; + + private CommandXboxController controller; + private XboxControllerSim controllerSim; + + @BeforeEach + void setUpController() { + // Arrange — shared per test (HAL initialized by WpilibSimulationExtension) + SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.0; + controller = new CommandXboxController(kControllerPort); + controllerSim = new XboxControllerSim(kControllerPort); + } + + @Test + void readTranslation_usesSimulatedJoystickAxes() { + // Arrange + controllerSim.setLeftY(-1.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + + // Act + SwerveTeleopInputs.TranslationalInput speeds = + SwerveTeleopInputs.readTranslation(controller, 0.05, 0.0); + + // Assert + assertEquals(4.0, speeds.xMetersPerSecond(), DELTA); + assertEquals(0.0, speeds.yMetersPerSecond(), DELTA); + } + + @Test + void readRotation_usesSimulatedRightStick() { + // Arrange + SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(360); + controllerSim.setRightX(-1.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + + // Act + double omega = SwerveTeleopInputs.readRotation(controller, 0.05, 0.0); + + // Assert + assertEquals(Math.toRadians(360), omega, DELTA); + } +} diff --git a/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsTest.java b/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsTest.java new file mode 100644 index 0000000..f75bee8 --- /dev/null +++ b/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsTest.java @@ -0,0 +1,113 @@ +package frc.robot.commands.drive; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +import frc.robot.hid.HIDUtils; + +class SwerveTeleopInputsTest { + @Test + void translationFromAxes_belowDeadband_isZero() { + // Arrange + double leftY = 0.01; + double leftX = 0.01; + double deadband = 0.05; + double sensitivity = 0.0; + double maxSpeedMetersPerSecond = 4.0; + + // Act + SwerveTeleopInputs.TranslationalInput speeds = SwerveTeleopInputs.translationFromAxes( + leftY, leftX, deadband, sensitivity, maxSpeedMetersPerSecond); + + // Assert + assertEquals(0.0, speeds.xMetersPerSecond(), 1e-9); + assertEquals(0.0, speeds.yMetersPerSecond(), 1e-9); + } + + @Test + void translationFromAxes_fullForward_usesInvertedLeftY() { + // Arrange + double leftY = -1.0; + double leftX = 0.0; + double deadband = 0.05; + double sensitivity = 0.0; + double maxSpeedMetersPerSecond = 4.0; + + // Act + SwerveTeleopInputs.TranslationalInput speeds = SwerveTeleopInputs.translationFromAxes( + leftY, leftX, deadband, sensitivity, maxSpeedMetersPerSecond); + + // Assert + assertEquals(4.0, speeds.xMetersPerSecond(), 1e-9); + assertEquals(0.0, speeds.yMetersPerSecond(), 1e-9); + } + + @Test + void fieldRelativeAllianceSign_redAllianceAndFieldRelative_flipsSign() { + // Arrange — field-relative on red alliance + + // Act + int signWhenFieldRelative = SwerveTeleopInputs.fieldRelativeAllianceSign(true, Alliance.Red); + int signWhenRobotRelative = SwerveTeleopInputs.fieldRelativeAllianceSign(false, Alliance.Red); + + // Assert + assertEquals(-1, signWhenFieldRelative); + assertEquals(1, signWhenRobotRelative); + } + + @Test + void translationFromAxes_fullStick_appliesCubicSmoothingToBothAxes() { + // Arrange + double maxTranslation = 4.6; + double deadband = 0.05; + double sensitivity = 0.5; + double expectedX = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxTranslation; + double expectedY = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxTranslation; + + // Act + SwerveTeleopInputs.TranslationalInput speeds = SwerveTeleopInputs.translationFromAxes( + -1.0, -1.0, deadband, 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 deadband = 0.05; + double sensitivity = 0.5; + double expectedOmega = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxAngular; + + // Act + double omega = SwerveTeleopInputs.rotationFromAxis(-1.0, deadband, sensitivity, maxAngular); + + // Assert + assertEquals(expectedOmega, omega, 1e-9); + } + + @Test + void rotationFromAxis_belowDeadband_isZero() { + // Act + double omega = SwerveTeleopInputs.rotationFromAxis(0.02, 0.05, 0.0, Math.toRadians(540)); + + // Assert + assertEquals(0.0, omega, 1e-9); + } + + @Test + void fieldRelativeAllianceSign_blueAlliance_doesNotFlip() { + // Arrange — field-relative on blue alliance + + // Act + int sign = SwerveTeleopInputs.fieldRelativeAllianceSign(true, Alliance.Blue); + + // Assert + assertEquals(1, sign); + } +} 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..a31eaab --- /dev/null +++ b/src/test/java/frc/robot/constants/FieldConstantsTest.java @@ -0,0 +1,28 @@ +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 + 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() { + // Act & Assert + assertEquals(1.016, FieldConstants.kDriverPracticeResetX, 1e-9); + assertEquals(4.0, FieldConstants.kDriverPracticeResetY, 1e-9); + } +} 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..22bd19e --- /dev/null +++ b/src/test/java/frc/robot/hid/HIDUtilsTest.java @@ -0,0 +1,47 @@ +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 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/SwerveDynamicsTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java new file mode 100644 index 0000000..441f21b --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java @@ -0,0 +1,52 @@ +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.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); + } +} 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..8215909 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java @@ -0,0 +1,65 @@ +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 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..07d1939 --- /dev/null +++ b/src/test/java/frc/robot/testsupport/WpilibSimulationExtension.java @@ -0,0 +1,35 @@ +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 { + /** {@inheritDoc} */ + @Override + public void beforeEach(ExtensionContext context) { + assertTrue(HAL.initialize(500, 0), "HAL.initialize failed"); + 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); + } +} From 1e394a74be61eee25d1ce0e50d1a6f235a3187a1 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 03:20:03 +0000 Subject: [PATCH 02/17] Add swerve motion, optimization boundary, and poseLog tests Co-authored-by: ernestgwilsoniii --- .../robot/subsystems/drive/SwerveDrive.java | 37 +++++++-- .../drive/SwerveDriveMotionTest.java | 78 +++++++++++++++++++ .../subsystems/drive/SwerveDynamicsTest.java | 41 ++++++++++ .../drive/SwerveModuleOptimizationTest.java | 31 ++++++++ 4 files changed, 179 insertions(+), 8 deletions(-) create mode 100644 src/test/java/frc/robot/subsystems/drive/SwerveDriveMotionTest.java diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index ab2102b..02dc31d 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -255,9 +255,7 @@ public void drive( double vyMetersPerSecond, double omegaRadiansPerSecond, boolean fieldRelative) { - if (Math.abs(omegaRadiansPerSecond) < kDriveOmegaDeadband - && Math.abs(vxMetersPerSecond) < kDriveVelocityDeadband - && Math.abs(vyMetersPerSecond) < kDriveVelocityDeadband) { + if (shouldCoastSwerveModules(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond)) { stopModulesButHoldPivotAngle(); return; } @@ -328,11 +326,34 @@ public void holdAngleWhileDriving( Rotation2d setpointAngle, double maxSpeed, boolean fieldOriented) { - double rotateOutput = MathUtil.clamp( - thetaPid.calculate(getPose().getRotation().getDegrees(), AngleUtils.normalizeDegrees(setpointAngle.getDegrees())), - -maxSpeed, - maxSpeed) * maxAngularSpeedRadiansPerSecond; - drive(xSpeed, ySpeed, rotateOutput, fieldOriented); + drive(xSpeed, ySpeed, computeHeadingHoldOmegaRadians(thetaPid, getPose().getRotation(), setpointAngle, maxSpeed, + maxAngularSpeedRadiansPerSecond), fieldOriented); + } + + /** + * True when teleop inputs are below {@link #kDriveVelocityDeadband} / {@link #kDriveOmegaDeadband} so modules + * coast with pivots held (package-visible for unit tests). + */ + static boolean shouldCoastSwerveModules(double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) { + return Math.abs(omegaRadiansPerSecond) < kDriveOmegaDeadband + && Math.abs(vxMetersPerSecond) < kDriveVelocityDeadband + && Math.abs(vyMetersPerSecond) < kDriveVelocityDeadband; + } + + /** + * Rotation rate from heading-hold PID (package-visible for unit tests). + */ + 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; } } 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..e9c2a30 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveMotionTest.java @@ -0,0 +1,78 @@ +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() { + assertTrue(SwerveDrive.shouldCoastSwerveModules(0.0, 0.0, 0.0)); + assertTrue(SwerveDrive.shouldCoastSwerveModules(0.004, -0.004, 0.004)); + } + + @Test + void shouldCoastSwerveModules_anyAxisAboveDeadband_isFalse() { + assertFalse(SwerveDrive.shouldCoastSwerveModules(0.006, 0.0, 0.0)); + assertFalse(SwerveDrive.shouldCoastSwerveModules(0.0, -0.006, 0.0)); + assertFalse(SwerveDrive.shouldCoastSwerveModules(0.0, 0.0, 0.006)); + } + + @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/SwerveDynamicsTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java index 441f21b..493cbdf 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java @@ -5,6 +5,9 @@ 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 { @@ -49,4 +52,42 @@ void correctForDynamics_curvedMotion_adjustsTranslation() { assertNotEquals(input.vyMetersPerSecond, output.vyMetersPerSecond, 1e-6); assertEquals(input.vxMetersPerSecond, output.vxMetersPerSecond, 1e-2); } + + @Test + void poseLog_zeroTransform_isZeroTwist() { + // Act + Twist2d twist = SwerveDynamics.poseLog(Pose2d.kZero); + + // 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 index 8215909..e7edbdd 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java @@ -50,6 +50,37 @@ void cosineScale_alignedModule_isOne() { 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() { + SwerveModuleOptimization.PivotTarget result = + SwerveModuleOptimization.shortestPivotTarget(0, 91); + + assertTrue(result.driveInverted()); + assertEquals(-89, result.angleDegrees(), 1e-9); + } + + @Test + void shortestPivotTarget_negativeAngles_usesShortestPath() { + SwerveModuleOptimization.PivotTarget result = + SwerveModuleOptimization.shortestPivotTarget(-10, -170); + + assertTrue(result.driveInverted()); + assertEquals(10, result.angleDegrees(), 1e-9); + } + @Test void cosineScale_perpendicularModule_isZero() { // Arrange From 1dc6459d353148321ad9a9ea8856b51a768e6224 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 04:42:15 +0000 Subject: [PATCH 03/17] Install PathPlanner and Elastic in cloud agent environment Co-authored-by: ernestgwilsoniii --- .cursor/cloud-install.sh | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) 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 From 861fef29fe92998b159cccbec79d84578f48f068 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 13:20:24 +0000 Subject: [PATCH 04/17] Refresh AGENTS.md for swerve layout and cloud sim tools Co-authored-by: ernestgwilsoniii --- AGENTS.md | 47 ++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 42 insertions(+), 5 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 434820c..d3238ee 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -8,18 +8,55 @@ 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 runs the same `build` task in the WPILib cross container. Use Java **17** (Gradle toolchain in `build.gradle`). + +## Cloud agent environment + +Config is **as code**: `.cursor/environment.json` runs `.cursor/cloud-install.sh`, which: + +- Installs OpenJDK 17 and runs `./gradlew classes` +- Installs **PathPlanner** (`pathplanner`, 2026.1.2) and **Elastic** WPILib (`elastic`, 2026.1.2) for the agent **Desktop** tab + +There is no long-running boot service; this is a Gradle robot project. After changing `cloud-install.sh`, merge to `main` and refresh the team environment **build**. + +**Desktop sim workflow (optional verification):** + +1. `./gradlew simulateJava` — enable the robot in Driver Station sim +2. `elastic` — connect to NT; import layout from `src/main/deploy/elastic/elastic-layout.json` +3. `pathplanner` — **Open Robot Project** at the repo root (`src/main/deploy/pathplanner/`) + +Vendor REV/Phoenix libraries come from Gradle `vendordeps` for compile/sim; no separate vendor desktop tools are installed in the cloud image. ## 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** layout: + +| Layer | Location | Responsibility | +| --- | --- | --- | +| Robot / container | `Robot.java`, `RobotContainer.java` | Scheduler, subsystem instances, bindings, PathPlanner named commands | +| Subsystems | `subsystems/**` | Hardware, `periodic()`, imperative APIs (`SwerveDrive.drive()`, setpoints, odometry) | +| Commands | `commands/**` | Command factories (`DriveCommands`, mechanism commands) — no CAN IDs here | +| Constants | `constants/`, `*Config`, `RobotMap` | Field poses (`FieldConstants`), tuning, CAN IDs | +| Deploy | `src/main/deploy/` | PathPlanner paths/autos, Elastic layout | + +**Swerve (current structure):** + +- `subsystems/drive/` — `SwerveDrive`, modules, `SwerveDynamics`, `SwerveModuleOptimization`, `SwerveDriveConfig` +- `commands/drive/` — `DriveCommands`, `SwerveTeleopInputs` (teleop, go-slow, heading hold, reset pose, stop) +- PathPlanner `AutoBuilder` stays on `SwerveDrive`; do not break `getPose`, `resetPose`, `getChassisSpeeds`, or the module-state consumer without updating PathPlanner. + +## Tests + +- Run `./gradlew test` after drive or command changes. +- Pure logic: `SwerveDynamics`, `SwerveModuleOptimization`, `SwerveTeleopInputs`, `AngleUtils` (see `.cursor/rules/frc-unit-tests.mdc`). +- Do not construct full `SwerveDrive` in unit tests (`RobotContainer` static init); use package-visible helpers or module-level sim tests. ## Cursor rules -See `.cursor/rules/*.mdc` for FRC-specific conventions. +See `.cursor/rules/*.mdc` for FRC-specific conventions (subsystems vs commands, PathPlanner naming, unit tests). ## PR expectations -- Keep robot behavior changes scoped and describe driver/auto impact in the PR. +- Keep robot behavior changes scoped; describe **driver** and **auto** impact in the PR. +- PathPlanner named command strings in Java must **exactly** match the PathPlanner GUI. - Ensure CI (`build`) passes before merge. From 9ac5f99efa4bd555a0407f4a57f4b28ff4dc90a1 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 13:22:38 +0000 Subject: [PATCH 05/17] Simplify AGENTS.md to structure-focused overview Co-authored-by: ernestgwilsoniii --- AGENTS.md | 53 +++++++++++++++-------------------------------------- 1 file changed, 15 insertions(+), 38 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index d3238ee..ff0d0d6 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -8,55 +8,32 @@ chmod +x gradlew ./gradlew build # full compile + tests (matches GitHub Actions CI) ``` -CI runs the same `build` task in the WPILib cross container. Use Java **17** (Gradle toolchain in `build.gradle`). +CI uses the same `build` task (WPILib cross container, Java 17 per `build.gradle`). ## Cloud agent environment -Config is **as code**: `.cursor/environment.json` runs `.cursor/cloud-install.sh`, which: +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**. -- Installs OpenJDK 17 and runs `./gradlew classes` -- Installs **PathPlanner** (`pathplanner`, 2026.1.2) and **Elastic** WPILib (`elastic`, 2026.1.2) for the agent **Desktop** tab - -There is no long-running boot service; this is a Gradle robot project. After changing `cloud-install.sh`, merge to `main` and refresh the team environment **build**. - -**Desktop sim workflow (optional verification):** - -1. `./gradlew simulateJava` — enable the robot in Driver Station sim -2. `elastic` — connect to NT; import layout from `src/main/deploy/elastic/elastic-layout.json` -3. `pathplanner` — **Open Robot Project** at the repo root (`src/main/deploy/pathplanner/`) - -Vendor REV/Phoenix libraries come from Gradle `vendordeps` for compile/sim; no separate vendor desktop tools are installed in the cloud image. +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** layout: - -| Layer | Location | Responsibility | -| --- | --- | --- | -| Robot / container | `Robot.java`, `RobotContainer.java` | Scheduler, subsystem instances, bindings, PathPlanner named commands | -| Subsystems | `subsystems/**` | Hardware, `periodic()`, imperative APIs (`SwerveDrive.drive()`, setpoints, odometry) | -| Commands | `commands/**` | Command factories (`DriveCommands`, mechanism commands) — no CAN IDs here | -| Constants | `constants/`, `*Config`, `RobotMap` | Field poses (`FieldConstants`), tuning, CAN IDs | -| Deploy | `src/main/deploy/` | PathPlanner paths/autos, Elastic layout | - -**Swerve (current structure):** - -- `subsystems/drive/` — `SwerveDrive`, modules, `SwerveDynamics`, `SwerveModuleOptimization`, `SwerveDriveConfig` -- `commands/drive/` — `DriveCommands`, `SwerveTeleopInputs` (teleop, go-slow, heading hold, reset pose, stop) -- PathPlanner `AutoBuilder` stays on `SwerveDrive`; do not break `getPose`, `resetPose`, `getChassisSpeeds`, or the module-state consumer without updating PathPlanner. +WPILib **command-based** robot: -## Tests +- **`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 teleop and utilities under `commands/drive/`). +- **`constants/`**, **`*Config`**, **`RobotMap`** — field/season poses, tuning, CAN IDs. +- **`src/main/deploy/`** — PathPlanner paths/autos, Elastic layout, other deploy files. -- Run `./gradlew test` after drive or command changes. -- Pure logic: `SwerveDynamics`, `SwerveModuleOptimization`, `SwerveTeleopInputs`, `AngleUtils` (see `.cursor/rules/frc-unit-tests.mdc`). -- Do not construct full `SwerveDrive` in unit tests (`RobotContainer` static init); use package-visible helpers or module-level sim tests. +**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 (subsystems vs commands, PathPlanner naming, unit tests). +Run `./gradlew test` (or `build`) when changing behavior. Testing patterns and scope live in `.cursor/rules/frc-unit-tests.mdc`. Other FRC conventions: `.cursor/rules/*.mdc`. ## PR expectations -- Keep robot behavior changes scoped; describe **driver** and **auto** impact in the PR. -- PathPlanner named command strings in Java must **exactly** match the PathPlanner GUI. -- 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. From 96c76e96a4f1b339533b2c94f838e024ede12b76 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 15:14:59 +0000 Subject: [PATCH 06/17] Soften Cursor test and drive rules to be less class-specific Co-authored-by: ernestgwilsoniii --- .cursor/rules/frc-commands-subsystems.mdc | 2 +- .cursor/rules/frc-unit-tests.mdc | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.cursor/rules/frc-commands-subsystems.mdc b/.cursor/rules/frc-commands-subsystems.mdc index f17024f..baac99c 100644 --- a/.cursor/rules/frc-commands-subsystems.mdc +++ b/.cursor/rules/frc-commands-subsystems.mdc @@ -10,7 +10,7 @@ alwaysApply: false - **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. - **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 diff --git a/.cursor/rules/frc-unit-tests.mdc b/.cursor/rules/frc-unit-tests.mdc index 9c76fb5..43d73b2 100644 --- a/.cursor/rules/frc-unit-tests.mdc +++ b/.cursor/rules/frc-unit-tests.mdc @@ -7,7 +7,7 @@ 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. +- **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 a full drivetrain subsystem graph in unit tests unless you add a dedicated fixture — extract testable logic instead (see `testsupport/` for shared sim setup). - After changes, run `./gradlew test` (Java 17 toolchain from `build.gradle`). From dda124a9e5020dfd8805a6506c5aaab4ff957137 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 21:07:56 +0000 Subject: [PATCH 07/17] Move swerve teleop axis mapping into hid package (DRY with HIDUtils) Co-authored-by: ernestgwilsoniii --- .cursor/rules/frc-commands-subsystems.mdc | 9 +- AGENTS.md | 3 +- .../robot/commands/drive/DriveCommands.java | 26 ++-- .../commands/drive/SwerveTeleopInputs.java | 112 ----------------- .../robot/hid/DemonCommandXboxController.java | 68 +++++++++++ src/main/java/frc/robot/hid/HIDUtils.java | 16 +++ .../SwerveTeleopInputsSimulationTest.java | 66 ---------- .../drive/SwerveTeleopInputsTest.java | 113 ------------------ ...onCommandXboxControllerSimulationTest.java | 50 ++++++++ .../hid/DemonCommandXboxControllerTest.java | 70 +++++++++++ src/test/java/frc/robot/hid/HIDUtilsTest.java | 10 ++ 11 files changed, 239 insertions(+), 304 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/drive/SwerveTeleopInputs.java delete mode 100644 src/test/java/frc/robot/commands/drive/SwerveTeleopInputsSimulationTest.java delete mode 100644 src/test/java/frc/robot/commands/drive/SwerveTeleopInputsTest.java create mode 100644 src/test/java/frc/robot/hid/DemonCommandXboxControllerSimulationTest.java create mode 100644 src/test/java/frc/robot/hid/DemonCommandXboxControllerTest.java diff --git a/.cursor/rules/frc-commands-subsystems.mdc b/.cursor/rules/frc-commands-subsystems.mdc index baac99c..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 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/AGENTS.md b/AGENTS.md index ff0d0d6..ffe0af3 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -22,7 +22,8 @@ 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 teleop and utilities under `commands/drive/`). +- **`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. diff --git a/src/main/java/frc/robot/commands/drive/DriveCommands.java b/src/main/java/frc/robot/commands/drive/DriveCommands.java index e0ddff0..bcb2f8a 100644 --- a/src/main/java/frc/robot/commands/drive/DriveCommands.java +++ b/src/main/java/frc/robot/commands/drive/DriveCommands.java @@ -7,8 +7,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; - +import frc.robot.hid.DemonCommandXboxController; import frc.robot.subsystems.drive.SwerveDrive; /** @@ -37,15 +36,21 @@ private DriveCommands() {} */ public static Command driveWithXboxController( SwerveDrive swerve, - CommandXboxController controller, + DemonCommandXboxController controller, BooleanSupplier fieldRelative, double joystickDeadband, double joystickSensitivity) { return swerve.runEnd( () -> { - var translation = SwerveTeleopInputs.readTranslation(controller, joystickDeadband, joystickSensitivity); - double rotationSpeed = SwerveTeleopInputs.readRotation(controller, joystickDeadband, joystickSensitivity); - int sign = SwerveTeleopInputs.fieldRelativeAllianceSign(fieldRelative); + 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()); }, @@ -58,7 +63,7 @@ public static Command driveWithXboxController( */ public static Command holdCurrentHeadingWhileDriving( SwerveDrive swerve, - CommandXboxController controller, + DemonCommandXboxController controller, BooleanSupplier fieldRelative, double joystickDeadband, double joystickSensitivity) { @@ -66,8 +71,11 @@ public static Command holdCurrentHeadingWhileDriving( Command setHeading = swerve.runOnce(() -> headingToHold[0] = swerve.getPose().getRotation()); Command holdHeading = swerve.runEnd( () -> { - var translation = SwerveTeleopInputs.readTranslation(controller, joystickDeadband, joystickSensitivity); - int sign = SwerveTeleopInputs.fieldRelativeAllianceSign(fieldRelative); + var translation = controller.getDriveTranslation( + joystickDeadband, + joystickSensitivity, + SwerveDrive.maxTranslationalSpeedMetersPerSecond); + int sign = DemonCommandXboxController.fieldRelativeAllianceSign(fieldRelative); swerve.holdAngleWhileDriving( sign * translation.xMetersPerSecond(), sign * translation.yMetersPerSecond(), diff --git a/src/main/java/frc/robot/commands/drive/SwerveTeleopInputs.java b/src/main/java/frc/robot/commands/drive/SwerveTeleopInputs.java deleted file mode 100644 index f6190cd..0000000 --- a/src/main/java/frc/robot/commands/drive/SwerveTeleopInputs.java +++ /dev/null @@ -1,112 +0,0 @@ -package frc.robot.commands.drive; - -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 frc.robot.hid.HIDUtils; -import frc.robot.subsystems.drive.SwerveDrive; - -/** - * Converts Xbox controller axes into chassis speeds for {@link DriveCommands}. - * - *

Axis helpers ({@link #translationFromAxes}, {@link #rotationFromAxis}) are package-visible for - * unit tests without HAL. Alliance sign flips field-relative translation on red alliance. - */ -final class SwerveTeleopInputs { - private SwerveTeleopInputs() {} - - /** - * Reads left stick with deadband, cubic smoothing, and current max translational speed. - * - * @see SwerveDrive#maxTranslationalSpeedMetersPerSecond - */ - static TranslationalInput readTranslation( - CommandXboxController controller, - double joystickDeadband, - double joystickSensitivity) { - return translationFromAxes( - controller.getLeftY(), - controller.getLeftX(), - joystickDeadband, - joystickSensitivity, - SwerveDrive.maxTranslationalSpeedMetersPerSecond); - } - - /** Reads right stick X as angular rate using {@link SwerveDrive#maxAngularSpeedRadiansPerSecond}. */ - static double readRotation( - CommandXboxController controller, - double joystickDeadband, - double joystickSensitivity) { - return rotationFromAxis( - controller.getRightX(), - joystickDeadband, - joystickSensitivity, - SwerveDrive.maxAngularSpeedRadiansPerSecond); - } - - /** - * Pure function version of {@link #readTranslation} for tests and simulation. - * - * @param leftY raw left Y axis (−1 forward on Xbox) - * @param leftX raw left X axis - */ - static TranslationalInput translationFromAxes( - double leftY, - double leftX, - double joystickDeadband, - double joystickSensitivity, - double maxTranslationalSpeedMetersPerSecond) { - double xSpeed = 0.0; - double leftYInput = -leftY; - if (Math.abs(leftYInput) > joystickDeadband) { - xSpeed = HIDUtils.smoothInputWithCubic(leftYInput, joystickSensitivity) * maxTranslationalSpeedMetersPerSecond; - } - - double ySpeed = 0.0; - double leftXInput = -leftX; - if (Math.abs(leftXInput) > joystickDeadband) { - ySpeed = HIDUtils.smoothInputWithCubic(leftXInput, joystickSensitivity) * maxTranslationalSpeedMetersPerSecond; - } - - return new TranslationalInput(xSpeed, ySpeed); - } - - /** Pure function version of {@link #readRotation} for tests. */ - static double rotationFromAxis( - double rightX, - double joystickDeadband, - double joystickSensitivity, - double maxAngularSpeedRadiansPerSecond) { - double rotationSpeed = 0.0; - double rightXInput = -rightX; - if (Math.abs(rightXInput) > joystickDeadband) { - rotationSpeed = HIDUtils.smoothInputWithCubic(rightXInput, joystickSensitivity) * maxAngularSpeedRadiansPerSecond; - } - return rotationSpeed; - } - - /** Uses {@link DriverStation#getAlliance()} to flip translation when field-relative on red. */ - static int fieldRelativeAllianceSign(BooleanSupplier fieldRelative) { - Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); - return fieldRelativeAllianceSign(fieldRelative.getAsBoolean(), alliance); - } - - /** - * @return {@code -1} when field-relative and red alliance (so blue-oriented field coords feel correct), - * otherwise {@code 1} - */ - static int fieldRelativeAllianceSign(boolean fieldRelative, Alliance alliance) { - return fieldRelative && alliance == Alliance.Red ? -1 : 1; - } - - /** - * Robot-frame translational speeds after deadband and scaling. - * - * @param xMetersPerSecond forward (m/s) - * @param yMetersPerSecond left strafe (m/s) - */ - record TranslationalInput(double xMetersPerSecond, double yMetersPerSecond) {} -} 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/test/java/frc/robot/commands/drive/SwerveTeleopInputsSimulationTest.java b/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsSimulationTest.java deleted file mode 100644 index 455b249..0000000 --- a/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsSimulationTest.java +++ /dev/null @@ -1,66 +0,0 @@ -package frc.robot.commands.drive; - -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 edu.wpi.first.wpilibj2.command.button.CommandXboxController; - -import frc.robot.subsystems.drive.SwerveDrive; -import frc.robot.testsupport.WpilibSimulationExtension; - -/** - * Verifies teleop input path through WPILib {@link edu.wpi.first.wpilibj.simulation.XboxControllerSim} - * and a live {@link edu.wpi.first.wpilibj2.command.button.CommandXboxController}. - */ -@ExtendWith(WpilibSimulationExtension.class) -class SwerveTeleopInputsSimulationTest { - private static final double DELTA = 1e-2; - private static final int kControllerPort = 0; - - private CommandXboxController controller; - private XboxControllerSim controllerSim; - - @BeforeEach - void setUpController() { - // Arrange — shared per test (HAL initialized by WpilibSimulationExtension) - SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.0; - controller = new CommandXboxController(kControllerPort); - controllerSim = new XboxControllerSim(kControllerPort); - } - - @Test - void readTranslation_usesSimulatedJoystickAxes() { - // Arrange - controllerSim.setLeftY(-1.0); - controllerSim.notifyNewData(); - SimHooks.stepTiming(0.02); - - // Act - SwerveTeleopInputs.TranslationalInput speeds = - SwerveTeleopInputs.readTranslation(controller, 0.05, 0.0); - - // Assert - assertEquals(4.0, speeds.xMetersPerSecond(), DELTA); - assertEquals(0.0, speeds.yMetersPerSecond(), DELTA); - } - - @Test - void readRotation_usesSimulatedRightStick() { - // Arrange - SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(360); - controllerSim.setRightX(-1.0); - controllerSim.notifyNewData(); - SimHooks.stepTiming(0.02); - - // Act - double omega = SwerveTeleopInputs.readRotation(controller, 0.05, 0.0); - - // Assert - assertEquals(Math.toRadians(360), omega, DELTA); - } -} diff --git a/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsTest.java b/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsTest.java deleted file mode 100644 index f75bee8..0000000 --- a/src/test/java/frc/robot/commands/drive/SwerveTeleopInputsTest.java +++ /dev/null @@ -1,113 +0,0 @@ -package frc.robot.commands.drive; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import org.junit.jupiter.api.Test; - -import edu.wpi.first.wpilibj.DriverStation.Alliance; - -import frc.robot.hid.HIDUtils; - -class SwerveTeleopInputsTest { - @Test - void translationFromAxes_belowDeadband_isZero() { - // Arrange - double leftY = 0.01; - double leftX = 0.01; - double deadband = 0.05; - double sensitivity = 0.0; - double maxSpeedMetersPerSecond = 4.0; - - // Act - SwerveTeleopInputs.TranslationalInput speeds = SwerveTeleopInputs.translationFromAxes( - leftY, leftX, deadband, sensitivity, maxSpeedMetersPerSecond); - - // Assert - assertEquals(0.0, speeds.xMetersPerSecond(), 1e-9); - assertEquals(0.0, speeds.yMetersPerSecond(), 1e-9); - } - - @Test - void translationFromAxes_fullForward_usesInvertedLeftY() { - // Arrange - double leftY = -1.0; - double leftX = 0.0; - double deadband = 0.05; - double sensitivity = 0.0; - double maxSpeedMetersPerSecond = 4.0; - - // Act - SwerveTeleopInputs.TranslationalInput speeds = SwerveTeleopInputs.translationFromAxes( - leftY, leftX, deadband, sensitivity, maxSpeedMetersPerSecond); - - // Assert - assertEquals(4.0, speeds.xMetersPerSecond(), 1e-9); - assertEquals(0.0, speeds.yMetersPerSecond(), 1e-9); - } - - @Test - void fieldRelativeAllianceSign_redAllianceAndFieldRelative_flipsSign() { - // Arrange — field-relative on red alliance - - // Act - int signWhenFieldRelative = SwerveTeleopInputs.fieldRelativeAllianceSign(true, Alliance.Red); - int signWhenRobotRelative = SwerveTeleopInputs.fieldRelativeAllianceSign(false, Alliance.Red); - - // Assert - assertEquals(-1, signWhenFieldRelative); - assertEquals(1, signWhenRobotRelative); - } - - @Test - void translationFromAxes_fullStick_appliesCubicSmoothingToBothAxes() { - // Arrange - double maxTranslation = 4.6; - double deadband = 0.05; - double sensitivity = 0.5; - double expectedX = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxTranslation; - double expectedY = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxTranslation; - - // Act - SwerveTeleopInputs.TranslationalInput speeds = SwerveTeleopInputs.translationFromAxes( - -1.0, -1.0, deadband, 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 deadband = 0.05; - double sensitivity = 0.5; - double expectedOmega = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxAngular; - - // Act - double omega = SwerveTeleopInputs.rotationFromAxis(-1.0, deadband, sensitivity, maxAngular); - - // Assert - assertEquals(expectedOmega, omega, 1e-9); - } - - @Test - void rotationFromAxis_belowDeadband_isZero() { - // Act - double omega = SwerveTeleopInputs.rotationFromAxis(0.02, 0.05, 0.0, Math.toRadians(540)); - - // Assert - assertEquals(0.0, omega, 1e-9); - } - - @Test - void fieldRelativeAllianceSign_blueAlliance_doesNotFlip() { - // Arrange — field-relative on blue alliance - - // Act - int sign = SwerveTeleopInputs.fieldRelativeAllianceSign(true, Alliance.Blue); - - // Assert - assertEquals(1, sign); - } -} 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..1e86999 --- /dev/null +++ b/src/test/java/frc/robot/hid/DemonCommandXboxControllerSimulationTest.java @@ -0,0 +1,50 @@ +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() { + controllerSim.setLeftY(-1.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + + DemonCommandXboxController.DriveTranslation speeds = controller.getDriveTranslation(0.05, 0.0, 4.0); + + assertEquals(4.0, speeds.xMetersPerSecond(), DELTA); + assertEquals(0.0, speeds.yMetersPerSecond(), DELTA); + } + + @Test + void getDriveRotation_usesSimulatedRightStick() { + controllerSim.setRightX(-1.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + + double omega = controller.getDriveRotation(0.05, 0.0, Math.toRadians(360)); + + 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..20c6399 --- /dev/null +++ b/src/test/java/frc/robot/hid/DemonCommandXboxControllerTest.java @@ -0,0 +1,70 @@ +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() { + DemonCommandXboxController.DriveTranslation speeds = DemonCommandXboxController.driveTranslationFromAxes( + 0.01, 0.01, 0.05, 0.0, 4.0); + + assertEquals(0.0, speeds.xMetersPerSecond(), 1e-9); + assertEquals(0.0, speeds.yMetersPerSecond(), 1e-9); + } + + @Test + void driveTranslationFromAxes_fullForward_usesInvertedLeftY() { + DemonCommandXboxController.DriveTranslation speeds = DemonCommandXboxController.driveTranslationFromAxes( + -1.0, 0.0, 0.05, 0.0, 4.0); + + assertEquals(4.0, speeds.xMetersPerSecond(), 1e-9); + assertEquals(0.0, speeds.yMetersPerSecond(), 1e-9); + } + + @Test + void fieldRelativeAllianceSign_redAllianceAndFieldRelative_flipsSign() { + assertEquals(-1, DemonCommandXboxController.fieldRelativeAllianceSign(true, Alliance.Red)); + assertEquals(1, DemonCommandXboxController.fieldRelativeAllianceSign(false, Alliance.Red)); + } + + @Test + void driveTranslationFromAxes_fullStick_appliesCubicSmoothingToBothAxes() { + double maxTranslation = 4.6; + double sensitivity = 0.5; + double expectedX = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxTranslation; + double expectedY = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxTranslation; + + DemonCommandXboxController.DriveTranslation speeds = DemonCommandXboxController.driveTranslationFromAxes( + -1.0, -1.0, 0.05, sensitivity, maxTranslation); + + assertEquals(expectedX, speeds.xMetersPerSecond(), 1e-9); + assertEquals(expectedY, speeds.yMetersPerSecond(), 1e-9); + } + + @Test + void rotationFromAxis_fullStick_appliesCubicSmoothing() { + double maxAngular = Math.toRadians(540); + double sensitivity = 0.5; + double expectedOmega = HIDUtils.smoothInputWithCubic(1.0, sensitivity) * maxAngular; + + double omega = DemonCommandXboxController.rotationFromAxis(-1.0, 0.05, sensitivity, maxAngular); + + assertEquals(expectedOmega, omega, 1e-9); + } + + @Test + void rotationFromAxis_belowDeadband_isZero() { + double omega = DemonCommandXboxController.rotationFromAxis(0.02, 0.05, 0.0, Math.toRadians(540)); + + assertEquals(0.0, omega, 1e-9); + } + + @Test + void fieldRelativeAllianceSign_blueAlliance_doesNotFlip() { + assertEquals(1, DemonCommandXboxController.fieldRelativeAllianceSign(true, Alliance.Blue)); + } +} diff --git a/src/test/java/frc/robot/hid/HIDUtilsTest.java b/src/test/java/frc/robot/hid/HIDUtilsTest.java index 22bd19e..ea93abb 100644 --- a/src/test/java/frc/robot/hid/HIDUtilsTest.java +++ b/src/test/java/frc/robot/hid/HIDUtilsTest.java @@ -32,6 +32,16 @@ void smoothInputWithCubic_cubicWhenSensitivityOne() { assertEquals(0.125, output, 1e-9); } + @Test + void scaledStickAxis_belowDeadband_isZero() { + assertEquals(0.0, HIDUtils.scaledStickAxis(0.02, 0.05, 0.0, 4.0), 1e-9); + } + + @Test + void scaledStickAxis_fullDeflection_scalesToMax() { + assertEquals(4.0, HIDUtils.scaledStickAxis(1.0, 0.05, 0.0, 4.0), 1e-9); + } + @Test void smoothInputWithCubic_zeroInput_isZero() { // Arrange From bfb41d1edffe3fd748abfeafa4bb835b2d642224 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 21:11:34 +0000 Subject: [PATCH 08/17] Document and apply Arrange-Act-Assert in unit tests Expand frc-unit-tests.mdc with AAA conventions; mention in AGENTS.md. Add section comments across HID, drive, and constants tests. Co-authored-by: ernestgwilsoniii --- .cursor/rules/frc-unit-tests.mdc | 17 ++++++- AGENTS.md | 2 +- .../commands/drive/DriveCommandsTest.java | 6 ++- .../robot/constants/FieldConstantsTest.java | 12 +++-- ...onCommandXboxControllerSimulationTest.java | 6 +++ .../hid/DemonCommandXboxControllerTest.java | 50 ++++++++++++++++--- src/test/java/frc/robot/hid/HIDUtilsTest.java | 22 +++++++- .../drive/SwerveDriveMotionTest.java | 21 ++++++-- .../subsystems/drive/SwerveDynamicsTest.java | 5 +- .../drive/SwerveModuleOptimizationTest.java | 16 +++++- 10 files changed, 134 insertions(+), 23 deletions(-) diff --git a/.cursor/rules/frc-unit-tests.mdc b/.cursor/rules/frc-unit-tests.mdc index 43d73b2..680c76a 100644 --- a/.cursor/rules/frc-unit-tests.mdc +++ b/.cursor/rules/frc-unit-tests.mdc @@ -6,7 +6,22 @@ alwaysApply: false # Unit tests -- **Runner:** JUnit 5 (`org.junit.jupiter`). Use **Arrange / Act / Assert** comments in non-trivial tests. +- **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 a full drivetrain subsystem graph in unit tests unless you add a dedicated fixture — extract testable logic instead (see `testsupport/` for shared sim setup). diff --git a/AGENTS.md b/AGENTS.md index ffe0af3..539b56b 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -31,7 +31,7 @@ WPILib **command-based** robot: ## Tests and conventions -Run `./gradlew test` (or `build`) when changing behavior. Testing patterns and scope live in `.cursor/rules/frc-unit-tests.mdc`. Other FRC conventions: `.cursor/rules/*.mdc`. +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 diff --git a/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java b/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java index a3b78e9..43744c7 100644 --- a/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java +++ b/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java @@ -56,6 +56,10 @@ void goSlow_whileScheduled_lowersMaxSpeeds_restoresOnEnd() { @Test void goSlow_hasExpectedCommandName() { - assertEquals("SwerveGoSlow", DriveCommands.goSlow(null).getName()); + // 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 index a31eaab..1dc901e 100644 --- a/src/test/java/frc/robot/constants/FieldConstantsTest.java +++ b/src/test/java/frc/robot/constants/FieldConstantsTest.java @@ -9,7 +9,7 @@ class FieldConstantsTest { @Test void defaultOdometryPose_isExpectedStartPose() { - // Arrange + // Arrange & Act var pose = FieldConstants.kDefaultOdometryPose; // Assert @@ -21,8 +21,12 @@ void defaultOdometryPose_isExpectedStartPose() { @Test void driverPracticeReset_hasExpectedFieldCoordinates() { - // Act & Assert - assertEquals(1.016, FieldConstants.kDriverPracticeResetX, 1e-9); - assertEquals(4.0, FieldConstants.kDriverPracticeResetY, 1e-9); + // 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 index 1e86999..7ad1899 100644 --- a/src/test/java/frc/robot/hid/DemonCommandXboxControllerSimulationTest.java +++ b/src/test/java/frc/robot/hid/DemonCommandXboxControllerSimulationTest.java @@ -27,24 +27,30 @@ void setUpController() { @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 index 20c6399..6a96e9c 100644 --- a/src/test/java/frc/robot/hid/DemonCommandXboxControllerTest.java +++ b/src/test/java/frc/robot/hid/DemonCommandXboxControllerTest.java @@ -9,62 +9,100 @@ 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( - 0.01, 0.01, 0.05, 0.0, 4.0); + 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( - -1.0, 0.0, 0.05, 0.0, 4.0); + 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() { - assertEquals(-1, DemonCommandXboxController.fieldRelativeAllianceSign(true, Alliance.Red)); - assertEquals(1, DemonCommandXboxController.fieldRelativeAllianceSign(false, Alliance.Red)); + // 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() { - double omega = DemonCommandXboxController.rotationFromAxis(0.02, 0.05, 0.0, Math.toRadians(540)); + // 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() { - assertEquals(1, DemonCommandXboxController.fieldRelativeAllianceSign(true, Alliance.Blue)); + // 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 index ea93abb..d5177d2 100644 --- a/src/test/java/frc/robot/hid/HIDUtilsTest.java +++ b/src/test/java/frc/robot/hid/HIDUtilsTest.java @@ -34,12 +34,30 @@ void smoothInputWithCubic_cubicWhenSensitivityOne() { @Test void scaledStickAxis_belowDeadband_isZero() { - assertEquals(0.0, HIDUtils.scaledStickAxis(0.02, 0.05, 0.0, 4.0), 1e-9); + // 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() { - assertEquals(4.0, HIDUtils.scaledStickAxis(1.0, 0.05, 0.0, 4.0), 1e-9); + // 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 diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveMotionTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveMotionTest.java index e9c2a30..eb1f119 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveDriveMotionTest.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveMotionTest.java @@ -24,15 +24,26 @@ void setUpPid() { @Test void shouldCoastSwerveModules_allBelowDeadband_isTrue() { - assertTrue(SwerveDrive.shouldCoastSwerveModules(0.0, 0.0, 0.0)); - assertTrue(SwerveDrive.shouldCoastSwerveModules(0.004, -0.004, 0.004)); + // 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() { - assertFalse(SwerveDrive.shouldCoastSwerveModules(0.006, 0.0, 0.0)); - assertFalse(SwerveDrive.shouldCoastSwerveModules(0.0, -0.006, 0.0)); - assertFalse(SwerveDrive.shouldCoastSwerveModules(0.0, 0.0, 0.006)); + // 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 diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java index 493cbdf..62bd894 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDynamicsTest.java @@ -55,8 +55,11 @@ void correctForDynamics_curvedMotion_adjustsTranslation() { @Test void poseLog_zeroTransform_isZeroTwist() { + // Arrange + Pose2d delta = Pose2d.kZero; + // Act - Twist2d twist = SwerveDynamics.poseLog(Pose2d.kZero); + Twist2d twist = SwerveDynamics.poseLog(delta); // Assert assertEquals(0.0, twist.dx, 1e-9); diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java index e7edbdd..bd28892 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveModuleOptimizationTest.java @@ -65,18 +65,30 @@ void shortestPivotTarget_deltaExactly90_doesNotFlipDrive() { @Test void shortestPivotTarget_deltaJustAbove90_flipsDrive() { + // Arrange + double currentAngleDegrees = 0; + double targetAngleDegrees = 91; + + // Act SwerveModuleOptimization.PivotTarget result = - SwerveModuleOptimization.shortestPivotTarget(0, 91); + 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(-10, -170); + SwerveModuleOptimization.shortestPivotTarget(currentAngleDegrees, targetAngleDegrees); + // Assert assertTrue(result.driveInverted()); assertEquals(10, result.angleDegrees(), 1e-9); } From 13da4698001daa1c514950d5d8d099d01d4b8f88 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 21:30:51 +0000 Subject: [PATCH 09/17] Inject gyro heading into SwerveDrive and add sim tests Break RobotContainer static init cycle with Supplier. Skip PathPlanner in tests; add SwerveDriveTest and sim test helpers. Co-authored-by: ernestgwilsoniii --- .cursor/rules/frc-unit-tests.mdc | 3 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/subsystems/drive/SwerveDrive.java | 95 +++++++++++++++--- .../robot/subsystems/drive/SwerveModule.java | 5 + .../subsystems/drive/SwerveDriveTest.java | 98 +++++++++++++++++++ 5 files changed, 188 insertions(+), 15 deletions(-) create mode 100644 src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java diff --git a/.cursor/rules/frc-unit-tests.mdc b/.cursor/rules/frc-unit-tests.mdc index 680c76a..da7b7c9 100644 --- a/.cursor/rules/frc-unit-tests.mdc +++ b/.cursor/rules/frc-unit-tests.mdc @@ -24,5 +24,6 @@ Rules of thumb: - 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 a full drivetrain subsystem graph in unit tests unless you add a dedicated fixture — extract testable logic instead (see `testsupport/` for shared sim setup). +- **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. +- 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/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 839453a..398eb72 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -89,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(); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index 02dc31d..bad31f5 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -7,6 +7,8 @@ package frc.robot.subsystems.drive; +import java.util.function.Supplier; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; @@ -26,12 +28,13 @@ 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.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.simulation.SimHooks; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotContainer; import frc.robot.RobotMap; import frc.robot.constants.FieldConstants; import frc.robot.util.AngleUtils; @@ -85,30 +88,47 @@ public class SwerveDrive extends SubsystemBase { */ public static final Matrix kDefaultVisionMeasurementStdDevs = VecBuilder.fill(0.7, 0.7, Double.MAX_VALUE); - private final SwerveDrivePoseEstimator kPoseEstimator = new SwerveDrivePoseEstimator( - kSwerveKinematics, - getAngleForOdometry(), - getSwerveModulePositions(), - new Pose2d(), - kDefaultStateStdDevs, - kDefaultVisionMeasurementStdDevs); + 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 modules, pose estimator, PathPlanner AutoBuilder, and initial field pose. */ - public SwerveDrive() { + /** + * 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(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); + + if (configurePathPlanner) { + configurePathPlannerAutoBuilder(); + } resetPose(FieldConstants.kDefaultOdometryPose); SmartDashboard.putData(this); } + /** @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( @@ -239,7 +259,56 @@ private SwerveModuleState[] getOptimizedModuleStatesFromChassisSpeeds(ChassisSpe } private Rotation2d getAngleForOdometry() { - return RobotContainer.kNavx.getRotation2d(); + return headingSupplier.get(); + } + + /** Releases module CAN devices; for simulation unit tests only. */ + void closeModulesForTest() { + kFrontLeft.close(); + kFrontRight.close(); + kRearLeft.close(); + kRearRight.close(); + } + + /** Seeds pivot CANcoder sim at 0° on all modules (sim unit tests). */ + void seedSimulationPivotEncodersZero() { + seedSimulationPivotEncoder(kFrontLeft); + seedSimulationPivotEncoder(kFrontRight); + seedSimulationPivotEncoder(kRearLeft); + seedSimulationPivotEncoder(kRearRight); + } + + 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); + } + + /** 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()))); + } + + /** 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); + } } /** diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 237df42..4cca1e6 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -207,6 +207,11 @@ 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. */ 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..e2c6132 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java @@ -0,0 +1,98 @@ +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.extension.ExtendWith; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.simulation.SimHooks; + +import frc.robot.testsupport.WpilibSimulationExtension; + +/** + * Constructs a real {@link SwerveDrive} (four sim-backed modules) without loading {@link frc.robot.RobotContainer}. + */ +@ExtendWith(WpilibSimulationExtension.class) +class SwerveDriveTest { + private static final double DELTA = 1e-3; + + private SwerveDrive swerve; + + @BeforeEach + void setUpSwerve() { + // Arrange — injected heading avoids RobotContainer ↔ SwerveDrive static init cycle + swerve = new SwerveDrive(() -> Rotation2d.kZero, false); + swerve.seedSimulationPivotEncodersZero(); + SimHooks.stepTiming(0.02); + } + + @AfterEach + void tearDownSwerve() { + if (swerve != null) { + swerve.closeModulesForTest(); + swerve = null; + } + } + + @Test + void resetPose_updatesEstimatedPose() { + // Arrange + Pose2d expected = new Pose2d(2.0, 1.5, Rotation2d.fromDegrees(30)); + + // 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 drive_belowDeadband_zerosModuleWheelSpeeds() { + // Arrange + 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() { + // Arrange + double forwardSpeed = 1.0; + + // Act + swerve.drive(forwardSpeed, 0.0, 0.0, false); + swerve.runSimulationSteps(5); + + // Assert — commanded output on sim motors (measured wheel speed may still be 0 without encoder motion) + assertTrue(swerve.maxAbsDriveMotorOutput() > 0.01); + } + + @Test + void holdAngleWhileDriving_headingError_commandsPivotMotors() { + // Arrange + 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); + } + +} From 34012a88eb94727dcccff35e13663a95c31d0f9c Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 21:38:21 +0000 Subject: [PATCH 10/17] Expand SwerveDrive and DriveCommands integration tests Add SwerveDriveTestFixture (shared sim instance), 13 subsystem tests, 6 command integration tests, HAL init guard, and test ordering fixes. Co-authored-by: ernestgwilsoniii --- .cursor/rules/frc-unit-tests.mdc | 1 + .../robot/subsystems/drive/SwerveDrive.java | 16 ++ .../DriveCommandsSwerveIntegrationTest.java | 217 ++++++++++++++++++ .../commands/drive/DriveCommandsTest.java | 12 +- .../subsystems/drive/SwerveDriveTest.java | 176 ++++++++++++-- .../drive/SwerveDriveTestFixture.java | 77 +++++++ .../WpilibSimulationExtension.java | 7 +- 7 files changed, 481 insertions(+), 25 deletions(-) create mode 100644 src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java create mode 100644 src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java diff --git a/.cursor/rules/frc-unit-tests.mdc b/.cursor/rules/frc-unit-tests.mdc index da7b7c9..edba964 100644 --- a/.cursor/rules/frc-unit-tests.mdc +++ b/.cursor/rules/frc-unit-tests.mdc @@ -25,5 +25,6 @@ Rules of thumb: - **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` and `@TestInstance(Lifecycle.PER_CLASS)` so the same four modules are not constructed on every test method (CAN reuse in one JVM). - 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/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index bad31f5..7adfb05 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -311,6 +311,22 @@ void runSimulationSteps(int iterations) { } } + /** 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()); + } + + /** 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(); + } + /** * Sets chassis motion from teleop, heading hold, or internal callers. * 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..a807988 --- /dev/null +++ b/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java @@ -0,0 +1,217 @@ +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.AfterAll; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.MethodOrderer; +import org.junit.jupiter.api.Order; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.TestInstance; +import org.junit.jupiter.api.TestMethodOrder; +import org.junit.jupiter.api.extension.ExtendWith; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +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) +@TestMethodOrder(MethodOrderer.OrderAnnotation.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; + + private double savedTranslationalMax; + private double savedAngularMax; + + @AfterAll + void releaseSharedHardware() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().run(); + SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.6; + SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); + fixture = null; + } + + @BeforeEach + void resetBetweenTests() { + if (fixture == null) { + savedTranslationalMax = SwerveDrive.maxTranslationalSpeedMetersPerSecond; + savedAngularMax = SwerveDrive.maxAngularSpeedRadiansPerSecond; + fixture = SwerveDriveTestFixture.open(); + swerve = fixture.swerve(); + controller = new DemonCommandXboxController(kControllerPort); + controllerSim = new XboxControllerSim(kControllerPort); + } + CommandScheduler.getInstance().cancelAll(); + SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.6; + SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); + swerve.stop(); + swerve.resetPose(FieldConstants.kDefaultOdometryPose); + fixture.reseedPivotEncoders(); + controllerSim.setLeftY(0.0); + controllerSim.setLeftX(0.0); + controllerSim.setRightX(0.0); + controllerSim.notifyNewData(); + SimHooks.stepTiming(0.02); + } + + @AfterEach + void cancelScheduledCommands() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().run(); + if (swerve != null) { + swerve.stop(); + } + } + + @Test + @Order(1) + void driveWithXboxController_centeredThenFullStick_togglesMotorOutput() { + // Arrange + Command teleop = DriveCommands.driveWithXboxController( + swerve, controller, () -> false, 0.05, 0.0); + CommandScheduler.getInstance().schedule(teleop); + + // Act — centered sticks + controllerSim.setLeftY(0.0); + controllerSim.setLeftX(0.0); + controllerSim.setRightX(0.0); + controllerSim.notifyNewData(); + 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 + @Order(2) + 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 + @Order(3) + 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 + @Order(4) + 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 + @Order(5) + 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 + @Order(6) + 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 — sequence started; heading capture is unchanged until driver resets pose + assertTrue(CommandScheduler.getInstance().isScheduled(defense) + || defense.isFinished()); + assertEquals(headingBefore.getDegrees(), swerve.getPose().getRotation().getDegrees(), DELTA); + } +} diff --git a/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java b/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java index 43744c7..766822d 100644 --- a/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java +++ b/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java @@ -20,16 +20,20 @@ class DriveCommandsTest { @BeforeEach void setUp() { - savedTranslationalMax = SwerveDrive.maxTranslationalSpeedMetersPerSecond; - savedAngularMax = SwerveDrive.maxAngularSpeedRadiansPerSecond; 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(); - SwerveDrive.maxTranslationalSpeedMetersPerSecond = savedTranslationalMax; - SwerveDrive.maxAngularSpeedRadiansPerSecond = savedAngularMax; + CommandScheduler.getInstance().run(); + SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.6; + SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); } @Test diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java index e2c6132..540e4ce 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java @@ -3,47 +3,68 @@ 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.AfterAll; 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.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; -/** - * Constructs a real {@link SwerveDrive} (four sim-backed modules) without loading {@link frc.robot.RobotContainer}. - */ +/** 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 SwerveDrive swerve; + private SwerveDriveTestFixture fixture; + + @AfterAll + void destroyFixture() { + SwerveDriveTestFixture.closeActive(); + fixture = null; + } @BeforeEach - void setUpSwerve() { - // Arrange — injected heading avoids RobotContainer ↔ SwerveDrive static init cycle - swerve = new SwerveDrive(() -> Rotation2d.kZero, false); + void resetSwerveBetweenTests() { + fixture = SwerveDriveTestFixture.open(); + CommandScheduler.getInstance().cancelAll(); + SwerveDrive swerve = fixture.swerve(); + swerve.stop(); + fixture.setHeading(Rotation2d.kZero); + swerve.resetPose(FieldConstants.kDefaultOdometryPose); swerve.seedSimulationPivotEncodersZero(); SimHooks.stepTiming(0.02); } - @AfterEach - void tearDownSwerve() { - if (swerve != null) { - swerve.closeModulesForTest(); - swerve = null; - } + @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); @@ -55,8 +76,35 @@ void resetPose_updatesEstimatedPose() { } @Test - void drive_belowDeadband_zerosModuleWheelSpeeds() { + 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 @@ -71,20 +119,58 @@ void drive_belowDeadband_zerosModuleWheelSpeeds() { @Test void drive_forwardRobotRelative_commandsDriveMotors() { - // Arrange - double forwardSpeed = 1.0; + 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(forwardSpeed, 0.0, 0.0, false); + swerve.drive(0.0, 1.0, 0.0, false); swerve.runSimulationSteps(5); - // Assert — commanded output on sim motors (measured wheel speed may still be 0 without encoder motion) + // 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() { - // Arrange + SwerveDrive swerve = fixture.swerve(); swerve.resetPose(Pose2d.kZero); // Act @@ -95,4 +181,54 @@ void holdAngleWhileDriving_headingError_commandsPivotMotors() { 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 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..fea8d89 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java @@ -0,0 +1,77 @@ +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; + +/** + * 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. */ + public static void closeActive() { + if (active != null) { + active.close(); + active = null; + } + } + + private SwerveDriveTestFixture() { + swerve = new SwerveDrive(headingSupplier(), false); + CommandScheduler.getInstance().registerSubsystem(swerve); + 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(); + } + + 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(); + swerve.closeModulesForTest(); + } +} diff --git a/src/test/java/frc/robot/testsupport/WpilibSimulationExtension.java b/src/test/java/frc/robot/testsupport/WpilibSimulationExtension.java index 07d1939..a06c138 100644 --- a/src/test/java/frc/robot/testsupport/WpilibSimulationExtension.java +++ b/src/test/java/frc/robot/testsupport/WpilibSimulationExtension.java @@ -19,10 +19,15 @@ * @see WPILib unit testing */ public final class WpilibSimulationExtension implements BeforeEachCallback { + private static boolean halInitialized; + /** {@inheritDoc} */ @Override public void beforeEach(ExtensionContext context) { - assertTrue(HAL.initialize(500, 0), "HAL.initialize failed"); + if (!halInitialized) { + assertTrue(HAL.initialize(500, 0), "HAL.initialize failed"); + halInitialized = true; + } DriverStationSim.resetData(); DriverStationSim.setDsAttached(true); DriverStationSim.setEnabled(true); From 922770062110f78c49e0a6085228f96f5f6602c4 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 21:42:19 +0000 Subject: [PATCH 11/17] Add more deterministic swerve and DriveCommands tests Module positions, vision heading std devs, goSlow scaling contract, DriverStation sim alliance sign; strengthen goSlow unit test. Co-authored-by: ernestgwilsoniii --- .../DriveCommandsSwerveIntegrationTest.java | 31 +++++++++++++-- .../commands/drive/DriveCommandsTest.java | 24 ++++++++++++ .../subsystems/drive/SwerveDriveTest.java | 39 +++++++++++++++++++ 3 files changed, 91 insertions(+), 3 deletions(-) diff --git a/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java b/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java index a807988..0930f9c 100644 --- a/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java +++ b/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java @@ -14,8 +14,12 @@ import org.junit.jupiter.api.TestMethodOrder; 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; @@ -72,6 +76,8 @@ void resetBetweenTests() { controllerSim.setLeftX(0.0); controllerSim.setRightX(0.0); controllerSim.notifyNewData(); + DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); + DriverStationSim.notifyNewData(); SimHooks.stepTiming(0.02); } @@ -209,9 +215,28 @@ void holdCurrentHeadingWhileDriving_schedulesAndRunsWithoutInterrupting() { fixture.runSimulationSteps(1); } - // Assert — sequence started; heading capture is unchanged until driver resets pose - assertTrue(CommandScheduler.getInstance().isScheduled(defense) - || defense.isFinished()); + // Assert + assertTrue(CommandScheduler.getInstance().isScheduled(defense) || defense.isFinished()); assertEquals(headingBefore.getDegrees(), swerve.getPose().getRotation().getDegrees(), DELTA); } + + @Test + @Order(7) + 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 + DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); + DriverStationSim.notifyNewData(); + + // 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 index 766822d..3e7ee5e 100644 --- a/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java +++ b/src/test/java/frc/robot/commands/drive/DriveCommandsTest.java @@ -10,6 +10,7 @@ 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; @@ -58,6 +59,29 @@ void goSlow_whileScheduled_lowersMaxSpeeds_restoresOnEnd() { 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 diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java index 540e4ce..bf8143c 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java @@ -14,6 +14,7 @@ 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; @@ -208,6 +209,44 @@ void lock_doesNotThrowAndStopsModulesOnStop() { 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(); From 524c81335d9fd5c61c952c463da9a309069d51ee Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 21:47:58 +0000 Subject: [PATCH 12/17] Make swerve tests order-independent without @Order Remove per-class fixture teardown that broke other classes, drop duplicate CommandScheduler registration, reset shared sim state between methods, recreate Xbox sim controllers after DriverStation reset, and enable random test order in CI. Co-authored-by: ernestgwilsoniii --- .cursor/rules/frc-unit-tests.mdc | 3 +- .../DriveCommandsSwerveIntegrationTest.java | 75 ++++++------------- .../subsystems/drive/SwerveDriveTest.java | 23 ++---- .../drive/SwerveDriveTestFixture.java | 28 ++++++- src/test/resources/junit-platform.properties | 2 + 5 files changed, 60 insertions(+), 71 deletions(-) create mode 100644 src/test/resources/junit-platform.properties diff --git a/.cursor/rules/frc-unit-tests.mdc b/.cursor/rules/frc-unit-tests.mdc index edba964..61747dd 100644 --- a/.cursor/rules/frc-unit-tests.mdc +++ b/.cursor/rules/frc-unit-tests.mdc @@ -25,6 +25,7 @@ Rules of thumb: - **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` and `@TestInstance(Lifecycle.PER_CLASS)` so the same four modules are not constructed on every test method (CAN reuse in one JVM). +- **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/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java b/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java index 0930f9c..2a287a0 100644 --- a/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java +++ b/src/test/java/frc/robot/commands/drive/DriveCommandsSwerveIntegrationTest.java @@ -4,14 +4,10 @@ import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; -import org.junit.jupiter.api.AfterAll; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.MethodOrderer; -import org.junit.jupiter.api.Order; import org.junit.jupiter.api.Test; import org.junit.jupiter.api.TestInstance; -import org.junit.jupiter.api.TestMethodOrder; import org.junit.jupiter.api.extension.ExtendWith; import edu.wpi.first.hal.AllianceStationID; @@ -34,7 +30,6 @@ /** End-to-end {@link DriveCommands} against a sim {@link SwerveDrive} (one drivetrain per class). */ @ExtendWith(WpilibSimulationExtension.class) @TestInstance(TestInstance.Lifecycle.PER_CLASS) -@TestMethodOrder(MethodOrderer.OrderAnnotation.class) class DriveCommandsSwerveIntegrationTest { private static final int kControllerPort = 3; private static final double DELTA = 1e-3; @@ -44,54 +39,38 @@ class DriveCommandsSwerveIntegrationTest { private DemonCommandXboxController controller; private XboxControllerSim controllerSim; - private double savedTranslationalMax; - private double savedAngularMax; + @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(); + } - @AfterAll - void releaseSharedHardware() { - CommandScheduler.getInstance().cancelAll(); - CommandScheduler.getInstance().run(); - SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.6; - SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); - fixture = null; + @AfterEach + void tearDown() { + fixture.resetBetweenTests(); + resetControllerSticks(); + setAllianceBlue(); } - @BeforeEach - void resetBetweenTests() { - if (fixture == null) { - savedTranslationalMax = SwerveDrive.maxTranslationalSpeedMetersPerSecond; - savedAngularMax = SwerveDrive.maxAngularSpeedRadiansPerSecond; - fixture = SwerveDriveTestFixture.open(); - swerve = fixture.swerve(); - controller = new DemonCommandXboxController(kControllerPort); - controllerSim = new XboxControllerSim(kControllerPort); - } - CommandScheduler.getInstance().cancelAll(); - SwerveDrive.maxTranslationalSpeedMetersPerSecond = 4.6; - SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); - swerve.stop(); - swerve.resetPose(FieldConstants.kDefaultOdometryPose); - fixture.reseedPivotEncoders(); + private void resetControllerSticks() { controllerSim.setLeftY(0.0); controllerSim.setLeftX(0.0); controllerSim.setRightX(0.0); controllerSim.notifyNewData(); - DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); - DriverStationSim.notifyNewData(); - SimHooks.stepTiming(0.02); } - @AfterEach - void cancelScheduledCommands() { - CommandScheduler.getInstance().cancelAll(); - CommandScheduler.getInstance().run(); - if (swerve != null) { - swerve.stop(); - } + private void setAllianceBlue() { + DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); + DriverStationSim.notifyNewData(); } @Test - @Order(1) void driveWithXboxController_centeredThenFullStick_togglesMotorOutput() { // Arrange Command teleop = DriveCommands.driveWithXboxController( @@ -99,10 +78,7 @@ void driveWithXboxController_centeredThenFullStick_togglesMotorOutput() { CommandScheduler.getInstance().schedule(teleop); // Act — centered sticks - controllerSim.setLeftY(0.0); - controllerSim.setLeftX(0.0); - controllerSim.setRightX(0.0); - controllerSim.notifyNewData(); + resetControllerSticks(); SimHooks.stepTiming(0.02); for (int i = 0; i < 3; i++) { CommandScheduler.getInstance().run(); @@ -123,7 +99,6 @@ void driveWithXboxController_centeredThenFullStick_togglesMotorOutput() { } @Test - @Order(2) void resetHeading_setsPracticeFieldPose() { // Arrange Command reset = DriveCommands.resetHeading( @@ -140,7 +115,6 @@ void resetHeading_setsPracticeFieldPose() { } @Test - @Order(3) void stopCommand_invokesSubsystemStop() { // Arrange swerve.drive(1.0, 0.0, 0.0, false); @@ -157,7 +131,6 @@ void stopCommand_invokesSubsystemStop() { } @Test - @Order(4) void lockCommand_whileScheduled_runsUntilCancelled() { // Arrange Command lock = DriveCommands.lock(swerve); @@ -175,7 +148,6 @@ void lockCommand_whileScheduled_runsUntilCancelled() { } @Test - @Order(5) void goSlow_whileScheduled_limitsTeleopMaxSpeed() { // Arrange Command goSlow = DriveCommands.goSlow(swerve); @@ -197,7 +169,6 @@ void goSlow_whileScheduled_limitsTeleopMaxSpeed() { } @Test - @Order(6) void holdCurrentHeadingWhileDriving_schedulesAndRunsWithoutInterrupting() { // Arrange swerve.resetPose(new Pose2d(0, 0, Rotation2d.kZero)); @@ -221,7 +192,6 @@ void holdCurrentHeadingWhileDriving_schedulesAndRunsWithoutInterrupting() { } @Test - @Order(7) void fieldRelativeAllianceSign_readsDriverStationSimAlliance() { // Arrange & Act — red DriverStationSim.setAllianceStationId(AllianceStationID.Red1); @@ -232,8 +202,7 @@ void fieldRelativeAllianceSign_readsDriverStationSimAlliance() { assertEquals(-1, DemonCommandXboxController.fieldRelativeAllianceSign(() -> true)); // Arrange & Act — blue - DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); - DriverStationSim.notifyNewData(); + setAllianceBlue(); // Assert assertEquals(Alliance.Blue, DriverStation.getAlliance().orElse(Alliance.Blue)); diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java index bf8143c..e2aa977 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTest.java @@ -3,7 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; -import org.junit.jupiter.api.AfterAll; +import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.junit.jupiter.api.TestInstance; @@ -33,22 +33,15 @@ class SwerveDriveTest { private SwerveDriveTestFixture fixture; - @AfterAll - void destroyFixture() { - SwerveDriveTestFixture.closeActive(); - fixture = null; - } - @BeforeEach - void resetSwerveBetweenTests() { + void setUp() { fixture = SwerveDriveTestFixture.open(); - CommandScheduler.getInstance().cancelAll(); - SwerveDrive swerve = fixture.swerve(); - swerve.stop(); - fixture.setHeading(Rotation2d.kZero); - swerve.resetPose(FieldConstants.kDefaultOdometryPose); - swerve.seedSimulationPivotEncodersZero(); - SimHooks.stepTiming(0.02); + fixture.resetBetweenTests(); + } + + @AfterEach + void tearDown() { + fixture.resetBetweenTests(); } @Test diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java index fea8d89..d917bb0 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java @@ -7,6 +7,8 @@ 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}. * @@ -26,7 +28,13 @@ public static SwerveDriveTestFixture open() { return active; } - /** Closes the shared instance if it was opened. */ + /** + * 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(); @@ -36,7 +44,6 @@ public static void closeActive() { private SwerveDriveTestFixture() { swerve = new SwerveDrive(headingSupplier(), false); - CommandScheduler.getInstance().registerSubsystem(swerve); swerve.seedSimulationPivotEncodersZero(); SimHooks.stepTiming(0.02); } @@ -57,6 +64,22 @@ 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); } @@ -72,6 +95,7 @@ public double maxAbsPivotMotorOutput() { @Override public void close() { CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().unregisterSubsystem(swerve); swerve.closeModulesForTest(); } } diff --git a/src/test/resources/junit-platform.properties b/src/test/resources/junit-platform.properties new file mode 100644 index 0000000..1fa53ef --- /dev/null +++ b/src/test/resources/junit-platform.properties @@ -0,0 +1,2 @@ +# Catch order-dependent tests during local/CI runs +junit.jupiter.testmethod.order.default=random From c08f14a15f8efbbd8eab305c94cb050bd7445e6f Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 21:56:15 +0000 Subject: [PATCH 13/17] Fix JUnit random method orderer FQCN in junit-platform.properties Co-authored-by: ernestgwilsoniii --- src/test/resources/junit-platform.properties | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/test/resources/junit-platform.properties b/src/test/resources/junit-platform.properties index 1fa53ef..871037e 100644 --- a/src/test/resources/junit-platform.properties +++ b/src/test/resources/junit-platform.properties @@ -1,2 +1,2 @@ -# Catch order-dependent tests during local/CI runs -junit.jupiter.testmethod.order.default=random +# 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 From e3b3d1d34500a621a40d3e8c11ca852db313a063 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 19 Oct 2026 21:58:23 +0000 Subject: [PATCH 14/17] Add mavenCentral for Gradle plugin resolution in CI Co-authored-by: ernestgwilsoniii --- settings.gradle | 1 + 1 file changed, 1 insertion(+) 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 From fc2e19cf8e901cc848842849a35bc270d1cff23c Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Tue, 20 Oct 2026 00:27:46 +0000 Subject: [PATCH 15/17] Fix desktop sim auto heading by integrating chassis speeds Kinematic module angles from toSwerveModuleStates were fed into the pose estimator each tick, which can jump during path rotation and unlike main (which uses measured pivot encoders) caused erratic Field2d heading in auto. Desktop simulateJava now integrates robot-relative chassis speeds with transformBy instead. Co-authored-by: ernestgwilsoniii --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../robot/subsystems/drive/SwerveDrive.java | 82 ++++++++++++++++--- .../robot/subsystems/drive/SwerveModule.java | 34 +++++++- .../drive/SwerveDriveTestFixture.java | 2 +- 4 files changed, 106 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 398eb72..c436d74 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -336,10 +336,8 @@ private void configureNamedCommandsForAuto() { /** https://pathplanner.dev/pplib-custom-logging.html */ private void configurePathPlannerLogging() { // Logging callback for current robot pose - PathPlannerLogging.setLogCurrentPoseCallback((pose) -> { - // Do whatever you want with the pose here - kField.setRobotPose(pose); - }); + // Robot.robotPeriodic() already sets kField from kSwerveDrive.getPose(); avoid a second pose source in sim. + PathPlannerLogging.setLogCurrentPoseCallback((pose) -> {}); // Logging callback for target robot pose PathPlannerLogging.setLogTargetPoseCallback((pose) -> { diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index 7adfb05..8b102f2 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -21,6 +21,7 @@ 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.Transform2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -30,7 +31,9 @@ import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.simulation.SimHooks; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -92,6 +95,19 @@ public class SwerveDrive extends SubsystemBase { private final Supplier headingSupplier; + /** Integrated gyro substitute for desktop sim (NavX USB is not modeled in sim). */ + private Rotation2d simulationHeading = Rotation2d.kZero; + + /** Last robot-relative chassis setpoint; drives smooth desktop-sim odometry (see {@link #simulationPeriodic()}). */ + private ChassisSpeeds lastRobotRelativeChassisSpeeds = new ChassisSpeeds(); + + private final SwerveModulePosition[] desktopSimModulePositions = { + new SwerveModulePosition(0.0, Rotation2d.kZero), + new SwerveModulePosition(0.0, Rotation2d.kZero), + new SwerveModulePosition(0.0, Rotation2d.kZero), + new SwerveModulePosition(0.0, Rotation2d.kZero), + }; + private final PIDController thetaPid = new PIDController( SwerveDriveConfig.DRIVE_ANGLE_P, SwerveDriveConfig.DRIVE_ANGLE_I, @@ -175,18 +191,52 @@ public void initSendable(SendableBuilder builder) { /** Updates {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator} from module encoders and gyro. */ @Override public void periodic() { + if (useDesktopKinematicSimulation()) { + return; + } kPoseEstimator.updateWithTime(Timer.getFPGATimestamp(), getAngleForOdometry(), getSwerveModulePositions()); } - /** Steps REV {@link com.revrobotics.sim.SparkMaxSim} on each module when running desktop simulation. */ + /** + * Desktop {@code simulateJava}: integrate commanded chassis speeds for the Field widget. JUnit sim tests + * still step REV {@link com.revrobotics.sim.SparkMaxSim} on each module. + */ @Override public void simulationPeriodic() { + if (useDesktopKinematicSimulation()) { + integrateDesktopSimulationOdometry(TimedRobot.kDefaultPeriod); + return; + } kFrontLeft.simulationPeriodic(); kFrontRight.simulationPeriodic(); kRearLeft.simulationPeriodic(); kRearRight.simulationPeriodic(); } + /** + * Integrates robot-relative chassis speeds into field pose. Does not feed kinematic module angles into + * {@link SwerveDrivePoseEstimator} — those angles can jump when paths rotate (and differ from pivot + * shortest-path flips on hardware). On {@code main}, sim odometry used measured module encoders instead. + */ + private void integrateDesktopSimulationOdometry(double dt) { + ChassisSpeeds speeds = ChassisSpeeds.discretize(lastRobotRelativeChassisSpeeds, dt); + Pose2d pose = getPose().transformBy( + new Transform2d( + speeds.vxMetersPerSecond * dt, + speeds.vyMetersPerSecond * dt, + Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * dt))); + simulationHeading = pose.getRotation(); + resetDesktopSimulationOdometryState(pose); + } + + private void resetDesktopSimulationOdometryState(Pose2d pose) { + simulationHeading = pose.getRotation(); + for (int i = 0; i < 4; i++) { + desktopSimModulePositions[i] = new SwerveModulePosition(0.0, desktopSimModulePositions[i].angle); + } + kPoseEstimator.resetPosition(simulationHeading, desktopSimModulePositions, pose); + } + /** * Fuses a vision pose into odometry (e.g. Limelight/Photon callbacks in {@link frc.robot.RobotContainer}). * @@ -235,6 +285,10 @@ public Pose2d getPose() { * @param pose new field pose */ public void resetPose(Pose2d pose) { + if (useDesktopKinematicSimulation()) { + resetDesktopSimulationOdometryState(pose); + return; + } kPoseEstimator.resetPosition(getAngleForOdometry(), getSwerveModulePositions(), pose); } @@ -244,6 +298,7 @@ public ChassisSpeeds getChassisSpeeds() { } private void setModuleStates(ChassisSpeeds chassisSpeeds) { + lastRobotRelativeChassisSpeeds = chassisSpeeds; SwerveModuleState[] desiredStates = getOptimizedModuleStatesFromChassisSpeeds(chassisSpeeds); kFrontLeft.setDesiredState(desiredStates[0]); kFrontRight.setDesiredState(desiredStates[1]); @@ -259,9 +314,17 @@ private SwerveModuleState[] getOptimizedModuleStatesFromChassisSpeeds(ChassisSpe } private Rotation2d getAngleForOdometry() { + if (useDesktopKinematicSimulation()) { + return simulationHeading; + } return headingSupplier.get(); } + /** {@code simulateJava} only — not JUnit HAL tests ({@link DriverStation#isTest()}). */ + private static boolean useDesktopKinematicSimulation() { + return RobotBase.isSimulation() && !DriverStation.isTest(); + } + /** Releases module CAN devices; for simulation unit tests only. */ void closeModulesForTest() { kFrontLeft.close(); @@ -272,16 +335,10 @@ void closeModulesForTest() { /** Seeds pivot CANcoder sim at 0° on all modules (sim unit tests). */ void seedSimulationPivotEncodersZero() { - seedSimulationPivotEncoder(kFrontLeft); - seedSimulationPivotEncoder(kFrontRight); - seedSimulationPivotEncoder(kRearLeft); - seedSimulationPivotEncoder(kRearRight); - } - - private static void seedSimulationPivotEncoder(SwerveModule module) { - module.pivotEncoderSimState().setSupplyVoltage(12.0); - module.pivotEncoderSimState().setRawPosition(Units.Rotations.of(0.0)); - module.refreshPivotAbsolutePosition(); + kFrontLeft.resetSimulationEncoderState(); + kFrontRight.resetSimulationEncoderState(); + kRearLeft.resetSimulationEncoderState(); + kRearRight.resetSimulationEncoderState(); SimHooks.stepTiming(0.02); } @@ -353,6 +410,7 @@ vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond, getPose().getRotati } private void stopModulesButHoldPivotAngle() { + lastRobotRelativeChassisSpeeds = new ChassisSpeeds(); 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()))); @@ -381,6 +439,7 @@ public SwerveModulePosition[] getSwerveModulePositions() { /** Stops all modules and resets the heading-hold PID controller. */ public void stop() { + lastRobotRelativeChassisSpeeds = new ChassisSpeeds(); kFrontRight.stop(); kFrontLeft.stop(); kRearRight.stop(); @@ -390,6 +449,7 @@ public void stop() { /** Angles modules in an X pattern to resist being pushed on defense or on a slope. */ public void lock() { + lastRobotRelativeChassisSpeeds = new ChassisSpeeds(); kFrontRight.setDesiredState(0, -45); kFrontLeft.setDesiredState(0, 45); kRearRight.setDesiredState(0, 45); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 4cca1e6..4837249 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -24,6 +24,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.Units; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; @@ -47,6 +48,9 @@ * @see SwerveDrive */ public class SwerveModule extends SubsystemBase { + private static final DCMotor kNeoMotor = DCMotor.getNEO(1); + private static final double kSimLoopPeriodSec = 0.02; + /** Module label (e.g. {@code FrontLeft}) for logging and Shuffleboard. */ public final String name; /** Calibrated CANcoder offset added to absolute reading (degrees). */ @@ -178,8 +182,18 @@ public void simulationPeriodic() { return; } double busVoltage = RoboRioSim.getVInVoltage(); - driveSim.iterate(driveSim.getVelocity(), busVoltage, 0.02); - pivotSim.iterate(pivotSim.getVelocity(), busVoltage, 0.02); + // REV iterate() expects motor RPM from a physics model; scale NEO free speed by applied output. + double neoFreeSpeedRpm = kNeoMotor.freeSpeedRadPerSec * 60.0 / (2.0 * Math.PI); + double driveMotorRpm = + driveSim.getAppliedOutput() * neoFreeSpeedRpm / SwerveDriveConfig.SDS_L2_DRIVE_GEARING; + driveSim.iterate(driveMotorRpm, busVoltage, kSimLoopPeriodSec); + + double pivotMotorRpm = pivotSim.getAppliedOutput() * neoFreeSpeedRpm; + pivotSim.iterate(pivotMotorRpm, busVoltage, kSimLoopPeriodSec); + + // Pivot angle odometry reads the CANcoder; keep sim encoder in sync with pivot motor rotation. + pivotEncoderSimState().setRawPosition(Units.Rotations.of(pivotSim.getPosition())); + refreshPivotAbsolutePosition(); } /** Steps REV physics sim; used by unit tests and robot simulation. */ @@ -202,6 +216,22 @@ void refreshPivotAbsolutePosition() { BaseStatusSignal.refreshAll(pivotEncoder.getAbsolutePosition()); } + /** Zeros drive/pivot REV sim encoder state (sim tests and fixture reset between tests). */ + void resetSimulationEncoderState() { + if (driveSim == null) { + return; + } + driveSim.setPosition(0.0); + driveSim.setVelocity(0.0); + driveEncoder.setPosition(0.0); + pivotSim.setPosition(0.0); + pivotSim.setVelocity(0.0); + pivotEncoderSimState().setSupplyVoltage(12.0); + pivotEncoderSimState().setRawPosition(Units.Rotations.of(0.0)); + refreshPivotAbsolutePosition(); + stop(); + } + /** @return pivot {@link SparkMaxSim}, or null on the roboRIO */ SparkMaxSim pivotSparkSim() { return pivotSim; diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java index d917bb0..2f73bb5 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java @@ -75,8 +75,8 @@ public void resetBetweenTests() { SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); heading.set(Rotation2d.kZero); swerve.stop(); - swerve.resetPose(FieldConstants.kDefaultOdometryPose); swerve.seedSimulationPivotEncodersZero(); + swerve.resetPose(FieldConstants.kDefaultOdometryPose); SimHooks.stepTiming(0.02); } From 62ae32af722d597c958034502357891b4d6279f1 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Tue, 20 Oct 2026 00:33:58 +0000 Subject: [PATCH 16/17] Split desktop sim odometry: encoders for auto, kinematic for teleop PathPlanner needs non-zero getChassisSpeeds() and pose that tracks the path. Kinematic sim reported zero module speeds and open-loop pose integration desynced from trajectory rotation targets. Auto now uses module physics odometry like main; teleop keeps chassis-speed integration. Sim gyro uses simulationHeading instead of NavX; skip vision fusion in simulateJava. Co-authored-by: ernestgwilsoniii --- .../robot/subsystems/drive/SwerveDrive.java | 61 ++++++++++++------- 1 file changed, 40 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index 8b102f2..a55345c 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -98,16 +98,14 @@ public class SwerveDrive extends SubsystemBase { /** Integrated gyro substitute for desktop sim (NavX USB is not modeled in sim). */ private Rotation2d simulationHeading = Rotation2d.kZero; + /** Teleop-only desktop pose; autonomous sim uses module encoders like {@code main}. */ + private Pose2d desktopSimPose = FieldConstants.kDefaultOdometryPose; + + private boolean desktopKinematicSimulationActive; + /** Last robot-relative chassis setpoint; drives smooth desktop-sim odometry (see {@link #simulationPeriodic()}). */ private ChassisSpeeds lastRobotRelativeChassisSpeeds = new ChassisSpeeds(); - private final SwerveModulePosition[] desktopSimModulePositions = { - new SwerveModulePosition(0.0, Rotation2d.kZero), - new SwerveModulePosition(0.0, Rotation2d.kZero), - new SwerveModulePosition(0.0, Rotation2d.kZero), - new SwerveModulePosition(0.0, Rotation2d.kZero), - }; - private final PIDController thetaPid = new PIDController( SwerveDriveConfig.DRIVE_ANGLE_P, SwerveDriveConfig.DRIVE_ANGLE_I, @@ -195,6 +193,9 @@ public void periodic() { return; } kPoseEstimator.updateWithTime(Timer.getFPGATimestamp(), getAngleForOdometry(), getSwerveModulePositions()); + if (useDesktopSimulation()) { + simulationHeading = kPoseEstimator.getEstimatedPosition().getRotation(); + } } /** @@ -204,9 +205,15 @@ public void periodic() { @Override public void simulationPeriodic() { if (useDesktopKinematicSimulation()) { + if (!desktopKinematicSimulationActive) { + desktopSimPose = kPoseEstimator.getEstimatedPosition(); + simulationHeading = desktopSimPose.getRotation(); + } + desktopKinematicSimulationActive = true; integrateDesktopSimulationOdometry(TimedRobot.kDefaultPeriod); return; } + desktopKinematicSimulationActive = false; kFrontLeft.simulationPeriodic(); kFrontRight.simulationPeriodic(); kRearLeft.simulationPeriodic(); @@ -220,21 +227,12 @@ public void simulationPeriodic() { */ private void integrateDesktopSimulationOdometry(double dt) { ChassisSpeeds speeds = ChassisSpeeds.discretize(lastRobotRelativeChassisSpeeds, dt); - Pose2d pose = getPose().transformBy( + desktopSimPose = desktopSimPose.transformBy( new Transform2d( speeds.vxMetersPerSecond * dt, speeds.vyMetersPerSecond * dt, Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * dt))); - simulationHeading = pose.getRotation(); - resetDesktopSimulationOdometryState(pose); - } - - private void resetDesktopSimulationOdometryState(Pose2d pose) { - simulationHeading = pose.getRotation(); - for (int i = 0; i < 4; i++) { - desktopSimModulePositions[i] = new SwerveModulePosition(0.0, desktopSimModulePositions[i].angle); - } - kPoseEstimator.resetPosition(simulationHeading, desktopSimModulePositions, pose); + simulationHeading = desktopSimPose.getRotation(); } /** @@ -245,6 +243,9 @@ private void resetDesktopSimulationOdometryState(Pose2d pose) { * @param stdDevs per-axis standard deviations; use {@link #kDefaultVisionMeasurementStdDevs} for defaults */ public void addVisionMeasurementForOdometry(Pose2d pose, double timestamp, Matrix stdDevs) { + if (useDesktopSimulation()) { + return; + } kPoseEstimator.addVisionMeasurement(pose, timestamp, stdDevs); } @@ -276,6 +277,9 @@ private void configurePathPlannerAutoBuilder() { /** @return fused field pose (meters, radians) from odometry and vision */ public Pose2d getPose() { + if (useDesktopKinematicSimulation()) { + return desktopSimPose; + } return kPoseEstimator.getEstimatedPosition(); } @@ -286,14 +290,21 @@ public Pose2d getPose() { */ public void resetPose(Pose2d pose) { if (useDesktopKinematicSimulation()) { - resetDesktopSimulationOdometryState(pose); + desktopSimPose = pose; + simulationHeading = pose.getRotation(); return; } + if (useDesktopSimulation()) { + simulationHeading = pose.getRotation(); + } kPoseEstimator.resetPosition(getAngleForOdometry(), getSwerveModulePositions(), pose); } /** @return current chassis speeds from measured module states (robot-relative) */ public ChassisSpeeds getChassisSpeeds() { + if (useDesktopKinematicSimulation()) { + return lastRobotRelativeChassisSpeeds; + } return kSwerveKinematics.toChassisSpeeds(getSwerveModuleStates()); } @@ -314,17 +325,25 @@ private SwerveModuleState[] getOptimizedModuleStatesFromChassisSpeeds(ChassisSpe } private Rotation2d getAngleForOdometry() { - if (useDesktopKinematicSimulation()) { + if (useDesktopSimulation()) { return simulationHeading; } return headingSupplier.get(); } /** {@code simulateJava} only — not JUnit HAL tests ({@link DriverStation#isTest()}). */ - private static boolean useDesktopKinematicSimulation() { + private static boolean useDesktopSimulation() { return RobotBase.isSimulation() && !DriverStation.isTest(); } + /** + * Teleop desktop sim integrates commanded chassis speeds; autonomous uses module physics odometry + * (PathPlanner needs measured {@link #getChassisSpeeds()} and encoder-based pose like on the real robot). + */ + private static boolean useDesktopKinematicSimulation() { + return useDesktopSimulation() && !DriverStation.isAutonomous(); + } + /** Releases module CAN devices; for simulation unit tests only. */ void closeModulesForTest() { kFrontLeft.close(); From a0089f6731aa2fb8ec6cf1a9906f2a70c7619e61 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Tue, 20 Oct 2026 00:37:39 +0000 Subject: [PATCH 17/17] Revert desktop sim teleop odometry experiments Restore drive and RobotContainer to e3b3d1d before the two recent simulateJava pose-integration commits that were pushed prematurely. Co-authored-by: ernestgwilsoniii --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../robot/subsystems/drive/SwerveDrive.java | 101 ++---------------- .../robot/subsystems/drive/SwerveModule.java | 34 +----- .../drive/SwerveDriveTestFixture.java | 2 +- 4 files changed, 18 insertions(+), 125 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c436d74..398eb72 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -336,8 +336,10 @@ private void configureNamedCommandsForAuto() { /** https://pathplanner.dev/pplib-custom-logging.html */ private void configurePathPlannerLogging() { // Logging callback for current robot pose - // Robot.robotPeriodic() already sets kField from kSwerveDrive.getPose(); avoid a second pose source in sim. - PathPlannerLogging.setLogCurrentPoseCallback((pose) -> {}); + PathPlannerLogging.setLogCurrentPoseCallback((pose) -> { + // Do whatever you want with the pose here + kField.setRobotPose(pose); + }); // Logging callback for target robot pose PathPlannerLogging.setLogTargetPoseCallback((pose) -> { diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java index a55345c..7adfb05 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java @@ -21,7 +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.Transform2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -31,9 +30,7 @@ import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.simulation.SimHooks; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -95,17 +92,6 @@ public class SwerveDrive extends SubsystemBase { private final Supplier headingSupplier; - /** Integrated gyro substitute for desktop sim (NavX USB is not modeled in sim). */ - private Rotation2d simulationHeading = Rotation2d.kZero; - - /** Teleop-only desktop pose; autonomous sim uses module encoders like {@code main}. */ - private Pose2d desktopSimPose = FieldConstants.kDefaultOdometryPose; - - private boolean desktopKinematicSimulationActive; - - /** Last robot-relative chassis setpoint; drives smooth desktop-sim odometry (see {@link #simulationPeriodic()}). */ - private ChassisSpeeds lastRobotRelativeChassisSpeeds = new ChassisSpeeds(); - private final PIDController thetaPid = new PIDController( SwerveDriveConfig.DRIVE_ANGLE_P, SwerveDriveConfig.DRIVE_ANGLE_I, @@ -189,52 +175,18 @@ public void initSendable(SendableBuilder builder) { /** Updates {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator} from module encoders and gyro. */ @Override public void periodic() { - if (useDesktopKinematicSimulation()) { - return; - } kPoseEstimator.updateWithTime(Timer.getFPGATimestamp(), getAngleForOdometry(), getSwerveModulePositions()); - if (useDesktopSimulation()) { - simulationHeading = kPoseEstimator.getEstimatedPosition().getRotation(); - } } - /** - * Desktop {@code simulateJava}: integrate commanded chassis speeds for the Field widget. JUnit sim tests - * still step REV {@link com.revrobotics.sim.SparkMaxSim} on each module. - */ + /** Steps REV {@link com.revrobotics.sim.SparkMaxSim} on each module when running desktop simulation. */ @Override public void simulationPeriodic() { - if (useDesktopKinematicSimulation()) { - if (!desktopKinematicSimulationActive) { - desktopSimPose = kPoseEstimator.getEstimatedPosition(); - simulationHeading = desktopSimPose.getRotation(); - } - desktopKinematicSimulationActive = true; - integrateDesktopSimulationOdometry(TimedRobot.kDefaultPeriod); - return; - } - desktopKinematicSimulationActive = false; kFrontLeft.simulationPeriodic(); kFrontRight.simulationPeriodic(); kRearLeft.simulationPeriodic(); kRearRight.simulationPeriodic(); } - /** - * Integrates robot-relative chassis speeds into field pose. Does not feed kinematic module angles into - * {@link SwerveDrivePoseEstimator} — those angles can jump when paths rotate (and differ from pivot - * shortest-path flips on hardware). On {@code main}, sim odometry used measured module encoders instead. - */ - private void integrateDesktopSimulationOdometry(double dt) { - ChassisSpeeds speeds = ChassisSpeeds.discretize(lastRobotRelativeChassisSpeeds, dt); - desktopSimPose = desktopSimPose.transformBy( - new Transform2d( - speeds.vxMetersPerSecond * dt, - speeds.vyMetersPerSecond * dt, - Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * dt))); - simulationHeading = desktopSimPose.getRotation(); - } - /** * Fuses a vision pose into odometry (e.g. Limelight/Photon callbacks in {@link frc.robot.RobotContainer}). * @@ -243,9 +195,6 @@ private void integrateDesktopSimulationOdometry(double dt) { * @param stdDevs per-axis standard deviations; use {@link #kDefaultVisionMeasurementStdDevs} for defaults */ public void addVisionMeasurementForOdometry(Pose2d pose, double timestamp, Matrix stdDevs) { - if (useDesktopSimulation()) { - return; - } kPoseEstimator.addVisionMeasurement(pose, timestamp, stdDevs); } @@ -277,9 +226,6 @@ private void configurePathPlannerAutoBuilder() { /** @return fused field pose (meters, radians) from odometry and vision */ public Pose2d getPose() { - if (useDesktopKinematicSimulation()) { - return desktopSimPose; - } return kPoseEstimator.getEstimatedPosition(); } @@ -289,27 +235,15 @@ public Pose2d getPose() { * @param pose new field pose */ public void resetPose(Pose2d pose) { - if (useDesktopKinematicSimulation()) { - desktopSimPose = pose; - simulationHeading = pose.getRotation(); - return; - } - if (useDesktopSimulation()) { - simulationHeading = pose.getRotation(); - } kPoseEstimator.resetPosition(getAngleForOdometry(), getSwerveModulePositions(), pose); } /** @return current chassis speeds from measured module states (robot-relative) */ public ChassisSpeeds getChassisSpeeds() { - if (useDesktopKinematicSimulation()) { - return lastRobotRelativeChassisSpeeds; - } return kSwerveKinematics.toChassisSpeeds(getSwerveModuleStates()); } private void setModuleStates(ChassisSpeeds chassisSpeeds) { - lastRobotRelativeChassisSpeeds = chassisSpeeds; SwerveModuleState[] desiredStates = getOptimizedModuleStatesFromChassisSpeeds(chassisSpeeds); kFrontLeft.setDesiredState(desiredStates[0]); kFrontRight.setDesiredState(desiredStates[1]); @@ -325,25 +259,9 @@ private SwerveModuleState[] getOptimizedModuleStatesFromChassisSpeeds(ChassisSpe } private Rotation2d getAngleForOdometry() { - if (useDesktopSimulation()) { - return simulationHeading; - } return headingSupplier.get(); } - /** {@code simulateJava} only — not JUnit HAL tests ({@link DriverStation#isTest()}). */ - private static boolean useDesktopSimulation() { - return RobotBase.isSimulation() && !DriverStation.isTest(); - } - - /** - * Teleop desktop sim integrates commanded chassis speeds; autonomous uses module physics odometry - * (PathPlanner needs measured {@link #getChassisSpeeds()} and encoder-based pose like on the real robot). - */ - private static boolean useDesktopKinematicSimulation() { - return useDesktopSimulation() && !DriverStation.isAutonomous(); - } - /** Releases module CAN devices; for simulation unit tests only. */ void closeModulesForTest() { kFrontLeft.close(); @@ -354,10 +272,16 @@ void closeModulesForTest() { /** Seeds pivot CANcoder sim at 0° on all modules (sim unit tests). */ void seedSimulationPivotEncodersZero() { - kFrontLeft.resetSimulationEncoderState(); - kFrontRight.resetSimulationEncoderState(); - kRearLeft.resetSimulationEncoderState(); - kRearRight.resetSimulationEncoderState(); + seedSimulationPivotEncoder(kFrontLeft); + seedSimulationPivotEncoder(kFrontRight); + seedSimulationPivotEncoder(kRearLeft); + seedSimulationPivotEncoder(kRearRight); + } + + 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); } @@ -429,7 +353,6 @@ vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond, getPose().getRotati } private void stopModulesButHoldPivotAngle() { - lastRobotRelativeChassisSpeeds = new ChassisSpeeds(); 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()))); @@ -458,7 +381,6 @@ public SwerveModulePosition[] getSwerveModulePositions() { /** Stops all modules and resets the heading-hold PID controller. */ public void stop() { - lastRobotRelativeChassisSpeeds = new ChassisSpeeds(); kFrontRight.stop(); kFrontLeft.stop(); kRearRight.stop(); @@ -468,7 +390,6 @@ public void stop() { /** Angles modules in an X pattern to resist being pushed on defense or on a slope. */ public void lock() { - lastRobotRelativeChassisSpeeds = new ChassisSpeeds(); kFrontRight.setDesiredState(0, -45); kFrontLeft.setDesiredState(0, 45); kRearRight.setDesiredState(0, 45); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 4837249..4cca1e6 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -24,7 +24,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.Units; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; @@ -48,9 +47,6 @@ * @see SwerveDrive */ public class SwerveModule extends SubsystemBase { - private static final DCMotor kNeoMotor = DCMotor.getNEO(1); - private static final double kSimLoopPeriodSec = 0.02; - /** Module label (e.g. {@code FrontLeft}) for logging and Shuffleboard. */ public final String name; /** Calibrated CANcoder offset added to absolute reading (degrees). */ @@ -182,18 +178,8 @@ public void simulationPeriodic() { return; } double busVoltage = RoboRioSim.getVInVoltage(); - // REV iterate() expects motor RPM from a physics model; scale NEO free speed by applied output. - double neoFreeSpeedRpm = kNeoMotor.freeSpeedRadPerSec * 60.0 / (2.0 * Math.PI); - double driveMotorRpm = - driveSim.getAppliedOutput() * neoFreeSpeedRpm / SwerveDriveConfig.SDS_L2_DRIVE_GEARING; - driveSim.iterate(driveMotorRpm, busVoltage, kSimLoopPeriodSec); - - double pivotMotorRpm = pivotSim.getAppliedOutput() * neoFreeSpeedRpm; - pivotSim.iterate(pivotMotorRpm, busVoltage, kSimLoopPeriodSec); - - // Pivot angle odometry reads the CANcoder; keep sim encoder in sync with pivot motor rotation. - pivotEncoderSimState().setRawPosition(Units.Rotations.of(pivotSim.getPosition())); - refreshPivotAbsolutePosition(); + 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. */ @@ -216,22 +202,6 @@ void refreshPivotAbsolutePosition() { BaseStatusSignal.refreshAll(pivotEncoder.getAbsolutePosition()); } - /** Zeros drive/pivot REV sim encoder state (sim tests and fixture reset between tests). */ - void resetSimulationEncoderState() { - if (driveSim == null) { - return; - } - driveSim.setPosition(0.0); - driveSim.setVelocity(0.0); - driveEncoder.setPosition(0.0); - pivotSim.setPosition(0.0); - pivotSim.setVelocity(0.0); - pivotEncoderSimState().setSupplyVoltage(12.0); - pivotEncoderSimState().setRawPosition(Units.Rotations.of(0.0)); - refreshPivotAbsolutePosition(); - stop(); - } - /** @return pivot {@link SparkMaxSim}, or null on the roboRIO */ SparkMaxSim pivotSparkSim() { return pivotSim; diff --git a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java index 2f73bb5..d917bb0 100644 --- a/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java +++ b/src/test/java/frc/robot/subsystems/drive/SwerveDriveTestFixture.java @@ -75,8 +75,8 @@ public void resetBetweenTests() { SwerveDrive.maxAngularSpeedRadiansPerSecond = Math.toRadians(540); heading.set(Rotation2d.kZero); swerve.stop(); - swerve.seedSimulationPivotEncodersZero(); swerve.resetPose(FieldConstants.kDefaultOdometryPose); + swerve.seedSimulationPivotEncodersZero(); SimHooks.stepTiming(0.02); }