replace SwerveDriveWheelPositions

Iron-Panthers · Jan 3, 2025 · 88e1905 · 88e1905
1 parent 84901f4
commit 88e1905
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 16 deletions.
diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -8,15 +8,14 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
-import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import frc.robot.subsystems.swerve.DriveConstants;
 import org.littletonrobotics.junction.AutoLogOutput;
 
 /* based on wpimath/../PoseEstimator.java */
 public class RobotState {
   public record OdometryMeasurement(
-      SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {}
+      SwerveModulePosition[] wheelPositions, Rotation2d gyroAngle, double timestamp) {}
 
   public record VisionMeasurement(Pose2d visionPose, double timestamp) {}
 
@@ -28,14 +27,13 @@ public record VisionMeasurement(Pose2d visionPose, double timestamp) {}
   private Pose2d odometryPose = new Pose2d(); // motion sensors
   private Pose2d estimatedPose = new Pose2d(); // odometry + vision
 
-  private SwerveDriveWheelPositions lastWheelPositions =
-      new SwerveDriveWheelPositions(
-          new SwerveModulePosition[] {
-            new SwerveModulePosition(),
-            new SwerveModulePosition(),
-            new SwerveModulePosition(),
-            new SwerveModulePosition()
-          });
+  private SwerveModulePosition[] lastWheelPositions =
+      new SwerveModulePosition[] {
+        new SwerveModulePosition(),
+        new SwerveModulePosition(),
+        new SwerveModulePosition(),
+        new SwerveModulePosition()
+      };
   private Rotation2d lastGyroAngle = new Rotation2d();
 
   private static RobotState instance;

diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java
@@ -6,7 +6,6 @@
 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.SwerveDriveWheelPositions;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -68,11 +67,10 @@ public void periodic() {
     }
 
     // pass odometry data to robotstate
-    SwerveDriveWheelPositions wheelPositions =
-        new SwerveDriveWheelPositions(
-            Arrays.stream(modules)
-                .map(module -> module.getModulePosition())
-                .toArray(SwerveModulePosition[]::new));
+    SwerveModulePosition[] wheelPositions =
+        Arrays.stream(modules)
+            .map(module -> module.getModulePosition())
+            .toArray(SwerveModulePosition[]::new);
     RobotState.getInstance()
         .addOdometryMeasurement(
             new RobotState.OdometryMeasurement(