added pose estimator

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -5,6 +5,7 @@
 package frc4388.robot.subsystems;
 
 
+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;
@@ -37,7 +38,9 @@ public class SwerveDrive extends SubsystemBase {
 
   private RobotGyro gyro;
 
-  private SwerveDriveOdometry odometry;
+  // private SwerveDriveOdometry odometry;
+
+  private SwerveDrivePoseEstimator poseEstimator;
 
   public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
   public Rotation2d rotTarget = new Rotation2d();
@@ -51,15 +54,28 @@ public class SwerveDrive extends SubsystemBase {
     this.rightBack = rightBack;
     this.gyro = gyro;
 
-    this.odometry = new SwerveDriveOdometry(
+    // this.odometry = new SwerveDriveOdometry(
+    //   kinematics, 
+    //   gyro.getRotation2d(),
+    //   new SwerveModulePosition[] {
+    //     leftFront.getPosition(),
+    //     rightFront.getPosition(),
+    //     leftBack.getPosition(),
+    //     rightBack.getPosition()
+    //   },
+    //   getOdometry()
+    // );
+
+    this.poseEstimator = new SwerveDrivePoseEstimator(
       kinematics, 
-      gyro.getRotation2d(),
+      gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
         rightFront.getPosition(),
         leftBack.getPosition(),
         rightBack.getPosition()
-      }
+      },
+      new Pose2d(0,0, new Rotation2d(0))
     );
 
     this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
@@ -113,15 +129,27 @@ public class SwerveDrive extends SubsystemBase {
 
   public void resetGyro() {
     gyro.reset();
-    setOdometry(getOdometry());
+    // setOdometry(getOdometry());
     rotTarget = new Rotation2d(0);
   }
 
   /**
    * Updates the odometry of the SwerveDrive.
    */
-  public void updateOdometry() {
-    odometry.update(
+  // public void updateOdometry() {
+  //   odometry.update(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     }
+  //   );
+  // }
+
+  public void updatePoseEstimator() {
+    poseEstimator.update(
       gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
@@ -136,16 +164,33 @@ public class SwerveDrive extends SubsystemBase {
    * Gets the odometry of the SwerveDrive.
    * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
    */
-  public Pose2d getOdometry() {
-    return odometry.getPoseMeters();
+  // public Pose2d getOdometry() {
+  //   return odometry.getPoseMeters();
+  // }
+
+  public Pose2d getPoseEstimator() {
+    return poseEstimator.getEstimatedPosition();
   }
 
   /**
    * Sets the odometry of the SwerveDrive.
    * @param pose Pose to set the odometry to.
    */
-  public void setOdometry(Pose2d pose) {
-    odometry.resetPosition(
+  // public void setOdometry(Pose2d pose) {
+  //   odometry.resetPosition(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     },
+  //     pose
+  //   );
+  // }
+
+  public void setPoseEstimator(Pose2d pose) {
+    poseEstimator.resetPosition(
       gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
@@ -157,13 +202,17 @@ public class SwerveDrive extends SubsystemBase {
     );
   }
 
+  public void resetPoseEstimator() {
+    setPoseEstimator(new Pose2d());
+  }
+
   /**
    * Resets the odometry of the SwerveDrive to 0.
    * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.
    */
-  public void resetOdometry() {
-    setOdometry(new Pose2d());
-  }
+  // public void resetOdometry() {
+  //   setOdometry(new Pose2d());
+  // }
 
   public SwerveDriveKinematics getKinematics() {
     return this.kinematics;
@@ -172,14 +221,14 @@ public class SwerveDrive extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    updateOdometry();
+    // updateOdometry();
+    updatePoseEstimator();
 
-    // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()});
-    SmartDashboard.putNumber("Odo X", getOdometry().getX());
-    SmartDashboard.putNumber("Odo Y", getOdometry().getY());
-    SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees());
+    // SmartDashboard.putNumber("Odo X", getOdometry().getX());
+    // SmartDashboard.putNumber("Odo Y", getOdometry().getY());
+    // SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees());
     
-    SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
+    // SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
   }
 
   /**