diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 17816575..d0d8bbe2 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -4,8 +4,8 @@ import java.util.List; -import com.ctre.phoenix.sensors.BasePigeonSimCollection; -import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.ctre.phoenix6.sim.Pigeon2SimState; +import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -64,8 +64,8 @@ public class SwerveSubsystem extends SubsystemBase implements AutoCloseable { private final List modules = List.of(m_frontLeft, m_frontRight, m_backLeft, m_backRight); - private WPI_Pigeon2 m_gyro = new WPI_Pigeon2(0); - private final BasePigeonSimCollection gyroSim = m_gyro.getSimCollection(); + private Pigeon2 m_gyro = new Pigeon2(0); + private final Pigeon2SimState gyroSim = new Pigeon2SimState(m_gyro); SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( DriveConstants.kDriveKinematics, @@ -116,7 +116,7 @@ public SwerveSubsystem() { } // ! For testing purposes only - public SwerveSubsystem(WPI_Pigeon2 gyro) { + public SwerveSubsystem(Pigeon2 gyro) { m_gyro = gyro; new Thread(() -> { @@ -246,7 +246,7 @@ public void simulate(){ SmartDashboard.putData("Module 2", m_frontRight); SmartDashboard.putData("Module 3", m_backLeft); SmartDashboard.putData("Module 4", m_backRight); - gyroSim.addHeading(Units.radiansToDegrees(DriveConstants.kDriveKinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond) + gyroSim.addYaw(Units.radiansToDegrees(DriveConstants.kDriveKinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond) * (DriveConstants.kGyroReversed ? -1.0 : 1.0) * 0.02); SmartDashboard.putNumber("Heading", getHeading()); } @@ -277,6 +277,6 @@ public void close() throws Exception { m_frontRight.close(); m_backLeft.close(); m_backRight.close(); - m_gyro.DestroyObject(); + m_gyro.close(); } } diff --git a/src/test/java/SwerveSubsystemTest.java b/src/test/java/SwerveSubsystemTest.java deleted file mode 100644 index d3d24932..00000000 --- a/src/test/java/SwerveSubsystemTest.java +++ /dev/null @@ -1,49 +0,0 @@ -import static org.junit.jupiter.api.Assertions.*; -import static org.mockito.Mockito.*; - -import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import org.mockito.Mock; -import org.mockito.MockitoAnnotations; - -import com.ctre.phoenix.sensors.WPI_Pigeon2; - -import frc.robot.subsystems.SwerveSubsystem; - -public class SwerveSubsystemTest { - SwerveSubsystem subsystem; - - @Mock WPI_Pigeon2 gyroMock; - - @BeforeEach - void setup() { - // Arrange - MockitoAnnotations.openMocks(this); - - // Put stubs here - - subsystem = new SwerveSubsystem(gyroMock); - } - - @Test - void testGetHeadingLarge() { - // Arrange - when(gyroMock.getYaw()).thenReturn(1000.0); - // Act and Assert - assertTrue(subsystem.getHeading() >= -360 && subsystem.getHeading() <= 360); - } - - @Test - void testGetHeadingSmall() { - // Arrange - when(gyroMock.getYaw()).thenReturn(1.0); - // Act and Assert - assertTrue(subsystem.getHeading() >= -360 && subsystem.getHeading() <= 360); - } - - @AfterEach - void shutdown() throws Exception { - subsystem.close(); - } -} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 4bd44248..e2eb60a9 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,56 +1,25 @@ { - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.30.2", + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "23.2.2", "frcYear": 2023, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2023-latest.json", "javaDependencies": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.30.2" - }, - { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "5.30.2" + "version": "23.2.2" } ], "jniDependencies": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.2", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -61,9 +30,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,9 +43,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -87,9 +56,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -100,9 +69,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -113,9 +82,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -126,9 +95,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -139,9 +108,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,9 +121,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -165,9 +134,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.1", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,10 +149,10 @@ ], "cppDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "5.30.2", - "libName": "CTRE_Phoenix_WPI", + "version": "23.2.2", + "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -195,10 +164,10 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.30.2", - "libName": "CTRE_Phoenix", + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "23.2.2", + "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -210,40 +179,40 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.2", - "libName": "CTRE_PhoenixCCI", + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "23.2.2", + "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", - "linuxathena" + "osxuniversal" ], - "simMode": "hwsim" + "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro", - "artifactId": "tools", - "version": "23.0.1", - "libName": "CTRE_PhoenixTools", + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "23.2.2", + "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", - "linuxathena" + "osxuniversal" ], - "simMode": "hwsim" + "simMode": "swsim" }, { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.30.2", - "libName": "CTRE_Phoenix_WPISim", + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "23.2.2", + "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -255,10 +224,10 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.30.2", - "libName": "CTRE_PhoenixSim", + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "23.2.2", + "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -270,10 +239,85 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.2", - "libName": "CTRE_PhoenixCCISim", + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "23.2.2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "23.2.2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "23.2.2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "23.2.2", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "23.2.2", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "23.2.2", + "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true,