From 84404dc9fd9c1bb9c63728d5d125d4013764280a Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Thu, 16 May 2024 06:27:05 +0300 Subject: [PATCH 1/5] make drive subsystem AutoClosable --- .../frc/robot/subsystems/DriveSubsystem.java | 22 ++++++++++++++++++- .../frc/robot/subsystems/MAXSwerveModule.java | 12 +++++++++- 2 files changed, 32 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 9d24c60..4b5deea 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -34,7 +34,7 @@ import frc.utils.SwerveUtils; import java.util.Optional; -public class DriveSubsystem extends SubsystemBase { +public class DriveSubsystem extends SubsystemBase implements AutoCloseable { private final AHRS navX = new AHRS(); private Field2d field = new Field2d(); @@ -155,6 +155,17 @@ public void simulationPeriodic() { // This method will be called once per scheduler run during simulation } + @Override + public void close() { + frontLeft.close(); + frontRight.close(); + rearLeft.close(); + rearRight.close(); + + field.close(); + publisher.close(); + } + private void updatePoseWithVision() { var poseOpt = PhotonCameraSystem.getEstimatedGlobalPose(field.getRobotPose()); SmartDashboard.putBoolean("AprilTag Seen", poseOpt.isPresent()); @@ -369,6 +380,15 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { rearRight.setDesiredState(desiredStates[3]); } + public SwerveModuleState[] getModuleDesiredStates() { + return new SwerveModuleState[] { + frontLeft.getDesiredState(), + frontRight.getDesiredState(), + rearLeft.getDesiredState(), + rearRight.getDesiredState() + }; + } + /** Resets the drive encoders to currently read a position of 0. */ public void resetEncoders() { frontLeft.resetEncoders(); diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index 80f29a7..87516c1 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -12,7 +12,7 @@ import frc.robot.Constants.ModuleConstants; import frc.utils.sim_utils.CANSparkMAXWrapped; -public class MAXSwerveModule { +public class MAXSwerveModule implements AutoCloseable { private final CANSparkMAXWrapped m_drivingSparkMax; private final CANSparkMAXWrapped m_turningSparkMax; @@ -106,6 +106,12 @@ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngular m_drivingEncoder.setPosition(0); } + @Override + public void close() { + m_drivingSparkMax.close(); + m_turningSparkMax.close(); + } + /** * Returns the current state of the module. * @@ -158,6 +164,10 @@ public void setDesiredState(SwerveModuleState desiredState) { m_desiredState = desiredState; } + public SwerveModuleState getDesiredState() { + return m_desiredState; + } + /** Zeroes all the SwerveModule encoders. */ public void resetEncoders() { m_drivingEncoder.setPosition(0); From 4fa9f41d11874e1499092490565269d4bb7a0533 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Thu, 16 May 2024 06:42:55 +0300 Subject: [PATCH 2/5] Create a test base for DriveSubsystem this should make things a lot easier ngl --- .../DriveSubsystemTestBase.java | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java diff --git a/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java new file mode 100644 index 0000000..553b4f7 --- /dev/null +++ b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java @@ -0,0 +1,30 @@ +package subsystem_tests.drive_subsystem_tests; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.DriveSubsystem; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; + +public class DriveSubsystemTestBase { + protected DriveSubsystem driveSubsystem; + protected CommandScheduler commandScheduler; + + @BeforeEach + public void setUp() { + assert HAL.initialize(500, 0); + commandScheduler = CommandScheduler.getInstance(); + driveSubsystem = new DriveSubsystem(); + + // Enable robot for commands to run + DriverStationSim.setEnabled(true); + DriverStationSim.setDsAttached(true); + DriverStationSim.notifyNewData(); // ! Breaks without this + } + + @AfterEach + public void tearDown() { + driveSubsystem.close(); + } +} From 00152bd9174a8f96f47caf9e9217e2fc0e34c93e Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Thu, 16 May 2024 06:43:49 +0300 Subject: [PATCH 3/5] Create NavXSim.java done using the documentation, so this is pretty much correct... this is much better than hand wrapping, might wanna do this with rev as well, as soon as possible... --- .../drive_subsystem_tests/utils/NavXSim.java | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 src/test/java/subsystem_tests/drive_subsystem_tests/utils/NavXSim.java diff --git a/src/test/java/subsystem_tests/drive_subsystem_tests/utils/NavXSim.java b/src/test/java/subsystem_tests/drive_subsystem_tests/utils/NavXSim.java new file mode 100644 index 0000000..f92f185 --- /dev/null +++ b/src/test/java/subsystem_tests/drive_subsystem_tests/utils/NavXSim.java @@ -0,0 +1,20 @@ +package subsystem_tests.drive_subsystem_tests.utils; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.simulation.SimDeviceDataJNI; + +public class NavXSim { + public static void setAngle(double yaw) { + int device = SimDeviceDataJNI.getSimDeviceHandle("navX-Sensor[0]"); + SimDouble angle = new SimDouble(SimDeviceDataJNI.getSimValueHandle(device, "Yaw")); + angle.set(yaw); + } + + public static void setConnected(boolean connected) { + int device = SimDeviceDataJNI.getSimDeviceHandle("navX-Sensor[0]"); + SimBoolean isConnected = + new SimBoolean(SimDeviceDataJNI.getSimValueHandle(device, "Connected")); + isConnected.set(connected); + } +} From 2c42cb84b08bb1092c29bf49ede340fd27df2850 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Thu, 16 May 2024 06:44:07 +0300 Subject: [PATCH 4/5] Create DriveTestUtils.java --- .../utils/DriveTestUtils.java | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 src/test/java/subsystem_tests/drive_subsystem_tests/utils/DriveTestUtils.java diff --git a/src/test/java/subsystem_tests/drive_subsystem_tests/utils/DriveTestUtils.java b/src/test/java/subsystem_tests/drive_subsystem_tests/utils/DriveTestUtils.java new file mode 100644 index 0000000..92fadb3 --- /dev/null +++ b/src/test/java/subsystem_tests/drive_subsystem_tests/utils/DriveTestUtils.java @@ -0,0 +1,35 @@ +package subsystem_tests.drive_subsystem_tests.utils; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.DriveConstants.SwerveModuleConstants; +import frc.robot.subsystems.DriveSubsystem; + +public class DriveTestUtils { + public static ChassisSpeeds getDesiredChassisSpeeds(DriveSubsystem driveSubsystem) { + return SwerveModuleConstants.kDriveKinematics.toChassisSpeeds( + driveSubsystem.getModuleDesiredStates()); + } + + public static ChassisSpeeds driveToChassisSpeeds( + double x, double y, double rot, Rotation2d gyroAngle) { + return ChassisSpeeds.fromFieldRelativeSpeeds( + x * DriveConstants.kMaxSpeedMetersPerSecond, + y * DriveConstants.kMaxSpeedMetersPerSecond, + rot * DriveConstants.kMaxAngularSpeed, + gyroAngle); + } + + public static ChassisSpeeds driveToChassisSpeeds(double x, double y, double rot) { + return driveToChassisSpeeds(x, y, rot, new Rotation2d()); + } + + public static void checkIfEqual(ChassisSpeeds shouldBe, ChassisSpeeds currentlyIs) { + assertEquals(shouldBe.vxMetersPerSecond, currentlyIs.vxMetersPerSecond, 0.01); + assertEquals(shouldBe.vyMetersPerSecond, currentlyIs.vyMetersPerSecond, 0.01); + assertEquals(shouldBe.omegaRadiansPerSecond, currentlyIs.omegaRadiansPerSecond, 0.01); + } +} From 3a2dd439bd906147f1a0b1e0fb21afe5b221dc2b Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Mon, 27 May 2024 15:23:47 +0300 Subject: [PATCH 5/5] improve teardown of Drive Subsystem Test Base --- .../drive_subsystem_tests/DriveSubsystemTestBase.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java index 553b4f7..8daf74d 100644 --- a/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java +++ b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveSubsystemTestBase.java @@ -26,5 +26,9 @@ public void setUp() { @AfterEach public void tearDown() { driveSubsystem.close(); + commandScheduler.cancelAll(); + commandScheduler.unregisterAllSubsystems(); // ! breaks all test tests if not done + commandScheduler.close(); + HAL.shutdown(); } }