From abaea2a8e04a724c14f01101799f76f9a56932cb Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 31 Dec 2024 15:56:57 -0500 Subject: [PATCH] Refactor SwerveDrive to use DriveConstants for kinematics and remove unused test files --- .../robot/subsystems/swerve/SwerveDrive.java | 16 +--- src/test/java/SwerveDriveTest.java | 83 ------------------- src/test/java/SwerveModuleTest.java | 39 --------- 3 files changed, 3 insertions(+), 135 deletions(-) delete mode 100644 src/test/java/SwerveDriveTest.java delete mode 100644 src/test/java/SwerveModuleTest.java diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index b314b0a..a3c1a76 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -42,7 +42,6 @@ public class SwerveDrive extends SubsystemBase { private final Alert gyroDisconnectedAlert = new Alert("Gyro Hardware Fault", Alert.AlertType.kError); - private SwerveDriveKinematics kinematics; public SwerveDrive( GyroInterface gyroIO, @@ -54,7 +53,6 @@ public SwerveDrive( this.gyroInputs = new GyroInputsAutoLogged(); this.rawGyroRotation = new Rotation2d(); - setKinematics(DriveConstants.DRIVE_KINEMATICS); swerveModules = new SwerveModule[] { new SwerveModule(frontLeftModuleIO, "FrontLeft"), @@ -72,7 +70,7 @@ public SwerveDrive( }; poseEstimator = new SwerveDrivePoseEstimator( - getKinematics(), + DriveConstants.DRIVE_KINEMATICS, rawGyroRotation, lastModulePositions, new Pose2d(), @@ -90,14 +88,6 @@ public SwerveDrive( gyroDisconnectedAlert.set(false); } - public SwerveDriveKinematics getKinematics() { - return kinematics; - } - - public void setKinematics(SwerveDriveKinematics newKinematics) { - kinematics = newKinematics; - } - /** * Gets the current velocity of the gyro's yaw * @@ -198,7 +188,7 @@ private void modulesPeriodic() { */ public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) { SwerveModuleState[] swerveModuleStates = - getKinematics() + DriveConstants.DRIVE_KINEMATICS .toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( @@ -243,7 +233,7 @@ private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { if (gyroInputs.isConnected) { rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; } else { - Twist2d twist = getKinematics().toTwist2d(moduleDeltas); + Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } diff --git a/src/test/java/SwerveDriveTest.java b/src/test/java/SwerveDriveTest.java deleted file mode 100644 index 718fa7f..0000000 --- a/src/test/java/SwerveDriveTest.java +++ /dev/null @@ -1,83 +0,0 @@ -import static org.junit.jupiter.api.Assertions.*; -import static org.mockito.ArgumentMatchers.any; -import static org.mockito.ArgumentMatchers.eq; -import static org.mockito.Mockito.*; - -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.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.subsystems.swerve.*; -import frc.robot.subsystems.swerve.gyroIO.GyroInterface; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; -import frc.robot.subsystems.swerve.odometryThread.OdometryThread; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import org.mockito.Mock; -import org.mockito.MockitoAnnotations; - -class SwerveDriveTest { - - @Mock private GyroInterface gyroIO; - - @Mock - private ModuleInterface frontLeftModuleIO, - frontRightModuleIO, - backLeftModuleIO, - backRightModuleIO; - @Mock private SwerveModule frontLeftModule, frontRightModule, backLeftModule, backRightModule; - - @Mock private OdometryThread mockOdometryThread; - - @Mock private SwerveDriveKinematics swerveDriveKinematics; - - private SwerveDrive swerveDrive; - - @BeforeEach - void setUp() { - // Initialize the mocks - MockitoAnnotations.openMocks(this); - - swerveDrive = - spy( - new SwerveDrive( - gyroIO, - frontLeftModuleIO, - frontRightModuleIO, - backLeftModuleIO, - backRightModuleIO)); - - swerveDrive.setKinematics(swerveDriveKinematics); - } - - @Test - void testDrive() { - // Prepare mock module states - SwerveModuleState[] mockStates = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - mockStates[i] = mock(SwerveModuleState.class); // Mock each SwerveModuleState - } - - // Mock the toSwerveModuleStates method to return mock states - when(swerveDriveKinematics.toSwerveModuleStates(any(ChassisSpeeds.class))) - .thenReturn(mockStates); - - // Now when the drive method calls toSwerveModuleStates, it will return mockStates - swerveDrive.drive(1.0, 1.0, 0.5, true); - - // Verify that setModuleStates was called with mockStates - verify(swerveDrive).setModuleStates(mockStates); - verify(swerveDrive.getKinematics()).toSwerveModuleStates(any(ChassisSpeeds.class)); - } - - @Test - void testSetPose() { - // Mock pose setter - Pose2d newPose = new Pose2d(3.0, 4.0, Rotation2d.fromDegrees(90)); - swerveDrive.setPose(newPose); - - // Verify that the setPose method was called with the new pose - verify(swerveDrive).setPose(eq(newPose)); - } -} diff --git a/src/test/java/SwerveModuleTest.java b/src/test/java/SwerveModuleTest.java deleted file mode 100644 index 08535c9..0000000 --- a/src/test/java/SwerveModuleTest.java +++ /dev/null @@ -1,39 +0,0 @@ -import static edu.wpi.first.units.Units.*; -import static org.junit.jupiter.api.Assertions.*; -import static org.mockito.Mockito.*; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.subsystems.swerve.*; -import frc.robot.subsystems.swerve.moduleIO.ModuleInputsAutoLogged; -import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import org.mockito.Mock; -import org.mockito.MockitoAnnotations; - -class SwerveModuleTest { - - private SwerveModule swerveModule; - @Mock private ModuleInterface mockModuleInterface; - @Mock private ModuleInputsAutoLogged mockInputs; - - @BeforeEach - void setUp() { - MockitoAnnotations.openMocks(this); - - // Initialize SwerveModule with the mocked interface - swerveModule = spy(new SwerveModule(mockModuleInterface, "Mocked Module")); - mockInputs = spy(mockInputs); - } - - @Test - void testRunSetpoint() { - // Prepare a mock state for the module - SwerveModuleState mockState = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45)); - - // Call runSetPoint and verify that the setDesiredState method is called on the module interface - swerveModule.runSetpoint(mockState); - verify(mockModuleInterface).setDesiredState(mockState); - } -}